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

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

4
tools/CMakeLists.txt
View File

@@ -7,8 +7,9 @@ ADD_SUBDIRECTORY( CameraRGBD )
ADD_SUBDIRECTORY( StereoEval )
ADD_SUBDIRECTORY( KittiDataset )
ADD_SUBDIRECTORY( RgbdDataset )
ADD_SUBDIRECTORY( EurocDataset )
ADD_SUBDIRECTORY( Recovery )
ADD_SUBDIRECTORY( Report )
ADD_SUBDIRECTORY( Reprocess )
IF(OPENCV_NONFREE_FOUND)
ADD_SUBDIRECTORY( VocabularyComparison )
@@ -20,6 +21,7 @@ IF(TARGET rtabmap_gui)
ADD_SUBDIRECTORY( OdometryViewer )
ADD_SUBDIRECTORY( DataRecorder )
ADD_SUBDIRECTORY( Calibration )
ADD_SUBDIRECTORY( Report )
ELSE()
MESSAGE(STATUS "RTAB-Map GUI lib is not built, some tools won't be built...")
ENDIF()

48
tools/EurocDataset/CMakeLists.txt Normal file
View File

@@ -0,0 +1,48 @@
cmake_minimum_required(VERSION 2.8)
FIND_PACKAGE(yaml-cpp REQUIRED)
IF(yaml-cpp_FOUND)
# inside rtabmap project (see below for external build)
SET(RTABMap_INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/utilite/include
${PROJECT_SOURCE_DIR}/corelib/include
)
SET(RTABMap_LIBRARIES
rtabmap_core
rtabmap_utilite
)
if(POLICY CMP0020)
cmake_policy(SET CMP0020 OLD)
endif()
SET(INCLUDE_DIRS
${RTABMap_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
SET(LIBRARIES
${RTABMap_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
${YAML_CPP_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(euroc_dataset main.cpp)
TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES})
SET_TARGET_PROPERTIES( euroc_dataset
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-euroc_dataset)
INSTALL(TARGETS euroc_dataset
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
ELSE()
MESSAGE(STATUS "yaml-cpp not found, euroc_dataset tool won't be built...")
ENDIF()

538
tools/EurocDataset/main.cpp Normal file
View File

@@ -0,0 +1,538 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/OdometryF2M.h>
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UDirectory.h"
#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 <yaml-cpp/yaml.h>
#include <stdio.h>
#include <signal.h>
using namespace rtabmap;
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-kitti_dataset [options] path\n"
" path Folder of the sequence (e.g., \"~/EuRoC/V1_03_difficult\")\n"
" containing least mav0/cam0/sensor.yaml, mav0/cam1/sensor.yaml, mav0/cam0/data and mav0/cam1/data 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"
" --quiet Don't show log messages and iteration updates.\n"
" --exposure_comp Do exposure compensation between left and right images.\n"
" --disp Generate full disparity.\n"
"%s\n"
"Example:\n\n"
" $ rtabmap-euroc_dataset --Rtabmap/DetectionRate 4 ~/EuRoC/V1_03_difficult\n\n", rtabmap::Parameters::showUsage());
exit(1);
}
// catch ctrl-c
bool g_forever = true;
void sighandler(int sig)
{
printf("\nSignal %d caught...\n", sig);
g_forever = false;
}
int main(int argc, char * argv[])
{
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ParametersMap parameters;
std::string path;
std::string output;
std::string outputName = "rtabmap";
std::string seq;
bool disp = false;
bool exposureCompensation = false;
bool quiet = false;
if(argc < 2)
{
showUsage();
}
else
{
for(int i=1; i<argc; ++i)
{
if(std::strcmp(argv[i], "--output") == 0)
{
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;
}
else if(std::strcmp(argv[i], "--disp") == 0)
{
disp = true;
}
else if(std::strcmp(argv[i], "--exposure_comp") == 0)
{
exposureCompensation = true;
}
}
parameters = Parameters::parseArguments(argc, argv);
path = argv[argc-1];
path = uReplaceChar(path, '~', UDirectory::homeDir());
path = uReplaceChar(path, '\\', '/');
if(output.empty())
{
output = path;
}
else
{
output = uReplaceChar(output, '~', UDirectory::homeDir());
UDirectory::makeDir(output);
}
parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
}
seq = uSplit(path, '/').back();
std::string pathLeftImages = path+"/mav0/cam0/data";
std::string pathRightImages = path+"/mav0/cam1/data";
std::string pathCalibLeft = path+"/mav0/cam0/sensor.yaml";
std::string pathCalibRight = path+"/mav0/cam1/sensor.yaml";
std::string pathGt = path+"/mav0/state_groundtruth_estimate0/data.csv";
if(!UFile::exists(pathGt))
{
UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
pathGt.clear();
}
printf("Paths:\n"
" Sequence number: %s\n"
" Sequence path: %s\n"
" Output: %s\n"
" Output name: %s\n"
" left images: %s\n"
" right images: %s\n"
" left calib: %s\n"
" right calib: %s\n",
seq.c_str(),
path.c_str(),
output.c_str(),
outputName.c_str(),
pathLeftImages.c_str(),
pathRightImages.c_str(),
pathCalibLeft.c_str(),
pathCalibRight.c_str());
if(!pathGt.empty())
{
printf(" Ground truth: %s\n", pathGt.c_str());
}
printf(" Exposure Compensation: %s\n", exposureCompensation?"true":"false");
printf(" Disparity: %s\n", disp?"true":"false");
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);
std::vector<CameraModel> models;
int rateHz = 20;
for(int k=0; k<2; ++k)
{
// Left calibration
std::string calibPath = k==0?pathCalibLeft:pathCalibRight;
YAML::Node config = YAML::LoadFile(calibPath);
if(config.IsNull())
{
UERROR("Cannot open calibration file \"%s\"", calibPath.c_str());
return -1;
}
YAML::Node T_BS = config["T_BS"];
YAML::Node data = T_BS["data"];
UASSERT(data.size() == 16);
rateHz = config["rate_hz"].as<int>();
YAML::Node resolution = config["resolution"];
UASSERT(resolution.size() == 2);
YAML::Node intrinsics = config["intrinsics"];
UASSERT(intrinsics.size() == 4);
YAML::Node distortion_coefficients = config["distortion_coefficients"];
UASSERT(distortion_coefficients.size() == 4 || distortion_coefficients.size() == 5 || distortion_coefficients.size() == 8);
cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
K.at<double>(0,0) = intrinsics[0].as<double>();
K.at<double>(1,1) = intrinsics[1].as<double>();
K.at<double>(0,2) = intrinsics[2].as<double>();
K.at<double>(1,2) = intrinsics[3].as<double>();
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
cv::Mat D = cv::Mat::zeros(1, distortion_coefficients.size(), CV_64FC1);
for(unsigned int i=0; i<distortion_coefficients.size(); ++i)
{
D.at<double>(i) = distortion_coefficients[i].as<double>();
}
Transform t(data[0].as<float>(), data[1].as<float>(), data[2].as<float>(), data[3].as<float>(),
data[4].as<float>(), data[5].as<float>(), data[6].as<float>(), data[7].as<float>(),
data[8].as<float>(), data[9].as<float>(), data[10].as<float>(), data[11].as<float>());
models.push_back(CameraModel(outputName+"_calib", cv::Size(resolution[0].as<int>(),resolution[1].as<int>()), K, D, R, P, t));
UASSERT(models.back().isValidForRectification());
}
StereoCameraModel model(outputName+"_calib", models[0], models[1], models[1].localTransform().inverse() * models[0].localTransform());
if(!model.save(output, true))
{
UERROR("Could not save calibration!");
return -1;
}
printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
if(quiet)
{
ULogger::setLevel(ULogger::kError);
}
// We use CameraThread only to use postUpdate() method
Transform opticalRotation(0,0,1,0, 0,-1,0,0, 1,0,0,0);
CameraThread cameraThread(new
CameraStereoImages(
pathLeftImages,
pathRightImages,
true,
0.0f,
opticalRotation*models[0].localTransform()), parameters);
((CameraStereoImages*)cameraThread.camera())->setTimestamps(true, "", false);
if(exposureCompensation)
{
cameraThread.setStereoExposureCompensation(true);
}
if(disp)
{
cameraThread.setStereoToDepth(true);
}
if(!pathGt.empty())
{
((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 9);
}
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);
int mapUpdate = rateHz / detectionRate;
if(mapUpdate < 1)
{
mapUpdate = 1;
}
std::string databasePath = output+"/"+outputName+".db";
UFile::erase(databasePath);
if(cameraThread.camera()->init(output, outputName+"_calib"))
{
int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
printf("Processing %d images...\n", totalImages);
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);
UTimer totalTime;
UTimer timer;
CameraInfo cameraInfo;
SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
int iteration = 0;
/////////////////////////////
// Processing dataset begin
/////////////////////////////
cv::Mat covariance;
int odomKeyFrames = 0;
while(data.isValid() && g_forever)
{
cameraThread.postUpdate(&data, &cameraInfo);
cameraInfo.timeTotal = timer.ticks();
OdometryInfo odomInfo;
Transform pose = odom->process(data, &odomInfo);
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)
{
// set negative id so rtabmap will detect it as an intermediate node
data.setId(-1);
data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
processData = intermediateNodes;
}
if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
{
covariance = odomInfo.reg.covariance;
}
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/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();
}
++iteration;
if(!quiet || iteration == totalImages)
{
double slamTime = timer.ticks();
float rmse = -1;
if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
{
rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
}
if(data.keypoints().size() == 0 && data.laserScanRaw().cols)
{
if(rmse >= 0.0f)
{
printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
}
else
{
printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
}
}
else
{
if(rmse >= 0.0f)
{
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
}
else
{
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)
{
printf(" *");
}
printf("\n");
}
else if(iteration % (totalImages/10) == 0)
{
printf(".");
fflush(stdout);
}
cameraInfo = CameraInfo();
timer.restart();
data = cameraThread.camera()->takeImage(&cameraInfo);
}
delete odom;
printf("Total time=%fs\n", totalTime.ticks());
/////////////////////////////
// Processing dataset end
/////////////////////////////
// Save trajectory
printf("Saving trajectory ...\n");
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures;
std::map<int, double> stamps;
rtabmap.getGraph(poses, links, true, true, &signatures);
for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
}
std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
{
printf("Saving %s... done!\n", pathTrajectory.c_str());
}
else
{
printf("Saving %s... failed!\n", pathTrajectory.c_str());
}
if(!pathGt.empty())
{
// Log ground truth statistics
std::map<int, Transform> groundTruth;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform o, gtPose;
int m,w;
std::string l;
double s;
std::vector<float> v;
GPS gps;
rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
if(!gtPose.isNull())
{
groundTruth.insert(std::make_pair(iter->first, gtPose));
}
}
// compute RMSE statistics
float translational_rmse = 0.0f;
float translational_mean = 0.0f;
float translational_median = 0.0f;
float translational_std = 0.0f;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_rmse = 0.0f;
float rotational_mean = 0.0f;
float rotational_median = 0.0f;
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
graph::calcRMSE(
groundTruth,
poses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
printf(" translational_rmse= %f m\n", translational_rmse);
printf(" rotational_rmse= %f deg\n", rotational_rmse);
FILE * pFile = 0;
std::string pathErrors = output+"/"+outputName+"_rmse.txt";
pFile = fopen(pathErrors.c_str(),"w");
if(!pFile)
{
UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
}
fprintf(pFile, "Ground truth comparison:\n");
fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
fprintf(pFile, " translational_mean= %f\n", translational_mean);
fprintf(pFile, " translational_median= %f\n", translational_median);
fprintf(pFile, " translational_std= %f\n", translational_std);
fprintf(pFile, " translational_min= %f\n", translational_min);
fprintf(pFile, " translational_max= %f\n", translational_max);
fprintf(pFile, " rotational_rmse= %f\n", rotational_rmse);
fprintf(pFile, " rotational_mean= %f\n", rotational_mean);
fprintf(pFile, " rotational_median= %f\n", rotational_median);
fprintf(pFile, " rotational_std= %f\n", rotational_std);
fprintf(pFile, " rotational_min= %f\n", rotational_min);
fprintf(pFile, " rotational_max= %f\n", rotational_max);
fclose(pFile);
}
}
else
{
UERROR("Camera init failed!");
}
printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
printf("Do:\n"
" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
return 0;
}

