i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

New rtabmap-reduceGraph CLI tool (#1655)

Browse Source 
* New rtabmap-reduceGraph CLI tool

* fixed some edge cases

* Regenerating optimized map if there was one before reducing the graph

* addMoreLoopClosures: refactored how ctrl-c is handled to stop faster when no loop closures are added

* Added kilted status

* Make offline tool always propagate neighbor merged links

* removed a parameter

* fixed disconnected graph

* fixed --help

* Added error log on Kp/NNStrategy not compatible with huge vocabulary. ReduceGraph/DetectMoreLoopClosures: Make sure original parameters are saved back on closing. g2o: fixing optimizer to Levenberg for SBA to avoid [SetJac] infinite jac fatal error.

* exposing neighbor merged ratio parameter to the tool

* show param in log

* refactored detectMoreLoopClosures to ignore too close nodes in terms of neighbor links based on Mem/STMSize parameter. Reduce graph: added direction parameter.

* Simplified: removed ratio parameter, removed recursive reduction. Just don't reduce if a NM link is longer than maxDistance.

* Removed NNStrategy override, as it was still done on closing when we changed back to original params

* DBViewer: show missing links when showing OptimizedPoses in GraphView, fixed clicking on landmark links

* DetectMoreLoopClosures: Added support for min graph distance option in MainWindow and DbViewer

* slight renaming of ROS jobs

* reprocess: added option --params_last
This commit is contained in:
matlabbe
2026-04-04 19:48:03 -07:00
committed by GitHub
parent 51cfc37923
commit 1ea8fa2e06
24 changed files with 865 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/OptimizerG2O.cpp
+15 -5
View File
@@ -1560,7 +1560,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#endif // RTABMAP_ORB_SLAM
#ifndef RTABMAP_ORB_SLAM
if(optimizer_ == 1)
// ISSUE: It seems the fatal error
// "[SetJac] infinite jac" happens relatively
// easily with GaussNewton on SBA problem,
// ignore optimizer_ and always use Levenberg for SBA.
// TODO: Note that g2o/RobustKernelDelta parameter could be
// potentially tuned to avoid that error with GaussNewton.
if(0)//optimizer_ == 1)
{
#ifdef RTABMAP_G2O_CPP11
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
@@ -2018,7 +2024,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(uIsNan(chi2))
{
UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current %s=%d) or solver (current %s=%d).",
Parameters::kg2oOptimizer().c_str(), optimizer_, Parameters::kg2oSolver().c_str(), solver_);
return optimizedPoses;
}
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
@@ -2052,15 +2059,18 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
#endif
cv::Point3f pt3d;
int id=-1;
if((*iter)->vertex(0)->id() > negVertexOffset)
{
pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
id = negVertexOffset - (*iter)->vertex(0)->id();
}
else
{
pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
id = (*iter)->vertex(0)->id() - stepVertexId;
}
UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)",
id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str());
cv::Point3f pt3d = points3DMap.at(id);
((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
if(outliers)