00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/Rtabmap.h>
00029 #include <rtabmap/core/DBDriver.h>
00030 #include <rtabmap/core/DBReader.h>
00031 #ifdef RTABMAP_OCTOMAP
00032 #include <rtabmap/core/OctoMap.h>
00033 #endif
00034 #include <rtabmap/core/OccupancyGrid.h>
00035 #include <rtabmap/utilite/UFile.h>
00036 #include <rtabmap/utilite/UDirectory.h>
00037 #include <rtabmap/utilite/UTimer.h>
00038 #include <rtabmap/utilite/UStl.h>
00039 #include <stdio.h>
00040 #include <string.h>
00041 #include <stdlib.h>
00042 #include <pcl/io/pcd_io.h>
00043 #include <signal.h>
00044
00045 using namespace rtabmap;
00046
00047 void showUsage()
00048 {
00049 printf("\nUsage:\n"
00050 "rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
00051 " Options:\n"
00052 " -r Use database stamps as input rate.\n"
00053 " -g2 Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n"
00054 " -g3 Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
00055 " -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
00056 " -o3 Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
00057 "%s\n"
00058 "\n", Parameters::showUsage());
00059 exit(1);
00060 }
00061
00062
00063 bool g_loopForever = true;
00064 void sighandler(int sig)
00065 {
00066 printf("\nSignal %d caught...\n", sig);
00067 g_loopForever = false;
00068 }
00069
00070 int main(int argc, char * argv[])
00071 {
00072 signal(SIGABRT, &sighandler);
00073 signal(SIGTERM, &sighandler);
00074 signal(SIGINT, &sighandler);
00075
00076 ULogger::setType(ULogger::kTypeConsole);
00077 ULogger::setLevel(ULogger::kError);
00078
00079 if(argc < 3)
00080 {
00081 showUsage();
00082 }
00083
00084 bool assemble2dMap = false;
00085 bool assemble3dMap = false;
00086 bool assemble2dOctoMap = false;
00087 bool assemble3dOctoMap = false;
00088 bool useDatabaseRate = false;
00089 for(int i=1; i<argc-2; ++i)
00090 {
00091 if(strcmp(argv[i], "-r") == 0)
00092 {
00093 useDatabaseRate = true;
00094 printf("Using database stamps as input rate.\n");
00095 }
00096 else if(strcmp(argv[i], "-g2") == 0)
00097 {
00098 assemble2dMap = true;
00099 printf("2D occupancy grid will be assembled (-g2 option).\n");
00100 }
00101 else if(strcmp(argv[i], "-g3") == 0)
00102 {
00103 assemble3dMap = true;
00104 printf("3D cloud map will be assembled (-g3 option).\n");
00105 }
00106 else if(strcmp(argv[i], "-o2") == 0)
00107 {
00108 #ifdef RTABMAP_OCTOMAP
00109 assemble2dOctoMap = true;
00110 printf("OctoMap will be assembled (-o2 option).\n");
00111 #else
00112 printf("RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
00113 #endif
00114 }
00115 else if(strcmp(argv[i], "-o3") == 0)
00116 {
00117 #ifdef RTABMAP_OCTOMAP
00118 assemble3dOctoMap = true;
00119 printf("OctoMap will be assembled (-o3 option).\n");
00120 #else
00121 printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
00122 #endif
00123 }
00124 }
00125
00126 ParametersMap customParameters = Parameters::parseArguments(argc, argv);
00127
00128 std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
00129 std::string outputDatabasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
00130
00131 if(!UFile::exists(inputDatabasePath))
00132 {
00133 printf("Input database \"%s\" doesn't exist!\n", inputDatabasePath.c_str());
00134 return -1;
00135 }
00136
00137 if(UFile::getExtension(inputDatabasePath).compare("db") != 0)
00138 {
00139 printf("File \"%s\" is not a database format (*.db)!\n", inputDatabasePath.c_str());
00140 return -1;
00141 }
00142 if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
00143 {
00144 printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
00145 return -1;
00146 }
00147
00148 if(UFile::exists(outputDatabasePath))
00149 {
00150 UFile::erase(outputDatabasePath);
00151 }
00152
00153 DBDriver * dbDriver = DBDriver::create();
00154 if(!dbDriver->openConnection(inputDatabasePath, false))
00155 {
00156 printf("Failed opening input database!\n");
00157 delete dbDriver;
00158 return -1;
00159 }
00160
00161 ParametersMap parameters = dbDriver->getLastParameters();
00162 if(parameters.empty())
00163 {
00164 printf("Failed getting parameters from database, reprocessing cannot be done. Database version may be too old.\n");
00165 dbDriver->closeConnection(false);
00166 delete dbDriver;
00167 return -1;
00168 }
00169 if(customParameters.size())
00170 {
00171 printf("Custom parameters:\n");
00172 for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
00173 {
00174 printf(" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
00175 }
00176 }
00177 uInsert(parameters, customParameters);
00178 std::set<int> ids;
00179 dbDriver->getAllNodeIds(ids);
00180 if(ids.empty())
00181 {
00182 printf("Input database doesn't have any nodes saved in it.\n");
00183 dbDriver->closeConnection(false);
00184 delete dbDriver;
00185 return -1;
00186 }
00187
00188 dbDriver->closeConnection(false);
00189 delete dbDriver;
00190
00191 Rtabmap rtabmap;
00192 rtabmap.init(parameters, outputDatabasePath);
00193
00194 bool rgbdEnabled = Parameters::defaultRGBDEnabled();
00195 Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
00196 bool odometryIgnored = !rgbdEnabled;
00197 DBReader dbReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored);
00198 dbReader.init();
00199
00200 OccupancyGrid grid(parameters);
00201 grid.setCloudAssembling(assemble3dMap);
00202 #ifdef RTABMAP_OCTOMAP
00203 OctoMap octomap(parameters);
00204 #endif
00205
00206 printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
00207 std::map<std::string, float> globalMapStats;
00208 int processed = 0;
00209 CameraInfo info;
00210 SensorData data = dbReader.takeImage(&info);
00211 while(data.isValid() && g_loopForever)
00212 {
00213 UTimer iterationTime;
00214 std::string status;
00215 if(!odometryIgnored && info.odomPose.isNull())
00216 {
00217 printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
00218 }
00219 else
00220 {
00221 if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
00222 {
00223 printf("High variance detected, triggering a new map...\n");
00224 rtabmap.triggerNewMap();
00225 }
00226 UTimer t;
00227 if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
00228 {
00229 printf("Failed processing node %d.\n", data.id());
00230 globalMapStats.clear();
00231 }
00232 else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
00233 {
00234 globalMapStats.clear();
00235 double timeRtabmap = t.ticks();
00236 double timeUpdateInit = 0.0;
00237 double timeUpdateGrid = 0.0;
00238 #ifdef RTABMAP_OCTOMAP
00239 double timeUpdateOctoMap = 0.0;
00240 #endif
00241 const rtabmap::Statistics & stats = rtabmap.getStatistics();
00242 if(stats.poses().size() && stats.getSignatures().size())
00243 {
00244 int id = stats.poses().rbegin()->first;
00245 if(stats.getSignatures().find(id)!=stats.getSignatures().end() &&
00246 stats.getSignatures().find(id)->second.sensorData().gridCellSize() > 0.0f)
00247 {
00248 bool updateGridMap = false;
00249 bool updateOctoMap = false;
00250 if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
00251 {
00252 updateGridMap = true;
00253 }
00254 #ifdef RTABMAP_OCTOMAP
00255 if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
00256 {
00257 updateOctoMap = true;
00258 }
00259 #endif
00260 if(updateGridMap || updateOctoMap)
00261 {
00262 cv::Mat ground, obstacles, empty;
00263 stats.getSignatures().find(id)->second.sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
00264
00265 timeUpdateInit = t.ticks();
00266
00267 if(updateGridMap)
00268 {
00269 grid.addToCache(id, ground, obstacles, empty);
00270 grid.update(stats.poses());
00271 timeUpdateGrid = t.ticks() + timeUpdateInit;
00272 }
00273 #ifdef RTABMAP_OCTOMAP
00274 if(updateOctoMap)
00275 {
00276 const cv::Point3f & viewpoint = stats.getSignatures().find(id)->second.sensorData().gridViewPoint();
00277 octomap.addToCache(id, ground, obstacles, empty, viewpoint);
00278 octomap.update(stats.poses());
00279 timeUpdateOctoMap = t.ticks() + timeUpdateInit;
00280 }
00281 #endif
00282 }
00283 }
00284 }
00285
00286 globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
00287 #ifdef RTABMAP_OCTOMAP
00288
00289 double timePub2dOctoMap = 0.0;
00290 double timePub3dOctoMap = 0.0;
00291 if(assemble2dOctoMap)
00292 {
00293 float xMin, yMin, size;
00294 octomap.createProjectionMap(xMin, yMin, size);
00295 timePub2dOctoMap = t.ticks();
00296 }
00297 if(assemble3dOctoMap)
00298 {
00299 octomap.createCloud();
00300 timePub3dOctoMap = t.ticks();
00301 }
00302
00303 globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
00304 globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
00305 globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
00306 globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
00307 #else
00308 globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
00309 #endif
00310 }
00311 }
00312
00313 printf("Processed %d/%d nodes... %dms\n", ++processed, (int)ids.size(), int(iterationTime.ticks()*1000));
00314
00315 data = dbReader.takeImage(&info);
00316 }
00317
00318 printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
00319 rtabmap.close(true);
00320 printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
00321
00322 if(assemble2dMap)
00323 {
00324 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
00325 float xMin,yMin;
00326 cv::Mat map = grid.getMap(xMin, yMin);
00327 if(!map.empty())
00328 {
00329 cv::Mat map8U(map.rows, map.cols, CV_8U);
00330
00331 for (int i = 0; i < map.rows; ++i)
00332 {
00333 for (int j = 0; j < map.cols; ++j)
00334 {
00335 char v = map.at<char>(i, j);
00336 unsigned char gray;
00337 if(v == 0)
00338 {
00339 gray = 178;
00340 }
00341 else if(v == 100)
00342 {
00343 gray = 0;
00344 }
00345 else
00346 {
00347 gray = 89;
00348 }
00349 map8U.at<unsigned char>(i, j) = gray;
00350 }
00351 }
00352 if(cv::imwrite(outputPath, map8U))
00353 {
00354 printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
00355 }
00356 else
00357 {
00358 printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
00359 }
00360 }
00361 else
00362 {
00363 printf("2D map is empty! Cannot save it!\n");
00364 }
00365 }
00366 if(assemble3dMap)
00367 {
00368 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_obstacles.pcd";
00369 if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
00370 {
00371 printf("Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str());
00372 }
00373 else
00374 {
00375 printf("Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str());
00376 }
00377 if(grid.getMapGround()->size())
00378 {
00379 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_ground.pcd";
00380 if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapGround()) == 0)
00381 {
00382 printf("Saving 3d ground \"%s\"... done!\n", outputPath.c_str());
00383 }
00384 else
00385 {
00386 printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
00387 }
00388 }
00389 if(grid.getMapEmptyCells()->size())
00390 {
00391 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_empty.pcd";
00392 if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapEmptyCells()) == 0)
00393 {
00394 printf("Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str());
00395 }
00396 else
00397 {
00398 printf("Saving 3d empty cells \"%s\"... failed!\n", outputPath.c_str());
00399 }
00400 }
00401 }
00402 #ifdef RTABMAP_OCTOMAP
00403 if(assemble2dOctoMap)
00404 {
00405 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pgm";
00406 float xMin,yMin,cellSize;
00407 cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
00408 if(!map.empty())
00409 {
00410 cv::Mat map8U(map.rows, map.cols, CV_8U);
00411
00412 for (int i = 0; i < map.rows; ++i)
00413 {
00414 for (int j = 0; j < map.cols; ++j)
00415 {
00416 char v = map.at<char>(i, j);
00417 unsigned char gray;
00418 if(v == 0)
00419 {
00420 gray = 178;
00421 }
00422 else if(v == 100)
00423 {
00424 gray = 0;
00425 }
00426 else
00427 {
00428 gray = 89;
00429 }
00430 map8U.at<unsigned char>(i, j) = gray;
00431 }
00432 }
00433 if(cv::imwrite(outputPath, map8U))
00434 {
00435 printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
00436 }
00437 else
00438 {
00439 printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
00440 }
00441 }
00442 else
00443 {
00444 printf("OctoMap 2D projection map is empty! Cannot save it!\n");
00445 }
00446 }
00447 if(assemble3dOctoMap)
00448 {
00449 std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
00450 std::vector<int> obstacles, emptySpace, ground;
00451 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace, &ground);
00452 if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
00453 {
00454 printf("Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
00455 }
00456 else
00457 {
00458 printf("Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
00459 }
00460 if(ground.size())
00461 {
00462 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_ground.pcd";
00463 if(pcl::io::savePCDFile(outputPath, *cloud, ground, true) == 0)
00464 {
00465 printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
00466 }
00467 else
00468 {
00469 printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
00470 }
00471 }
00472 if(emptySpace.size())
00473 {
00474 outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_empty.pcd";
00475 if(pcl::io::savePCDFile(outputPath, *cloud, emptySpace, true) == 0)
00476 {
00477 printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
00478 }
00479 else
00480 {
00481 printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
00482 }
00483 }
00484 }
00485 #endif
00486
00487 return 0;
00488 }