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

rtabmap-reprocess: support Rtabmap/DetectionRate when intermediate nodes are created and -skip option is not used

Browse Source
This commit is contained in:
matlabbe
2026-05-10 14:30:57 -07:00
parent a8a9159e77
commit 2e2f3f079c
1 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/Reprocess/main.cpp
+27 -10
View File
@@ -953,6 +953,11 @@ int main(int argc, char * argv[])
Odometry * odometry = 0; Odometry * odometry = 0;
float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate(); float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
if(rtabmapUpdateRate!=0)
{
rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
}
double lastUpdateStamp = 0; double lastUpdateStamp = 0;
if(recomputeOdometry) if(recomputeOdometry)
{ {
@@ -965,14 +970,17 @@ int main(int argc, char * argv[])
{ {
printf("Odometry will be recomputed (\"odom\" option is set)%s.\n", printf("Odometry will be recomputed (\"odom\" option is set)%s.\n",
useInputOdometryAsGuess?" with input odometry guess (\"odom_guess_input\" option is set)":""); useInputOdometryAsGuess?" with input odometry guess (\"odom_guess_input\" option is set)":"");
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
if(rtabmapUpdateRate!=0)
{
rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
}
odometry = Odometry::create(parameters); odometry = Odometry::create(parameters);
} }
} }
else if(!intermediateNodes && framesToSkip == 0 &&
(configParameters.find(Parameters::kRtabmapDetectionRate())!=configParameters.end() ||
customParameters.find(Parameters::kRtabmapDetectionRate())!=customParameters.end()))
{
printf("[Warning] Parameter %s is ignored because parameter %s=false.\n",
Parameters::kRtabmapDetectionRate().c_str(),
Parameters::kRtabmapCreateIntermediateNodes().c_str());
}
printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str()); printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
std::map<std::string, float> globalMapStats; std::map<std::string, float> globalMapStats;
@@ -1079,11 +1087,16 @@ int main(int argc, char * argv[])
info.odomPose = pose; info.odomPose = pose;
info.odomCovariance = odomCovariance; info.odomCovariance = odomCovariance;
odomCovariance = cv::Mat(); odomCovariance = cv::Mat();
if(data.id() != -1)
lastUpdateStamp = data.stamp();
uInsert(globalMapStats, odomInfo.statistics(pose)); uInsert(globalMapStats, odomInfo.statistics(pose));
} }
else if(framesToSkip==0 && intermediateNodes && lastUpdateStamp > 0.0 && (data.stamp() < lastUpdateStamp + rtabmapUpdateRate))
{
data.setId(-1); // intermediate node
}
if(data.id() != -1)
lastUpdateStamp = data.stamp();
UTimer iterationTime; UTimer iterationTime;
std::string status; std::string status;
@@ -1274,15 +1287,19 @@ int main(int argc, char * argv[])
{ {
++loopIntra; ++loopIntra;
} }
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId); printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
} }
else if(landmarkId != 0) else if(landmarkId != 0)
{ {
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), landmarkId); printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), landmarkId);
}
else if(data.id() == -1)
{
printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Intermediate node\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
} }
else else
{ {
printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000)); printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
} }
// Here we accumulate statistics about distance from last localization // Here we accumulate statistics about distance from last localization