mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Update 0.15.4.
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
428
tools/Reprocess/main.cpp
Normal file
428
tools/Reprocess/main.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user