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

add warning and cleanup only when option is used

Browse Source
This commit is contained in:
matlabbe
2026-07-10 16:42:43 -07:00
parent 65c1ddc0cf
commit 8d84b1642d
1 changed files with 106 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/ReduceGraph/main.cpp
+106 -14
View File
@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/global_map/OccupancyGrid.h>
@@ -57,8 +58,9 @@ void showUsage(const char * exec)
"Options:\n"
" --keep_latest Merge old nodes to newer nodes, thus keeping only latest nodes.\n"
" --keep_linked Keep reduced nodes linked to graph.\n"
" --pre_cleanup Remove all user loop closures linking nodes closer than %s in the graph before reducing the graph.\n"
" --pre_cleanup Remove all user loop closures linking nodes closer than %s nodes in the graph before reducing the graph.\n"
" --radius #.# Maximum loop closure distance that can be merged. Default is 1 m. Should be > 0.\n"
" --sync_wm_and_opt_graph Keep in sync the WM state and the optimized graph, transfering in LTM disconnected nodes from the optimized graph during reduction process.\n"
" --udebug/--uinfo/--warn can also be used to change verbosity.\n"
"\n", exec, Parameters::kMemSTMSize().c_str());
exit(1);
@@ -78,6 +80,7 @@ int main(int argc, char * argv[])
bool keepLinked = false;
float radius = 1.0f;
bool preCleanup = false;
bool syncWmAndOptGraph = false;
for(int i=1; i<argc; ++i)
{
if(std::strcmp(argv[i], "--help") == 0)
@@ -96,6 +99,10 @@ int main(int argc, char * argv[])
{
preCleanup = true;
}
else if(std::strcmp(argv[i], "--sync_wm_and_opt_graph") == 0)
{
syncWmAndOptGraph = true;
}
else if(std::strcmp(argv[i], "--radius") == 0)
{
++i;
@@ -118,6 +125,7 @@ int main(int argc, char * argv[])
printf(" keep_latest = %s\n", keepLatest?"true":"false");
printf(" keep_linked = %s\n", keepLinked?"true":"false");
printf(" pre_cleanup = %s\n", preCleanup?"true":"false");
printf(" sync_wm_and_opt_graph = %s\n", syncWmAndOptGraph?"true":"false");
#ifdef RTABMAP_PYTHON
rtabmap::PythonInterface pythonInterface;
@@ -178,6 +186,37 @@ int main(int argc, char * argv[])
float xMin, yMin, cellSize;
bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty();
bool isWholeWMContainedInOptGraph = !optimizedPoses.empty();
bool isWholeGraphConnected = false;
std::shared_ptr<Optimizer> optimizer(Optimizer::create(parameters));
if(!optimizedPoses.empty())
{
std::map<int, Transform> tmp;
std::multimap<int, Link> optimizedConstraints;
memory.getMetricConstraints(uKeysSet(optimizedPoses), tmp, optimizedConstraints, false, true);
for(std::map<int, double>::const_iterator iter=memory.getWorkingMem().lower_bound(0);
iter!=memory.getWorkingMem().end() && isWholeWMContainedInOptGraph;
++iter)
{
if(optimizedPoses.find(iter->first) == optimizedPoses.end())
{
isWholeWMContainedInOptGraph = false;
}
}
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
optimizer->getConnectedGraph(
keepLatest?optimizedPoses.rbegin()->first:optimizedPoses.begin()->first,
optimizedPoses,
optimizedConstraints,
posesOut,
linksOut);
isWholeGraphConnected = posesOut.size() == optimizedPoses.size();
printf("Whole WM is contained in optimized graph: %s (WM=%ld Graph=%ld)\n", isWholeWMContainedInOptGraph?"true":"false", memory.getWorkingMem().size(), optimizedPoses.size());
printf("Whole optimized graph is connected: %s (org=%ld recomputed=%ld)\n", isWholeGraphConnected?"true":"false", optimizedPoses.size(), posesOut.size());
}
int totalNodesReduced = 0;
std::vector<int> vids;
vids.reserve(ids.size());
@@ -235,19 +274,6 @@ int main(int argc, char * argv[])
}
printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size());
// Remove orphan nodes
std::map<int, double> wm = memory.getWorkingMem();
for(std::map<int, double>::iterator iter=wm.lower_bound(0); iter!=wm.end(); ++iter)
{
std::multimap<int, rtabmap::Link> links = memory.getLinks(iter->first);
if(graph::filterLinks(links, Link::kSelfRefLink).empty())
{
// orphan
memory.deleteLocation(iter->first, 0, keepLinked);
printf("Removed orphan node %d\n", iter->first);
}
}
if(!optimizedPoses.empty())
{
size_t removed = 0;
@@ -265,6 +291,72 @@ int main(int argc, char * argv[])
}
}
printf("Updated optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size());
// The optimized graph should be all connected, check if some nodes got disconnected
std::map<int, Transform> tmp;
std::multimap<int, Link> optimizedConstraints;
memory.getMetricConstraints(uKeysSet(optimizedPoses), tmp, optimizedConstraints, false, true);
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
optimizer->getConnectedGraph(
keepLatest?optimizedPoses.rbegin()->first:optimizedPoses.lower_bound(0)->first,
optimizedPoses,
optimizedConstraints,
posesOut,
linksOut);
// Let's warn the user if WM was previously containing only
// nodes from optimized graph but now it doesn't
if( posesOut.size() != optimizedPoses.size() &&
isWholeWMContainedInOptGraph &&
isWholeGraphConnected)
{
int count = 0;
std::stringstream stream;
for(std::map<int, Transform>::iterator iter = optimizedPoses.begin();
iter!=optimizedPoses.end();)
{
if(posesOut.find(iter->first)==posesOut.end())
{
stream << iter->first << " ";
++count;
iter = optimizedPoses.erase(iter);
}
else
{
++iter;
}
}
UWARN("The optimized graph in working memory got split during graph "
"reduction process while it was all connected before! You may re-run "
"with --sync_wm_and_opt_graph option if needed. Here are the %d "
"nodes still in working memory but not connected to the root ID %d "
"of the optimized graph anymore: %s",
count,
keepLatest?optimizedPoses.rbegin()->first:optimizedPoses.lower_bound(0)->first,
stream.str().c_str());
}
if(syncWmAndOptGraph)
{
printf("Option --sync_wm_and_opt_graph is used, let's cleanup working memory "
"from nodes not in the optimized graph anymore.\n");
// Synchronize WM and the optimized graph. For all nodes in WM that
// are not in optimized graph, transfer to LTM.
std::map<int, double> wm = memory.getWorkingMem();
for(std::map<int, double>::iterator iter=wm.lower_bound(0);
iter!=wm.end();
++iter)
{
if(optimizedPoses.find(iter->first) == optimizedPoses.end())
{
memory.deleteLocation(iter->first, 0, true);
printf("Transferred %d from WM to LTM.\n", iter->first);
}
}
}
printf("Saving back %ld optimized poses to database.\n", optimizedPoses.size());
memory.saveOptimizedPoses(optimizedPoses, lastLocalizationPose);
}
if(hasOptimizedMap)