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

Update 0.15.4.

Browse Source 
Parameters:
-Added "GridGlobal/MaxNodes=0", "Rtabmap/PublishRAMUsage=false", "Mem/DepthAsMask=true", "Kp/FlannRebalancingFactor=2.0", "Vis/DepthAsMask=true".
-Modified "Kp/DetectorStrategy=6", "Kp/MaxFeatures=500",  "Mem/UseOdomFeatures=true", "GFTT/QualityLevel=0.001", "GFTT/MinDistance=3", "RGBD/OptimizeMaxError=1", "RGBD/ProximityPathFilteringRadius=1", "Odom/GuessMotion=true", "Odom/VisKeyFrameThr=150", "OdomF2M/BundleAdjustment=1", "Vis/Iterations=300" if built with g2o, "OdomF2M/BundleAdjustmentMaxFrames=10", "OdomFovis/MinFeaturesForEstimate=20", "OdomORBSLAM2/MapSize=3000", "Reg/RepeatOnce=true", "Vis/PnPRefineIterations=0" if built with g2o, "Vis/CorGuessMatchToProjection=true", "Vis/BundleAdjustment=1" if built with g2o, "Icp/MaxCorrespondenceDistance=0.1", "Icp/PointToPlaneK=5", "Icp/PointToPlaneRadius=1", "Icp/PM=true" if built with libpointmatcher, "Stereo/MaxLevel=5", "Stereo/MinDisparity=0.5".

