i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27: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/Graph.cpp
+12 -23
View File
@@ -2020,19 +2020,21 @@ std::list<std::pair<int, Transform> > computePath(
bool lookInDatabase,
bool updateNewCosts,
float linearVelocity, // m/sec
float angularVelocity) // rad/sec
float angularVelocity, // rad/sec
bool ignoreDirectLinks)
{
UASSERT(memory!=0);
UASSERT(fromId>=0);
UASSERT(toId!=0);
std::list<std::pair<int, Transform> > path;
UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f ignoreDirectLinks=%d",
fromId,
toId,
lookInDatabase?1:0,
updateNewCosts?1:0,
linearVelocity,
angularVelocity);
angularVelocity,
ignoreDirectLinks?1:0);
std::multimap<int, Link> allLinks;
if(lookInDatabase)
@@ -2110,7 +2112,9 @@ std::list<std::pair<int, Transform> > computePath(
}
for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
if(iter->second.from() != iter->second.to() &&
(!ignoreDirectLinks ||
(!(iter->second.from()==fromId && iter->second.to()==toId) && !(iter->second.to()==fromId && iter->second.from()==toId))))
{
Transform nextPose = currentNode->pose()*iter->second.transform();
float cost = 0.0f;
@@ -2396,26 +2400,15 @@ std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const st
float computePathLength(
const std::vector<std::pair<int, Transform> > & path,
unsigned int fromIndex,
unsigned int toIndex)
const std::vector<std::pair<int, Transform> > & path)
{
float length = 0.0f;
if(path.size() > 1)
{
UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex);
if(fromIndex >= toIndex)
for(unsigned int i=0; i<path.size()-1; ++i)
{
toIndex = (unsigned int)path.size()-1;
length+=path[i].second.getDistance(path[i+1].second);
}
float x=0, y=0, z=0;
for(unsigned int i=fromIndex; i<toIndex-1; ++i)
{
x += fabs(path[i].second.x() - path[i+1].second.x());
y += fabs(path[i].second.y() - path[i+1].second.y());
z += fabs(path[i].second.z() - path[i+1].second.z());
}
length = sqrt(x*x + y*y + z*z);
}
return length;
}
@@ -2426,19 +2419,15 @@ float computePathLength(
float length = 0.0f;
if(path.size() > 1)
{
float x=0, y=0, z=0;
std::map<int, Transform>::const_iterator iter=path.begin();
Transform previousPose = iter->second;
++iter;
for(; iter!=path.end(); ++iter)
{
const Transform & currentPose = iter->second;
x += fabs(previousPose.x() - currentPose.x());
y += fabs(previousPose.y() - currentPose.y());
z += fabs(previousPose.z() - currentPose.z());
length+=previousPose.getDistance(currentPose);
previousPose = currentPose;
}
length = sqrt(x*x + y*y + z*z);
}
return length;
}