4
tools/KittiDataset/CMakeLists.txt
View File

@@ -34,3 +34,7 @@ TARGET_LINK_LIBRARIES(kitti_dataset ${LIBRARIES})
SET_TARGET_PROPERTIES( kitti_dataset
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-kitti_dataset)
INSTALL(TARGETS kitti_dataset
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

180
tools/KittiDataset/main.cpp
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;
}

3
tools/Recovery/CMakeLists.txt
View File

@@ -33,6 +33,9 @@ TARGET_LINK_LIBRARIES(recovery ${LIBRARIES})
SET_TARGET_PROPERTIES( recovery
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-recovery)
INSTALL(TARGETS recovery
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

11
tools/Report/CMakeLists.txt
View File

@@ -4,10 +4,12 @@ cmake_minimum_required(VERSION 2.8)
SET(RTABMap_INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/utilite/include
${PROJECT_SOURCE_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/guilib/include
)
SET(RTABMap_LIBRARIES
rtabmap_core
rtabmap_utilite
rtabmap_gui
)
if(POLICY CMP0020)
@@ -20,10 +22,15 @@ SET(INCLUDE_DIRS
${PCL_INCLUDE_DIRS}
)
IF(QT4_FOUND)
INCLUDE(${QT_USE_FILE})
ENDIF(QT4_FOUND)
SET(LIBRARIES
${RTABMap_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
${QT_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
@@ -34,3 +41,7 @@ TARGET_LINK_LIBRARIES(report ${LIBRARIES})
SET_TARGET_PROPERTIES( report
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-report)
INSTALL(TARGETS report
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

449
tools/Report/main.cpp
View File

@@ -26,10 +26,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UPlot.h>
#include <QApplication>
#include <stdio.h>
using namespace rtabmap;
@@ -37,8 +41,8 @@ using namespace rtabmap;
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-report path\n"
" path Directory containing rtabmap databases.\n\n");
"rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] path\n"
" path Directory containing rtabmap databases or path of a database.\n\n");
exit(1);
}
@@ -49,34 +53,113 @@ int main(int argc, char * argv[])
showUsage();
}
std::string path = argv[1];
QApplication app(argc, argv);
bool outputLatex = false;
bool outputScaled = false;
bool outputKittiError = false;
std::map<std::string, UPlot*> figures;
for(int i=1; i<argc-1; ++i)
{
if(strcmp(argv[i], "--latex") == 0)
{
outputLatex = true;
}
else if(strcmp(argv[i], "--kitti") == 0)
{
outputKittiError = true;
}
else if(strcmp(argv[i], "--scale") == 0)
{
outputScaled = true;
}
else
{
std::string figureTitle = argv[i];
printf("Plot %s\n", figureTitle.c_str());
UPlot * fig = new UPlot();
fig->setTitle(figureTitle.c_str());
fig->setXLabel("Time (s)");
figures.insert(std::make_pair(figureTitle, fig));
}
}
std::string path = argv[argc-1];
path = uReplaceChar(path, '~', UDirectory::homeDir());
std::string fileName;
std::list<std::string> paths;
paths.push_back(path);
std::vector<std::map<std::string, std::vector<float> > > outputLatexStatistics;
std::map<std::string, std::vector<float> > outputLatexStatisticsMap;
bool odomRAMSet = false;
std::set<std::string> topDirs;
while(paths.size())
{
std::string currentPath = paths.front();
UDirectory currentDir(currentPath);
paths.pop_front();
bool currentPathIsDatabase = false;
if(!currentDir.isValid())
{
continue;
if(UFile::getExtension(currentPath).compare("db") == 0)
{
currentPathIsDatabase=true;
printf("Database: %s\n", currentPath.c_str());
}
else
{
continue;
}
}
std::list<std::string> subDirs;
printf("Directory: %s\n", currentPath.c_str());
while(!(fileName = currentDir.getNextFileName()).empty())
if(!currentPathIsDatabase)
{
if(UFile::getExtension(fileName).compare("db") == 0)
printf("Directory: %s\n", currentPath.c_str());
std::list<std::string> fileNames = currentDir.getFileNames();
if(topDirs.empty())
{
std::string filePath = currentPath + UDirectory::separator() + fileName;
for(std::list<std::string>::iterator iter = fileNames.begin(); iter!=fileNames.end(); ++iter)
{
topDirs.insert(currentPath+"/"+*iter);
}
}
else
{
if(topDirs.find(currentPath) != topDirs.end())
{
if(outputLatexStatisticsMap.size())
{
outputLatexStatistics.push_back(outputLatexStatisticsMap);
outputLatexStatisticsMap.clear();
}
}
}
}
while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
{
if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
{
std::string filePath;
if(currentPathIsDatabase)
{
filePath = currentPath;
}
else
{
filePath = currentPath + UDirectory::separator() + fileName;
}
DBDriver * driver = DBDriver::create();
ParametersMap params;
if(driver->openConnection(filePath))
{
params = driver->getLastParameters();
std::set<int> ids;
driver->getAllNodeIds(ids);
std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
std::map<int, Transform> odomPoses, gtPoses;
std::vector<float> cameraTime;
cameraTime.reserve(ids.size());
std::vector<float> odomTime;
@@ -85,49 +168,251 @@ int main(int argc, char * argv[])
slamTime.reserve(ids.size());
float rmse = -1;
float maxRMSE = -1;
float rmseAng = -1;
float maxOdomRAM = -1;
float maxMapRAM = -1;
std::map<std::string, UPlotCurve*> curves;
std::map<std::string, double> firstStamps;
for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
{
curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
}
for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
Transform p, gt;
GPS gps;
int m, w;
int m=-1, w=-1;
std::string l;
double s;
std::vector<float> v;
driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps);
if(uContains(stats, *iter))
if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps))
{
const std::map<std::string, float> & stat = stats.at(*iter).first;
if(uContains(stat, Statistics::kGtTranslational_rmse()))
odomPoses.insert(std::make_pair(*iter, p));
if(!gt.isNull())
{
rmse = stat.at(Statistics::kGtTranslational_rmse());
if(maxRMSE==-1 || maxRMSE < rmse)
gtPoses.insert(std::make_pair(*iter, gt));
}
if(uContains(stats, *iter))
{
const std::map<std::string, float> & stat = stats.at(*iter).first;
if(uContains(stat, Statistics::kGtTranslational_rmse()))
{
maxRMSE = rmse;
rmse = stat.at(Statistics::kGtTranslational_rmse());
if(maxRMSE==-1 || maxRMSE < rmse)
{
maxRMSE = rmse;
}
}
if(uContains(stat, Statistics::kGtRotational_rmse()))
{
rmseAng = stat.at(Statistics::kGtRotational_rmse());
}
if(uContains(stat, std::string("Camera/TotalTime/ms")))
{
cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
}
if(uContains(stat, std::string("Odometry/TotalTime/ms")))
{
odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
}
if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
{
if(w>=0 || stat.at("RtabmapROS/TotalTime/ms") > 10.0f)
{
slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
}
}
else if(uContains(stat, Statistics::kTimingTotal()))
{
if(w>=0 || stat.at(Statistics::kTimingTotal()) > 10.0f)
{
slamTime.push_back(stat.at(Statistics::kTimingTotal()));
}
}
if(uContains(stat, std::string(Statistics::kMemoryRAM_usage())))
{
float ram = stat.at(Statistics::kMemoryRAM_usage());
if(maxMapRAM==-1 || maxMapRAM < ram)
{
maxMapRAM = ram;
}
}
if(uContains(stat, std::string("Odometry/RAM_usage/MB")))
{
float ram = stat.at("Odometry/RAM_usage/MB");
if(maxOdomRAM==-1 || maxOdomRAM < ram)
{
maxOdomRAM = ram;
}
}
for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
{
if(uContains(stat, jter->first))
{
if(!uContains(firstStamps, jter->first))
{
firstStamps.insert(std::make_pair(jter->first, s));
}
float x = s - firstStamps.at(jter->first);
float y = stat.at(jter->first);
jter->second->addValue(x,y);
}
}
}
if(uContains(stat, std::string("Camera/TotalTime/ms")))
{
cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
}
if(uContains(stat, std::string("Odometry/TotalTime/ms")))
{
odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
}
if(w >= 0 && uContains(stat, Statistics::kTimingTotal()))
{
slamTime.push_back(stat.at(Statistics::kTimingTotal()));
}
}
}
printf(" %s (%d): %fm (max=%fm), slam: avg=%dms max=%dms, odom: avg=%dms max=%dms, camera: avg=%dms max=%dms\n",
std::multimap<int, Link> links;
driver->getAllLinks(links, true);
std::multimap<int, Link> loopClosureLinks;
for(std::multimap<int, Link>::iterator jter=links.begin(); jter!=links.end(); ++jter)
{
if(jter->second.type() == Link::kGlobalClosure &&
graph::findLink(loopClosureLinks, jter->second.from(), jter->second.to()) == loopClosureLinks.end())
{
loopClosureLinks.insert(*jter);
}
}
UERROR("");
float bestScale = 1.0f;
float bestRMSE = rmse;
float bestRMSEAng = rmseAng;
float kitti_t_err = 0.0f;
float kitti_r_err = 0.0f;
if(ids.size())
{
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
int firstId = *ids.begin();
rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
optimizer->getConnectedGraph(firstId, odomPoses, graph::filterDuplicateLinks(links), posesOut, linksOut);
std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut);
std::map<int, Transform> groundTruth;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(gtPoses.find(iter->first) != gtPoses.end())
{
groundTruth.insert(*gtPoses.find(iter->first));
}
}
if(outputScaled)
{
for(float scale=0.900f; scale<1.100f; scale+=0.001)
{
std::map<int, Transform> scaledPoses;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform t = iter->second.clone();
t.x() *= scale;
t.y() *= scale;
t.z() *= scale;
scaledPoses.insert(std::make_pair(iter->first, t));
}
// compute RMSE statistics
float translational_rmse = 0.0f;
float translational_mean = 0.0f;
float translational_median = 0.0f;
float translational_std = 0.0f;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_rmse = 0.0f;
float rotational_mean = 0.0f;
float rotational_median = 0.0f;
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
graph::calcRMSE(
groundTruth,
scaledPoses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
if(scale!=0.900f && translational_rmse > bestRMSE)
{
break;
}
bestRMSE = translational_rmse;
bestRMSEAng = rotational_rmse;
bestScale = scale;
}
if(bestScale!=1.0f)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
iter->second.x()*=bestScale;
iter->second.y()*=bestScale;
iter->second.z()*=bestScale;
}
}
}
if(outputKittiError)
{
if(groundTruth.size() == poses.size())
{
// compute KITTI statistics
graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), kitti_t_err, kitti_r_err);
}
else
{
printf("Cannot compute KITTI statistics as optimized poses and ground truth don't have the same size (%d vs %d).\n",
(int)poses.size(), (int)groundTruth.size());
}
}
}
printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm) ang=%.1fdeg%s, slam: avg=%dms (max=%dms) loops=%d, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
fileName.c_str(),
(int)ids.size(),
rmse,
bestScale,
bestRMSE,
maxRMSE,
bestRMSEAng,
!outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
(int)uMean(slamTime), (int)uMax(slamTime),
(int)loopClosureLinks.size(),
(int)uMean(odomTime), (int)uMax(odomTime),
(int)uMean(cameraTime), (int)uMax(cameraTime));
(int)uMean(cameraTime),
maxOdomRAM!=-1.0f?uFormat("RAM odom=%dMB ", (int)maxOdomRAM).c_str():"",
(int)maxMapRAM);
if(outputLatex)
{
std::vector<float> stats;
stats.push_back(ids.size());
stats.push_back(bestRMSE);
stats.push_back(maxRMSE);
stats.push_back(bestRMSEAng);
stats.push_back(uMean(odomTime));
stats.push_back(uMean(slamTime));
stats.push_back(uMax(slamTime));
stats.push_back(maxOdomRAM);
stats.push_back(maxMapRAM);
outputLatexStatisticsMap.insert(std::make_pair(filePath, stats));
if(maxOdomRAM != -1.0f)
{
odomRAMSet = true;
}
}
}
driver->closeConnection();
delete driver;
@@ -137,12 +422,112 @@ int main(int argc, char * argv[])
//sub directory
subDirs.push_front(currentPath + UDirectory::separator() + fileName);
}
currentPathIsDatabase = false;
}
for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
{
paths.push_front(*iter);
}
if(outputLatexStatisticsMap.size() && paths.empty())
{
outputLatexStatistics.push_back(outputLatexStatisticsMap);
outputLatexStatisticsMap.clear();
}
}
if(outputLatex && outputLatexStatistics.size())
{
printf("\nLaTeX output:\n----------------\n");
printf("\\begin{table*}[!t]\n");
printf("\\caption{$t_{end}$ is the absolute translational RMSE value at the end "
"of the experiment as $ATE_{max}$ is the maximum during the experiment. "
"$r_{end}$ is rotational RMSE value at the end of the experiment. "
"$o_{avg}$ and $m_{avg}$ are the average computational time "
"for odometry (front-end) and map update (back-end). "
"$m_{avg}$ is the maximum computational time for map update. "
"$O_{end}$ and $M_{end}$ are the RAM usage at the end of the experiment "
"for odometry and map management respectively.}\n");
printf("\\label{}\n");
printf("\\centering\n");
if(odomRAMSet)
{
printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c|c}\n");
printf("\\cline{2-10}\n");
printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $O_{end}$ & $M_{end}$ \\\\\n");
printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) & (MB) \\\\\n");
}
else
{
printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c}\n");
printf("\\cline{2-9}\n");
printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $M_{end}$ \\\\\n");
printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) \\\\\n");
}
printf("\\hline\n");
for(unsigned int j=0; j<outputLatexStatistics.size(); ++j)
{
if(outputLatexStatistics[j].size())
{
std::vector<int> lowestIndex;
if(outputLatexStatistics[j].size() > 1)
{
std::vector<float> lowestValue(outputLatexStatistics[j].begin()->second.size(),-1);
lowestIndex = std::vector<int>(lowestValue.size(),0);
int index = 0;
for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
{
UASSERT(lowestValue.size() == iter->second.size());
for(unsigned int i=0; i<iter->second.size(); ++i)
{
if(lowestValue[i] == -1 || (iter->second[i]>0.0f && lowestValue[i]>iter->second[i]))
{
lowestValue[i] = iter->second[i];
lowestIndex[i] = index;
}
}
++index;
}
}
int index = 0;
for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
{
UASSERT(iter->second.size() == 9);
printf("%s & ", uReplaceChar(iter->first.c_str(), '_', '-').c_str());
printf("%d & ", (int)iter->second[0]);
printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[1]==index?"\\textbf{":"", iter->second[1], lowestIndex.size()&&lowestIndex[1]==index?"}":"");
printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[2]==index?"\\textbf{":"", iter->second[2], lowestIndex.size()&&lowestIndex[2]==index?"}":"");
printf("%s%.2f%s & ", lowestIndex.size()&&lowestIndex[3]==index?"\\textbf{":"", iter->second[3], lowestIndex.size()&&lowestIndex[3]==index?"}":"");
printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[4]==index?"\\textbf{":"", (int)iter->second[4], lowestIndex.size()&&lowestIndex[4]==index?"}":"");
printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[5]==index?"\\textbf{":"", (int)iter->second[5], lowestIndex.size()&&lowestIndex[5]==index?"}":"");
printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[6]==index?"\\textbf{":"", (int)iter->second[6], lowestIndex.size()&&lowestIndex[6]==index?"}":"");
if(odomRAMSet)
{
printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[7]==index?"\\textbf{":"", (int)iter->second[7], lowestIndex.size()&&lowestIndex[7]==index?"}":"");
}
printf("%s%d%s ", lowestIndex.size()&&lowestIndex[8]==index?"\\textbf{":"", (int)iter->second[8], lowestIndex.size()&&lowestIndex[8]==index?"}":"");
printf("\\\\\n");
++index;
}
printf("\\hline\n");
}
}
printf("\\end{tabular}\n");
printf("\\end{table*}\n----------------\n");
}
if(figures.size())
{
for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
{
iter->second->show();
}
return app.exec();
}
return 0;
}