BayesFilter: optimized prediction matrix update. Use of new argument "ignoreLocalSpaceLoopIds" of Memory::getNeighborsId() to ignore loop closure link by space in prediction update.
CameraThread: Added stereo exposure compensation option.
CameraRGB: Added forceGroundNormalsUp option and added support of ground truth from EuRoC dataset.
Statistics: Added "Memory/RAM_usage/MB".
Transform: Added clone() method to do deep copy.
Graph::importPoses(): EuRoC format support (9).
Rtabmap: Local visual loop closures are now identified as GlobalClosure link type.
OccupancyGrid/OctoMap: updated how cache is used (old node retrieved can be re-added to map without re-assembling the whole map).
OdometryF2F: when using ICP, increasing correspondence distance for first two frames. If Vis/CorType=1 and registration fails, second guess without motion is done with Vis/CorType=0.
OdometryF2M/RegVis: updated how features are removed from the map, using new projectedIDs filled in RegistrationInfo by RegistrationVis.
OdometryORBSLAM2: Maximum size of the feature map can be set with "OdomORBSLAM2/MapSize" parameter.
CloudViewer: fixed opengl camera drifting in follow mode.
DatabaseViewer: Added optimization scale option. ConstraintsView: hide loop closure links if type is ignored in gui parameters.
MainWindow: Support of "GridGlobal/MaxNodes" parameters when updating the maps.
UPlot: don't show ellipses when not in graphics view mode, updated how "random" colors are attributed to curves
Added rtabmap-euroc_dataset tool. Updated rtabmap-kitti_dataset and rtabmap-rgbd_dataset tools.
Added rtabmap-reprocess tool.
This commit is contained in:
matlabbe
2018-02-01 22:17:46 -05:00
parent 9f80f4ac42
commit 977d21eed5
76 changed files with 3844 additions and 1307 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/KittiDataset/main.cpp
+124 -56
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UFile.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UProcessInfo.h"
#include <pcl/common/common.h>
#include <stdio.h>
#include <signal.h>
@@ -53,14 +54,17 @@ void showUsage()
" containing least calib.txt, times.txt, image_0 and image_1 folders.\n"
" Optional image_2, image_3 and velodyne folders.\n"
" --output Output directory. By default, results are saved in \"path\".\n"
" --output_name Output database name (default \"rtabmap\").\n"
" --gt \"path\" Ground truth path (e.g., ~/KITTI/devkit/cpp/data/odometry/poses/07.txt)\n"
" --quiet Don't show log messages and iteration updates.\n"
" --color Use color images for stereo (image_2 and image_3 folders).\n"
" --scaling Scale stereo baseline on some sequences (03-04-05-06).\n"
" --disp Generate full disparity.\n"
" --exposure_comp Do exposure compensation between left and right images.\n"
" --scan Include velodyne scan in node's data.\n"
" --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_step # Scan downsample step (default=1).\n"
" --scan_voxel #.# Scan voxel size (default 0.5 m).\n"
" --scan_k Scan normal K (default 0).\n"
" --scan_radius Scan normal radius (default 0).\n\n"
"%s\n"
"Example:\n\n"
@@ -108,13 +112,16 @@ int main(int argc, char * argv[])
ParametersMap parameters;
std::string path;
std::string output;
std::string outputName = "rtabmap";
std::string seq;
bool color = false;
bool scaling = false;
bool scan = false;
bool disp = false;
int scanStep = 10;
float scanVoxel = 0.3f;
int scanNormalK = 5;
bool exposureCompensation = false;
int scanStep = 1;
float scanVoxel = 0.5f;
int scanNormalK = 0;
float scanNormalRadius = 0.0f;
std::string gtPath;
bool quiet = false;
@@ -130,6 +137,10 @@ int main(int argc, char * argv[])
{
output = argv[++i];
}
else if(std::strcmp(argv[i], "--output_name") == 0)
{
outputName = argv[++i];
}
else if(std::strcmp(argv[i], "--quiet") == 0)
{
quiet = true;
@@ -178,6 +189,10 @@ int main(int argc, char * argv[])
{
color = true;
}
else if(std::strcmp(argv[i], "--scaling") == 0)
{
scaling = true;
}
else if(std::strcmp(argv[i], "--scan") == 0)
{
scan = true;
@@ -186,6 +201,10 @@ int main(int argc, char * argv[])
{
disp = true;
}
else if(std::strcmp(argv[i], "--exposure_comp") == 0)
{
exposureCompensation = true;
}
}
parameters = Parameters::parseArguments(argc, argv);
path = argv[argc-1];
@@ -200,6 +219,8 @@ int main(int argc, char * argv[])
output = uReplaceChar(output, '~', UDirectory::homeDir());
UDirectory::makeDir(output);
}
parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
}
seq = uSplit(path, '/').back();
@@ -218,6 +239,7 @@ int main(int argc, char * argv[])
" Sequence number: %s\n"
" Sequence path: %s\n"
" Output: %s\n"
" Output name: %s\n"
" left images: %s\n"
" right images: %s\n"
" calib.txt: %s\n"
@@ -225,6 +247,7 @@ int main(int argc, char * argv[])
seq.c_str(),
path.c_str(),
output.c_str(),
outputName.c_str(),
pathLeftImages.c_str(),
pathRightImages.c_str(),
pathCalib.c_str(),
@@ -243,10 +266,8 @@ int main(int argc, char * argv[])
printf(" Ground Truth: %s\n", gtPath.c_str());
}
}
if(disp)
{
printf(" Disparity: %s\n", disp?"true":"false");
}
printf(" Exposure Compensation: %s\n", exposureCompensation?"true":"false");
printf(" Disparity: %s\n", disp?"true":"false");
if(scan)
{
pathScan = path+"/velodyne";
@@ -256,15 +277,6 @@ int main(int argc, char * argv[])
printf(" Scan normal k: %d\n", scanNormalK);
printf(" Scan normal radius: %f\n", scanNormalRadius);
}
if(!parameters.empty())
{
printf("Parameters:\n");
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
printf(" %s=%s\n", iter->first.c_str(), iter->second.c_str());
}
}
printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
// convert calib.txt to rtabmap format (yaml)
FILE * pFile = 0;
@@ -320,7 +332,28 @@ int main(int argc, char * argv[])
UERROR("Failed to read first image of \"%s\"", firstImage.c_str());
return -1;
}
StereoCameraModel model("rtabmap_calib"+seq,
if(scaling)
{
// scale baseline
if(uStr2Int(seq) == 3 || uStr2Int(seq) == 5 || uStr2Int(seq) == 9)
{
P1.at<double>(0,3) *= 0.9905;
printf(" Baseline scaling factor: %f\n", 0.9905);
}
else if(uStr2Int(seq) == 4)
{
P1.at<double>(0,3) *= 0.987000;
printf(" Baseline scaling factor: %f\n", 0.987000);
}
else if(uStr2Int(seq) == 6)
{
P1.at<double>(0,3) *= 0.985000;
printf(" Baseline scaling factor: %f\n", 0.985000);
}
}
StereoCameraModel model(outputName+"_calib",
image.size(), P0.colRange(0,3), cv::Mat(), cv::Mat(), P0,
image.size(), P1.colRange(0,3), cv::Mat(), cv::Mat(), P1,
cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat());
@@ -329,8 +362,18 @@ int main(int argc, char * argv[])
UERROR("Could not save calibration!");
return -1;
}
printf("Saved calibration \"%s\" to \"%s\"\n", ("rtabmap_calib"+seq).c_str(), output.c_str());
printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
if(!parameters.empty())
{
printf("Parameters:\n");
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
printf(" %s=%s\n", iter->first.c_str(), iter->second.c_str());
}
}
printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
if(quiet)
{
@@ -347,6 +390,10 @@ int main(int argc, char * argv[])
0.0f,
opticalRotation), parameters);
((CameraStereoImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
if(exposureCompensation)
{
cameraThread.setStereoExposureCompensation(true);
}
if(disp)
{
cameraThread.setStereoToDepth(true);
@@ -364,11 +411,14 @@ int main(int argc, char * argv[])
scanVoxel,
scanNormalK,
scanNormalRadius,
Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f),
true);
}
float detectionRate = Parameters::defaultRtabmapDetectionRate();
bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
int odomStrategy = Parameters::defaultOdomStrategy();
Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
@@ -379,15 +429,17 @@ int main(int argc, char * argv[])
mapUpdate = 1;
}
std::string databasePath = output+"/rtabmap" + seq + ".db";
std::string databasePath = output+"/"+outputName+".db";
UFile::erase(databasePath);
if(cameraThread.camera()->init(output, "rtabmap_calib"+seq))
if(cameraThread.camera()->init(output, outputName+"_calib"))
{
int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
printf("Processing %d images...\n", totalImages);
Odometry * odom = Odometry::create(parameters);
ParametersMap odomParameters = parameters;
odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
Odometry * odom = Odometry::create(odomParameters);
Rtabmap rtabmap;
rtabmap.init(parameters, databasePath);
@@ -404,43 +456,29 @@ int main(int argc, char * argv[])
int odomKeyFrames = 0;
while(data.isValid() && g_forever)
{
std::map<std::string, float> externalStats;
cameraThread.postUpdate(&data, &cameraInfo);
cameraInfo.timeTotal = timer.ticks();
// save camera statistics to database
externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
OdometryInfo odomInfo;
Transform pose = odom->process(data, &odomInfo);
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
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;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
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));
externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
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;
}
if(odomStrategy == 2)
{
//special case for FOVIS, set covariance 1 if 9999 is detected
if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
{
odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
}
bool processData = true;
if(iteration % mapUpdate != 0)
{
@@ -457,6 +495,32 @@ int main(int argc, char * argv[])
timer.restart();
if(processData)
{
std::map<std::string, float> externalStats;
// save camera statistics to database
externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
// save odometry statistics to database
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
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/Speed/kph", speed));
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));
externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
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));
OdometryEvent e(SensorData(), Transform(), odomInfo);
rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
covariance = cv::Mat();
@@ -477,8 +541,10 @@ int main(int argc, char * argv[])
{
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)));
//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)));
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
}
else
{
@@ -490,8 +556,10 @@ int main(int argc, char * argv[])
{
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)));
//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)));
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
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);
}
else
{
@@ -526,7 +594,7 @@ int main(int argc, char * argv[])
std::map<int, Transform> poses;
std::multimap<int, Link> links;
rtabmap.getGraph(poses, links, true, true);
std::string pathTrajectory = output+"/rtabmap_poses"+seq+".txt";
std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
{
printf("Saving %s... done!\n", pathTrajectory.c_str());
@@ -597,7 +665,7 @@ int main(int argc, char * argv[])
printf(" rotational_rmse= %f deg\n", rotational_rmse);
pFile = 0;
std::string pathErrors = output+"/rtabmap_rmse"+seq+".txt";
std::string pathErrors = output+"/"+outputName+"_rmse.txt";
pFile = fopen(pathErrors.c_str(),"w");
if(!pFile)
{
@@ -626,9 +694,9 @@ int main(int argc, char * argv[])
UERROR("Camera init failed!");
}
printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/rtabmap" + seq + ".db").c_str());
printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
printf("Do:\n"
" $ rtabmap-databaseViewer %s\n\n", (output+"/rtabmap" + seq + ".db").c_str());
" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
return 0;
}