i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.15.2: Fixed Transform's not orthogonal rotation matrix causing problems with GTSAM (normalize rotation each time transforms are multiplied, and for backward compatibility automatically normalize rotation of all transforms loaded from old databases prior to 0.15.2). Added Vis/CorGuessMatchToProjection, Icp/PMMatcherKnn and Icp/PMMatcherEpsilon parameters. CameraRGB: fixed max scans number when filtering. Link: transfering maximum covariance when merging. OptimizerGTSAM: updated how covariance are copied based on GTSAM official example. util3d::loadBINCloud(): fixed dim parameter not used. DatabaseViewer: Added option in graph view to ignore intermediate nodes when optimizing the graph.

Browse Source
This commit is contained in:
matlabbe
2017-12-09 21:44:53 -05:00
parent dafaac412f
commit 775b80eff5
25 changed files with 560 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
tools/KittiDataset/main.cpp
View File

@@ -61,9 +61,7 @@ void showUsage()
" --scan_step # Scan downsample step (default=10).\n"
" --scan_voxel #.# Scan voxel size (default 0.3 m).\n"
" --scan_k Scan normal K (default 5).\n"
" --scan_radius Scan normal radius (default 0).\n"
" --map_update # Do map update each X odometry frames (default=10, which\n"
" gives 1 Hz map update assuming images are at 10 Hz).\n\n"
" --scan_radius Scan normal radius (default 0).\n\n"
"%s\n"
"Example:\n\n"
" $ rtabmap-kitti_dataset \\\n"
@@ -111,7 +109,6 @@ int main(int argc, char * argv[])
std::string path;
std::string output;
std::string seq;
int mapUpdate = 10;
bool color = false;
bool scan = false;
bool disp = false;
@@ -137,15 +134,6 @@ int main(int argc, char * argv[])
{
quiet = true;
}
else if(std::strcmp(argv[i], "--map_update") == 0)
{
mapUpdate = atoi(argv[++i]);
if(mapUpdate <= 0)
{
printf("map_update should be > 0\n");
showUsage();
}
}
else if(std::strcmp(argv[i], "--scan_step") == 0)
{
scanStep = atoi(argv[++i]);
@@ -379,8 +367,18 @@ int main(int argc, char * argv[])
Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
}
float detectionRate = Parameters::defaultRtabmapDetectionRate();
bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
// assuming source is 10 Hz
int mapUpdate = 10 / detectionRate;
if(mapUpdate < 1)
{
mapUpdate = 1;
}
std::string databasePath = output+"/rtabmap" + seq + ".db";
UFile::erase(databasePath);
if(cameraThread.camera()->init(output, "rtabmap_calib"+seq))
@@ -403,6 +401,7 @@ int main(int argc, char * argv[])
// Processing dataset begin
/////////////////////////////
cv::Mat covariance;
int odomKeyFrames = 0;
while(data.isValid() && g_forever)
{
std::map<std::string, float> externalStats;
@@ -425,6 +424,7 @@ int main(int argc, char * argv[])
externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
float speed = 0.0f;
if(odomInfo.interval>0.0)
speed = odomInfo.transform.x()/odomInfo.interval*3.6;
@@ -436,6 +436,10 @@ int main(int argc, char * argv[])
externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
if(odomInfo.keyFrameAdded)
{
++odomKeyFrames;
}
bool processData = true;
if(iteration % mapUpdate != 0)
@@ -459,7 +463,7 @@ int main(int argc, char * argv[])
}
++iteration;
if(!quiet)
if(!quiet || iteration == totalImages)
{
double slamTime = timer.ticks();
@@ -469,15 +473,31 @@ int main(int argc, char * argv[])
rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
}
if(rmse >= 0.0f)
if(data.keypoints().size() == 0 && data.laserScanRaw().cols)
{
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm, stddev=%fm %frad",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
if(rmse >= 0.0f)
{
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
}
else
{
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
}
}
else
{
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
if(rmse >= 0.0f)
{
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
}
else
{
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
}
}
if(processData && rtabmap.getLoopClosureId()>0)
{

16
tools/RgbdDataset/main.cpp
View File

@@ -230,6 +230,7 @@ int main(int argc, char * argv[])
// Processing dataset begin
/////////////////////////////
cv::Mat covariance;
int odomKeyFrames = 0;
double previousStamp = 0.0;
while(data.isValid() && g_forever)
{
@@ -253,6 +254,7 @@ int main(int argc, char * argv[])
externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
@@ -260,6 +262,10 @@ int main(int argc, char * argv[])
externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
if(odomInfo.keyFrameAdded)
{
++odomKeyFrames;
}
bool processData = true;
if(detectionRate>0.0f &&
@@ -295,7 +301,7 @@ int main(int argc, char * argv[])
}
++iteration;
if(!quiet)
if(!quiet || iteration == totalImages)
{
double slamTime = timer.ticks();
@@ -307,13 +313,13 @@ int main(int argc, char * argv[])
if(rmse >= 0.0f)
{
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm, stddev=%fm %frad",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
}
else
{
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
}
if(processData && rtabmap.getLoopClosureId()>0)
{