mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
0.15.1: CameraImages: added ground truth time diff, fixed memory leak when loading binary scans. CameraRGBDImages and CameraStereoImages: fixed start id. Feature2D: added grid rows and cols parameters (Kp/GridRows, Kp/GridCols, Vis/GridRows, Vis/GridCols). OdomInfo: publish bundle frames. OdometryORBSLAM2: added OdomORBSLAM2/Fps and OdomORBSLAM2/MaxFeatures parameters. Registration: added Reg/RepeatOnce parameter and removed variance normalization. For util2d::getDepth() and util3d::projectDepthTo3D(), maxZError parameter is now depthErrorRatio to be dependent of the sensor range. Database: save image width and height from stereo calibration. OptimizerG2O: fixed SBA optimization when using g2o built from ORBSLAM2 library. OptimizerGTSAM: to increase optimization stability, all rotations in information matrix are divided by 100000. Added rtabmap-report tool.
This commit is contained in:
@@ -8,6 +8,7 @@ ADD_SUBDIRECTORY( StereoEval )
|
||||
ADD_SUBDIRECTORY( KittiDataset )
|
||||
ADD_SUBDIRECTORY( RgbdDataset )
|
||||
ADD_SUBDIRECTORY( Recovery )
|
||||
ADD_SUBDIRECTORY( Report )
|
||||
|
||||
IF(OPENCV_NONFREE_FOUND)
|
||||
ADD_SUBDIRECTORY( VocabularyComparison )
|
||||
|
||||
@@ -60,19 +60,31 @@ void showUsage()
|
||||
" --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 20).\n"
|
||||
" --scan_k Scan normal K (default 5).\n"
|
||||
" --scan_radius Scan normal radius (default 0).\n"
|
||||
" --map_update # Do map update each X odometry frames (default=10, which\n"
|
||||
" gives 1 Hz map update assuming images are at 10 Hz).\n\n"
|
||||
"%s\n"
|
||||
"Example:\n\n"
|
||||
" $ rtabmap-kitti_dataset \\\n"
|
||||
" --Vis/EstimationType 1\\\n"
|
||||
" --Vis/BundleAdjustment 1\\\n"
|
||||
" --Vis/PnPReprojError 1.5\\\n"
|
||||
" --Vis/PnPRefineIterations 0\\\n"
|
||||
" --Vis/MaxFeatures 1800\\\n"
|
||||
" --Vis/BundleAdjustment 1\\\n"
|
||||
" --Vis/Iterations 300\\\n"
|
||||
" --GFTT/QualityLevel 0.01\\\n"
|
||||
" --GFTT/MinDistance 7\\\n"
|
||||
" --Odom/GuessMotion true\\\n"
|
||||
" --OdomF2M/BundleAdjustment 1\\\n"
|
||||
" --Mem/UseOdomFeatures true\\\n"
|
||||
" --Kp/DetectorStrategy true\\\n"
|
||||
" --Kp/MaxFeatures 900\\\n"
|
||||
" --Rtabmap/DetectionRate 2\\\n"
|
||||
" --Rtabmap/CreateIntermediateNodes true\\\n"
|
||||
" --RGBD/ProximityBySpace false\\\n"
|
||||
" --Stereo/MaxLevel 5\\\n"
|
||||
" --Stereo/MaxDisparity 256\\\n"
|
||||
" --Stereo/MinDisparity 0.5\\\n"
|
||||
" --gt \"~/KITTI/devkit/cpp/data/odometry/poses/07.txt\"\\\n"
|
||||
" ~/KITTI/dataset/sequences/07\n\n", rtabmap::Parameters::showUsage());
|
||||
exit(1);
|
||||
@@ -105,7 +117,7 @@ int main(int argc, char * argv[])
|
||||
bool disp = false;
|
||||
int scanStep = 10;
|
||||
float scanVoxel = 0.3f;
|
||||
int scanNormalK = 20;
|
||||
int scanNormalK = 5;
|
||||
float scanNormalRadius = 0.0f;
|
||||
std::string gtPath;
|
||||
bool quiet = false;
|
||||
@@ -377,7 +389,7 @@ int main(int argc, char * argv[])
|
||||
|
||||
printf("Processing %d images...\n", totalImages);
|
||||
|
||||
OdometryF2M odom(parameters);
|
||||
Odometry * odom = Odometry::create(parameters);
|
||||
Rtabmap rtabmap;
|
||||
rtabmap.init(parameters, databasePath);
|
||||
|
||||
@@ -408,7 +420,7 @@ int main(int argc, char * argv[])
|
||||
externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
|
||||
|
||||
OdometryInfo odomInfo;
|
||||
Transform pose = odom.process(data, &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));
|
||||
@@ -433,14 +445,10 @@ int main(int argc, char * argv[])
|
||||
data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
|
||||
processData = intermediateNodes;
|
||||
}
|
||||
if(covariance.empty())
|
||||
if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
|
||||
{
|
||||
covariance = odomInfo.reg.covariance;
|
||||
}
|
||||
else
|
||||
{
|
||||
covariance += odomInfo.reg.covariance;
|
||||
}
|
||||
|
||||
timer.restart();
|
||||
if(processData)
|
||||
@@ -463,8 +471,8 @@ int main(int argc, char * argv[])
|
||||
|
||||
if(rmse >= 0.0f)
|
||||
{
|
||||
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm",
|
||||
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
|
||||
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm, stddev=%fm %frad",
|
||||
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -487,13 +495,14 @@ int main(int argc, char * argv[])
|
||||
timer.restart();
|
||||
data = cameraThread.camera()->takeImage(&cameraInfo);
|
||||
}
|
||||
delete odom;
|
||||
printf("Total time=%fs\n", totalTime.ticks());
|
||||
/////////////////////////////
|
||||
// Processing dataset end
|
||||
/////////////////////////////
|
||||
|
||||
// Save trajectory
|
||||
printf("Saving rtabmap_trajectory.txt ...\n");
|
||||
printf("Saving trajectory ...\n");
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, Link> links;
|
||||
rtabmap.getGraph(poses, links, true, true);
|
||||
@@ -575,6 +584,8 @@ int main(int argc, char * argv[])
|
||||
UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
|
||||
}
|
||||
fprintf(pFile, "Ground truth comparison:\n");
|
||||
fprintf(pFile, " KITTI t_err = %f %%\n", t_err);
|
||||
fprintf(pFile, " KITTI r_err = %f deg/m\n", r_err);
|
||||
fprintf(pFile, " translational_rmse= %f\n", translational_rmse);
|
||||
fprintf(pFile, " translational_mean= %f\n", translational_mean);
|
||||
fprintf(pFile, " translational_median= %f\n", translational_median);
|
||||
|
||||
36
tools/Report/CMakeLists.txt
Normal file
36
tools/Report/CMakeLists.txt
Normal file
@@ -0,0 +1,36 @@
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
# 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}
|
||||
)
|
||||
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
ADD_EXECUTABLE(report main.cpp)
|
||||
|
||||
TARGET_LINK_LIBRARIES(report ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( report
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-report)
|
||||
142
tools/Report/main.cpp
Normal file
142
tools/Report/main.cpp
Normal file
@@ -0,0 +1,142 @@
|
||||
/*
|
||||
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/DBDriver.h>
|
||||
#include <rtabmap/utilite/UDirectory.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UMath.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
void showUsage()
|
||||
{
|
||||
printf("\nUsage:\n"
|
||||
"rtabmap-report path\n"
|
||||
" path Directory containing rtabmap databases.\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
if(argc < 2)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
|
||||
std::string path = argv[1];
|
||||
path = uReplaceChar(path, '~', UDirectory::homeDir());
|
||||
|
||||
std::string fileName;
|
||||
std::list<std::string> paths;
|
||||
paths.push_back(path);
|
||||
while(paths.size())
|
||||
{
|
||||
std::string currentPath = paths.front();
|
||||
UDirectory currentDir(currentPath);
|
||||
paths.pop_front();
|
||||
if(!currentDir.isValid())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
std::list<std::string> subDirs;
|
||||
printf("Directory: %s\n", currentPath.c_str());
|
||||
while(!(fileName = currentDir.getNextFileName()).empty())
|
||||
{
|
||||
if(UFile::getExtension(fileName).compare("db") == 0)
|
||||
{
|
||||
std::string filePath = currentPath + UDirectory::separator() + fileName;
|
||||
DBDriver * driver = DBDriver::create();
|
||||
if(driver->openConnection(filePath))
|
||||
{
|
||||
std::set<int> ids;
|
||||
driver->getAllNodeIds(ids);
|
||||
std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
|
||||
std::vector<float> cameraTime;
|
||||
cameraTime.reserve(ids.size());
|
||||
std::vector<float> odomTime;
|
||||
odomTime.reserve(ids.size());
|
||||
std::vector<float> slamTime;
|
||||
slamTime.reserve(ids.size());
|
||||
float rmse = -1;
|
||||
for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
|
||||
{
|
||||
Transform p, gt;
|
||||
GPS gps;
|
||||
int m, w;
|
||||
std::string l;
|
||||
double s;
|
||||
std::vector<float> v;
|
||||
driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps);
|
||||
|
||||
if(uContains(stats, *iter))
|
||||
{
|
||||
const std::map<std::string, float> & stat = stats.at(*iter).first;
|
||||
if(uContains(stat, Statistics::kGtTranslational_rmse()))
|
||||
{
|
||||
rmse = stat.at(Statistics::kGtTranslational_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(w >= 0 && uContains(stat, Statistics::kTimingTotal()))
|
||||
{
|
||||
slamTime.push_back(stat.at(Statistics::kTimingTotal()));
|
||||
}
|
||||
}
|
||||
}
|
||||
printf(" %s (%d): %fm, slam: avg=%dms max=%dms, odom: avg=%dms max=%dms, camera: avg=%dms max=%dms\n",
|
||||
fileName.c_str(),
|
||||
(int)ids.size(),
|
||||
rmse,
|
||||
(int)uMean(slamTime), (int)uMax(slamTime),
|
||||
(int)uMean(odomTime), (int)uMax(odomTime),
|
||||
(int)uMean(cameraTime), (int)uMax(cameraTime));
|
||||
}
|
||||
driver->closeConnection();
|
||||
delete driver;
|
||||
}
|
||||
else if(uSplit(fileName, '.').size() == 1)
|
||||
{
|
||||
//sub directory
|
||||
subDirs.push_front(currentPath + UDirectory::separator() + fileName);
|
||||
}
|
||||
}
|
||||
for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
|
||||
{
|
||||
paths.push_front(*iter);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -59,13 +59,20 @@ void showUsage()
|
||||
"%s\n"
|
||||
"Example:\n\n"
|
||||
" $ rtabmap-rgbd_dataset \\\n"
|
||||
" --Vis/EstimationType 1\\\n"
|
||||
" --Vis/BundleAdjustment 1\\\n"
|
||||
" --Vis/PnPReprojError 1.5\\\n"
|
||||
" --Vis/PnPRefineIterations 0\\\n"
|
||||
" --Vis/BundleAdjustment 1\\\n"
|
||||
" --Vis/Iterations 300\\\n"
|
||||
" --GFTT/QualityLevel 0.001\\\n"
|
||||
" --GFTT/MinDistance 3\\\n"
|
||||
" --Odom/GuessMotion true\\\n"
|
||||
" --OdomF2M/BundleAdjustment 1\\\n"
|
||||
" --Mem/UseOdomFeatures true\\\n"
|
||||
" --Kp/DetectorStrategy true\\\n"
|
||||
" --Kp/MaxFeatures 600\\\n"
|
||||
" --Rtabmap/DetectionRate 4\\\n"
|
||||
" --Rtabmap/CreateIntermediateNodes true\\\n"
|
||||
" --Rtabmap/DetectionRate 1\\\n"
|
||||
" --RGBD/ProximityBySpace false\\\n"
|
||||
" ~/rgbd_dataset_freiburg3_long_office_household\n\n", rtabmap::Parameters::showUsage());
|
||||
exit(1);
|
||||
}
|
||||
@@ -123,6 +130,7 @@ int main(int argc, char * argv[])
|
||||
}
|
||||
}
|
||||
|
||||
std::string seq = uSplit(path, '/').back();
|
||||
std::string pathRgbImages = path+"/rgb_sync";
|
||||
std::string pathDepthImages = path+"/depth_sync";
|
||||
std::string pathGt = path+"/groundtruth.txt";
|
||||
@@ -138,10 +146,12 @@ int main(int argc, char * argv[])
|
||||
}
|
||||
|
||||
printf("Paths:\n"
|
||||
" Dataset name: %s\n"
|
||||
" Dataset path: %s\n"
|
||||
" RGB path: %s\n"
|
||||
" Depth path: %s\n"
|
||||
" Output: %s\n",
|
||||
seq.c_str(),
|
||||
path.c_str(),
|
||||
pathRgbImages.c_str(),
|
||||
pathDepthImages.c_str(),
|
||||
@@ -172,13 +182,14 @@ int main(int argc, char * argv[])
|
||||
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;
|
||||
depthFactor = 5.208f; // based on TUM2.yaml ORB_SLAM2 file
|
||||
}
|
||||
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.save(output);
|
||||
//parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx())));
|
||||
model.save(path);
|
||||
|
||||
CameraThread cameraThread(new
|
||||
CameraRGBDImages(
|
||||
@@ -197,15 +208,15 @@ int main(int argc, char * argv[])
|
||||
float detectionRate = Parameters::defaultRtabmapDetectionRate();
|
||||
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
|
||||
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
|
||||
std::string databasePath = output+"/rtabmap.db";
|
||||
std::string databasePath = output+"/"+seq+".db";
|
||||
UFile::erase(databasePath);
|
||||
if(cameraThread.camera()->init(output, "rtabmap_calib"))
|
||||
if(cameraThread.camera()->init(path, "rtabmap_calib"))
|
||||
{
|
||||
int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
|
||||
|
||||
printf("Processing %d images...\n", totalImages);
|
||||
|
||||
OdometryF2M odom(parameters);
|
||||
Odometry * odom = Odometry::create(parameters);
|
||||
Rtabmap rtabmap;
|
||||
rtabmap.init(parameters, databasePath);
|
||||
|
||||
@@ -237,11 +248,18 @@ int main(int argc, char * argv[])
|
||||
externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
|
||||
|
||||
OdometryInfo odomInfo;
|
||||
Transform pose = odom.process(data, &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/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));
|
||||
|
||||
bool processData = true;
|
||||
if(detectionRate>0.0f &&
|
||||
@@ -263,14 +281,10 @@ int main(int argc, char * argv[])
|
||||
data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
|
||||
processData = intermediateNodes;
|
||||
}
|
||||
if(covariance.empty())
|
||||
if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
|
||||
{
|
||||
covariance = odomInfo.reg.covariance;
|
||||
}
|
||||
else
|
||||
{
|
||||
covariance += odomInfo.reg.covariance;
|
||||
}
|
||||
|
||||
timer.restart();
|
||||
if(processData)
|
||||
@@ -293,8 +307,8 @@ int main(int argc, char * argv[])
|
||||
|
||||
if(rmse >= 0.0f)
|
||||
{
|
||||
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm",
|
||||
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
|
||||
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm, stddev=%fm %frad",
|
||||
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -317,18 +331,25 @@ int main(int argc, char * argv[])
|
||||
timer.restart();
|
||||
data = cameraThread.camera()->takeImage(&cameraInfo);
|
||||
}
|
||||
delete odom;
|
||||
printf("Total time=%fs\n", totalTime.ticks());
|
||||
/////////////////////////////
|
||||
// Processing dataset end
|
||||
/////////////////////////////
|
||||
|
||||
// Save trajectory
|
||||
printf("Saving rtabmap_trajectory.txt ...\n");
|
||||
printf("Saving trajectory...\n");
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, Link> links;
|
||||
rtabmap.getGraph(poses, links, true, true);
|
||||
std::string pathTrajectory = output+"/rtabmap_poses.txt";
|
||||
if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, 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+"/"+seq+"_poses.txt";
|
||||
if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
|
||||
{
|
||||
printf("Saving %s... done!\n", pathTrajectory.c_str());
|
||||
}
|
||||
@@ -357,14 +378,7 @@ int main(int argc, char * argv[])
|
||||
}
|
||||
}
|
||||
|
||||
// compute KITTI statistics
|
||||
float t_err = 0.0f;
|
||||
float r_err = 0.0f;
|
||||
graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
|
||||
printf("Ground truth comparison:\n");
|
||||
printf(" KITTI t_err = %f %%\n", t_err);
|
||||
printf(" KITTI r_err = %f deg/m\n", r_err);
|
||||
|
||||
|
||||
// compute RMSE statistics
|
||||
float translational_rmse = 0.0f;
|
||||
float translational_mean = 0.0f;
|
||||
@@ -398,7 +412,7 @@ int main(int argc, char * argv[])
|
||||
printf(" rotational_rmse= %f deg\n", rotational_rmse);
|
||||
|
||||
FILE * pFile = 0;
|
||||
std::string pathErrors = output+"/rtabmap_rmse.txt";
|
||||
std::string pathErrors = output+"/"+seq+"_rmse.txt";
|
||||
pFile = fopen(pathErrors.c_str(),"w");
|
||||
if(!pFile)
|
||||
{
|
||||
@@ -425,9 +439,9 @@ int main(int argc, char * argv[])
|
||||
UERROR("Camera init failed!");
|
||||
}
|
||||
|
||||
printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/rtabmap.db").c_str());
|
||||
printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+seq+".db").c_str());
|
||||
printf("Do:\n"
|
||||
" $ rtabmap-databaseViewer %s\n\n", (output+"/rtabmap.db").c_str());
|
||||
" $ rtabmap-databaseViewer %s\n\n", (output+"/"+seq+".db").c_str());
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user