52
tools/Reprocess/CMakeLists.txt Normal file
View File

@@ -0,0 +1,52 @@
SET(RTABMap_INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/utilite/include
${PROJECT_SOURCE_DIR}/corelib/include
)
SET(RTABMap_LIBRARIES
rtabmap_core
rtabmap_utilite
)
if(POLICY CMP0020)
cmake_policy(SET CMP0020 OLD)
endif()
SET(INCLUDE_DIRS
${RTABMap_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
SET(LIBRARIES
${RTABMap_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
)
IF(OCTOMAP_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${OCTOMAP_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${OCTOMAP_LIBRARIES}
)
ENDIF(OCTOMAP_FOUND)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(reprocess main.cpp)
TARGET_LINK_LIBRARIES(reprocess ${LIBRARIES})
SET_TARGET_PROPERTIES( reprocess
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reprocess)
INSTALL(TARGETS reprocess
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

428
tools/Reprocess/main.cpp Normal file
View File

@@ -0,0 +1,428 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/DBReader.h>
#ifdef RTABMAP_OCTOMAP
#include <rtabmap/core/OctoMap.h>
#endif
#include <rtabmap/core/OccupancyGrid.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <pcl/io/pcd_io.h>
using namespace rtabmap;
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
" Options:\n"
" -r Use database stamps as input rate.\n"
" -g2 Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n"
" -g3 Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
" -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
" -o3 Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
"%s\n"
"\n", Parameters::showUsage());
exit(1);
}
class RecoveryProgressState: public ProgressState
{
virtual bool callback(const std::string & msg) const
{
if(!msg.empty())
printf("%s\n", msg.c_str());
return true;
}
};
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError);
if(argc < 3)
{
showUsage();
}
bool assemble2dMap = false;
bool assemble3dMap = false;
bool assemble2dOctoMap = false;
bool assemble3dOctoMap = false;
bool useDatabaseRate = false;
for(int i=1; i<argc-2; ++i)
{
if(strcmp(argv[i], "-r") == 0)
{
useDatabaseRate = true;
printf("Using database stamps as input rate.\n");
}
else if(strcmp(argv[i], "-g2") == 0)
{
assemble2dMap = true;
printf("2D occupancy grid will be assembled (-g2 option).\n");
}
else if(strcmp(argv[i], "-g3") == 0)
{
assemble3dMap = true;
printf("3D cloud map will be assembled (-g3 option).\n");
}
else if(strcmp(argv[i], "-o2") == 0)
{
#ifdef RTABMAP_OCTOMAP
assemble2dOctoMap = true;
printf("OctoMap will be assembled (-o2 option).\n");
#else
printf("RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
#endif
}
else if(strcmp(argv[i], "-o3") == 0)
{
#ifdef RTABMAP_OCTOMAP
assemble3dOctoMap = true;
printf("OctoMap will be assembled (-o3 option).\n");
#else
printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
#endif
}
}
ParametersMap customParameters = Parameters::parseArguments(argc, argv);
std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
std::string outputDatabasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
if(!UFile::exists(inputDatabasePath))
{
printf("Input database \"%s\" doesn't exist!\n", inputDatabasePath.c_str());
return -1;
}
if(UFile::getExtension(inputDatabasePath).compare("db") != 0)
{
printf("File \"%s\" is not a database format (*.db)!\n", inputDatabasePath.c_str());
return -1;
}
if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
{
printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
return -1;
}
if(UFile::exists(outputDatabasePath))
{
UFile::erase(outputDatabasePath);
}
DBDriver * dbDriver = DBDriver::create();
if(!dbDriver->openConnection(inputDatabasePath, false))
{
printf("Failed opening input database!\n");
delete dbDriver;
return -1;
}
ParametersMap parameters = dbDriver->getLastParameters();
if(parameters.empty())
{
printf("Failed getting parameters from database, reprocessing cannot be done. Database version may be too old.\n");
dbDriver->closeConnection(false);
delete dbDriver;
return -1;
}
if(customParameters.size())
{
printf("Custom parameters:\n");
for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
{
printf(" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
}
}
uInsert(parameters, customParameters);
std::set<int> ids;
dbDriver->getAllNodeIds(ids);
if(ids.empty())
{
printf("Input database doesn't have any nodes saved in it.\n");
dbDriver->closeConnection(false);
delete dbDriver;
return -1;
}
dbDriver->closeConnection(false);
delete dbDriver;
Rtabmap rtabmap;
rtabmap.init(parameters, outputDatabasePath);
bool odometryIgnored = false;
Parameters::parse(parameters, Parameters::kRGBDEnabled(), odometryIgnored);
DBReader dbReader(inputDatabasePath, useDatabaseRate?-1:0, !odometryIgnored);
dbReader.init();
OccupancyGrid grid(parameters);
grid.setCloudAssembling(assemble3dMap);
#ifdef RTABMAP_OCTOMAP
OctoMap octomap(parameters);
#endif
printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
std::map<std::string, float> globalMapStats;
int processed = 0;
CameraInfo info;
SensorData data = dbReader.takeImage(&info);
while(data.isValid())
{
UTimer iterationTime;
std::string status;
if(!odometryIgnored && info.odomPose.isNull())
{
printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
}
else
{
if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
{
printf("High variance detected, triggering a new map...\n");
rtabmap.triggerNewMap();
}
if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
{
printf("Failed processing node %d.\n", data.id());
globalMapStats.clear();
}
else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
{
globalMapStats.clear();
double timeUpdateInit = 0.0;
double timeUpdateGrid = 0.0;
double timeUpdateOctoMap = 0.0;
const rtabmap::Statistics & stats = rtabmap.getStatistics();
UTimer t;
if(stats.poses().size() && stats.getSignatures().size())
{
int id = stats.poses().rbegin()->first;
if(stats.getSignatures().find(id)!=stats.getSignatures().end() &&
stats.getSignatures().find(id)->second.sensorData().gridCellSize() > 0.0f)
{
bool updateGridMap = false;
bool updateOctoMap = false;
if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
{
updateGridMap = true;
}
#ifdef RTABMAP_OCTOMAP
if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
{
updateOctoMap = true;
}
#endif
if(updateGridMap || updateOctoMap)
{
cv::Mat ground, obstacles;
stats.getSignatures().find(id)->second.sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
const cv::Point3f & viewpoint = stats.getSignatures().find(id)->second.sensorData().gridViewPoint();
timeUpdateInit = t.ticks();
if(updateGridMap)
{
grid.addToCache(id, ground, obstacles);
grid.update(stats.poses());
timeUpdateGrid = t.ticks() + timeUpdateInit;
}
#ifdef RTABMAP_OCTOMAP
if(updateOctoMap)
{
octomap.addToCache(id, ground, obstacles, viewpoint);
octomap.update(stats.poses());
timeUpdateOctoMap = t.ticks() + timeUpdateInit;
}
#endif
}
}
}
//Simulate publishing
double timePub2dOctoMap = 0.0;
double timePub3dOctoMap = 0.0;
if(assemble2dOctoMap)
{
float xMin, yMin, size;
octomap.createProjectionMap(xMin, yMin, size);
timePub2dOctoMap = t.ticks();
}
if(assemble3dOctoMap)
{
octomap.createCloud();
timePub3dOctoMap = t.ticks();
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
}
}
printf("Processed %d/%d nodes... %dms\n", ++processed, (int)ids.size(), int(iterationTime.ticks()*1000));
data = dbReader.takeImage(&info);
}
printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
rtabmap.close(true);
printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
if(assemble2dMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
float xMin,yMin;
cv::Mat map = grid.getMap(xMin, yMin);
if(!map.empty())
{
cv::Mat map8U(map.rows, map.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < map.rows; ++i)
{
for (int j = 0; j < map.cols; ++j)
{
char v = map.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}
if(cv::imwrite(outputPath, map8U))
{
printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
}
}
else
{
printf("2D map is empty! Cannot save it!\n");
}
}
if(assemble3dMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pcd";
if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
{
printf("Saving 3d cloud map \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving 3d cloud map \"%s\"... failed!\n", outputPath.c_str());
}
}
#ifdef RTABMAP_OCTOMAP
if(assemble2dOctoMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pgm";
float xMin,yMin,cellSize;
cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
if(!map.empty())
{
cv::Mat map8U(map.rows, map.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < map.rows; ++i)
{
for (int j = 0; j < map.cols; ++j)
{
char v = map.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}
if(cv::imwrite(outputPath, map8U))
{
printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
}
}
else
{
printf("OctoMap 2D projection map is empty! Cannot save it!\n");
}
}
if(assemble3dOctoMap)
{
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pcd";
std::vector<int> obstacles;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles);
if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
{
printf("Saving octomap cloud \"%s\"... done!\n", outputPath.c_str());
}
else
{
printf("Saving octomap cloud \"%s\"... failed!\n", outputPath.c_str());
}
}
#endif
return 0;
}

4
tools/RgbdDataset/CMakeLists.txt
View File

@@ -35,3 +35,7 @@ TARGET_LINK_LIBRARIES(rgbd_dataset ${LIBRARIES})
SET_TARGET_PROPERTIES( rgbd_dataset
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_dataset)
INSTALL(TARGETS rgbd_dataset
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

107
tools/RgbdDataset/main.cpp
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>
@@ -54,7 +55,8 @@ void showUsage()
" synchronized images using associate.py tool (use tool version from\n"
" https://gist.github.com/matlabbe/484134a2d9da8ad425362c6669824798). If \n"
" \"groundtruth.txt\" is found in the sequence folder, they will be saved in the database.\n"
" --output Output directory. By default, results are saved in \"path\".\n\n"
" --output Output directory. By default, results are saved in \"path\".\n"
" --output_name Output database name (default \"rtabmap\").\n"
" --quiet Don't show log messages and iteration updates.\n"
"%s\n"
"Example:\n\n"
@@ -97,6 +99,7 @@ int main(int argc, char * argv[])
ParametersMap parameters;
std::string path;
std::string output;
std::string outputName = "rtabmap";
bool quiet = false;
if(argc < 2)
{
@@ -110,6 +113,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;
@@ -128,6 +135,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"));
}
std::string seq = uSplit(path, '/').back();
@@ -150,12 +159,14 @@ int main(int argc, char * argv[])
" Dataset path: %s\n"
" RGB path: %s\n"
" Depth path: %s\n"
" Output: %s\n",
" Output: %s\n"
" Output name: %s\n",
seq.c_str(),
path.c_str(),
pathRgbImages.c_str(),
pathDepthImages.c_str(),
output.c_str());
output.c_str(),
outputName.c_str());
if(!pathGt.empty())
{
printf(" groundtruth.txt: %s\n", pathGt.c_str());
@@ -177,16 +188,15 @@ int main(int argc, char * argv[])
float depthFactor = 5.0f;
if(sequenceName.find("freiburg1") != std::string::npos)
{
model = CameraModel("rtabmap_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
model = CameraModel(outputName+"_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
}
else if(sequenceName.find("freiburg2") != std::string::npos)
{
model = CameraModel("rtabmap_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
depthFactor = 5.208f; // based on TUM2.yaml ORB_SLAM2 file
model = CameraModel(outputName+"_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
}
else //if(sequenceName.find("freiburg3") != std::string::npos)
{
model = CameraModel("rtabmap_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
model = CameraModel(outputName+"_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
}
//parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx())));
model.save(path);
@@ -206,17 +216,20 @@ int main(int argc, char * argv[])
bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
float detectionRate = Parameters::defaultRtabmapDetectionRate();
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
int odomStrategy = Parameters::defaultOdomStrategy();
Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
std::string databasePath = output+"/"+seq+".db";
std::string databasePath = output+"/"+outputName+".db";
UFile::erase(databasePath);
if(cameraThread.camera()->init(path, "rtabmap_calib"))
if(cameraThread.camera()->init(path, outputName+"_calib"))
{
int totalImages = (int)((CameraRGBDImages*)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);
@@ -234,34 +247,21 @@ int main(int argc, char * argv[])
double previousStamp = 0.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));
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(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);
}
}
if(odomInfo.keyFrameAdded)
{
++odomKeyFrames;
@@ -295,6 +295,31 @@ 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/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();
@@ -313,8 +338,10 @@ int main(int argc, char * argv[])
if(rmse >= 0.0f)
{
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)));
//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)));
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
}
else
{
@@ -354,7 +381,7 @@ int main(int argc, char * argv[])
{
stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
}
std::string pathTrajectory = output+"/"+seq+"_poses.txt";
std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
{
printf("Saving %s... done!\n", pathTrajectory.c_str());
@@ -418,7 +445,7 @@ int main(int argc, char * argv[])
printf(" rotational_rmse= %f deg\n", rotational_rmse);
FILE * pFile = 0;
std::string pathErrors = output+"/"+seq+"_rmse.txt";
std::string pathErrors = output+"/"+outputName+"_rmse.txt";
pFile = fopen(pathErrors.c_str(),"w");
if(!pFile)
{
@@ -445,9 +472,9 @@ int main(int argc, char * argv[])
UERROR("Camera init failed!");
}
printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+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+"/"+seq+".db").c_str());
" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
return 0;
}