41 #include <pcl/filters/filter.h>
42 #include <pcl/io/ply_io.h>
43 #include <pcl/io/obj_io.h>
44 #include <pcl/common/common.h>
45 #include <pcl/surface/poisson.h>
57 "rtabmap-export [options] database.db\n"
59 " --output \"\" Output name (default: name of the database is used).\n"
60 " --output_dir \"\" Output directory (default: same directory than the database).\n"
61 " --ascii Export PLY in ascii format.\n"
62 " --las Export cloud in LAS instead of PLY (PDAL dependency required).\n"
63 " --mesh Create a mesh.\n"
64 " --texture Create a mesh with texture.\n"
65 " --texture_size # Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n"
66 " --texture_count # Maximum textures generated (default 1). Ignored by --multiband option (adjust --multiband_contrib instead).\n"
67 " --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
68 " --texture_angle # Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
69 " --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used, default=0).\n"
70 " --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%% of the image bottom not used).\n"
71 " --texture_d2c Distance to camera policy.\n"
72 " --texture_blur # Motion blur threshold (default 0: disabled). Below this threshold, the image is considered blurred. 0 means disabled. 50 can be good default.\n"
73 " --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
74 " --cam_projection_keep_all Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
75 " --cam_projection_decimation Decimate images before projecting the points.\n"
76 " --cam_projection_mask \"\" File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.\n"
77 " --poses Export optimized poses of the robot frame (e.g., base_link).\n"
78 " --poses_camera Export optimized poses of the camera frame (e.g., optical frame).\n"
79 " --poses_scan Export optimized poses of the scan frame.\n"
80 " --poses_format # Format used for exported poses (default is 11):\n"
81 " 0=Raw 3x4 transformation matrix (r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz)\n"
82 " 1=RGBD-SLAM (in motion capture coordinate frame)\n"
83 " 2=KITTI (same as raw but in optical frame)\n"
86 " 10=RGBD-SLAM in ROS coordinate frame (stamp x y z qx qy qz qw)\n"
87 " 11=RGBD-SLAM in ROS coordinate frame + ID (stamp x y z qx qy qz qw id)\n"
88 " --images Export images with stamp as file name.\n"
89 " --images_id Export images with node id as file name.\n"
90 " --ba Do global bundle adjustment before assembling the clouds.\n"
91 " --gain # Gain compensation value (default 1, set 0 to disable).\n"
92 " --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n"
93 " --no_blending Disable blending when texturing.\n"
94 " --no_clean Disable cleaning colorless polygons.\n"
95 " --min_cluster # When meshing, filter clusters of polygons with size less than this threshold (default 200, -1 means keep only biggest contiguous surface).\n"
96 " --low_gain # Low brightness gain 0-100 (default 0).\n"
97 " --high_gain # High brightness gain 0-100 (default 10).\n"
98 " --multiband Enable multiband texturing (AliceVision dependency required).\n"
99 " --multiband_downscale # Downscaling reduce the texture quality but speed up the computation time (default 2).\n"
100 " --multiband_contrib \"# # # # \" Number of contributions per frequency band for the multi-band blending, should be 4 values! (default \"1 5 10 0\").\n"
101 " --multiband_unwrap # Method to unwrap input mesh: 0=basic (default, >600k faces, fast), 1=ABF (<=300k faces, generate 1 atlas), 2=LSCM (<=600k faces, optimize space).\n"
102 " --multiband_fillholes Fill Texture holes with plausible values.\n"
103 " --multiband_padding # Texture edge padding size in pixel (0-100) (default 5).\n"
104 " --multiband_scorethr # 0 to disable filtering based on threshold to relative best score (0.0-1.0). (default 0.1).\n"
105 " --multiband_anglethr # 0 to disable angle hard threshold filtering (0.0, 180.0) (default 90.0).\n"
106 " --multiband_forcevisible Triangle visibility is based on the union of vertices visibility.\n"
107 " --poisson_depth # Set Poisson depth for mesh reconstruction.\n"
108 " --poisson_size # Set target polygon size when computing Poisson's depth for mesh reconstruction (default 0.03 m).\n"
109 " --max_polygons # Maximum polygons when creating a mesh (default 300000, set 0 for no limit).\n"
110 " --min_range # Minimum range of the created clouds (default 0 m).\n"
111 " --max_range # Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
112 " --decimation # Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
113 " --voxel # Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
114 " --ground_normals_up # Flip ground normals up if close to -z axis (default 0, 0=disabled, value should be >0 and <1, typical 0.9).\n"
115 " --noise_radius # Noise filtering search radius (default 0, 0=disabled).\n"
116 " --noise_k # Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
117 " --prop_radius_factor # Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
118 " --prop_radius_scale # Proportional radius filter neighbor scale (default 2).\n"
119 " --random_samples # Number of output samples using a random filter (default 0, 0=disabled).\n"
120 " --color_radius # Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
121 " --scan Use laser scan for the point cloud.\n"
122 " --save_in_db Save resulting assembled point cloud or mesh in the database.\n"
123 " --xmin # Minimum range on X axis to keep nodes to export.\n"
124 " --xmax # Maximum range on X axis to keep nodes to export.\n"
125 " --ymin # Minimum range on Y axis to keep nodes to export.\n"
126 " --ymax # Maximum range on Y axis to keep nodes to export.\n"
127 " --zmin # Minimum range on Z axis to keep nodes to export.\n"
128 " --zmax # Maximum range on Z axis to keep nodes to export.\n"
129 " --filter_ceiling # Filter points over a custom height (default 0 m, 0=disabled).\n"
130 " --filter_floor # Filter points below a custom height (default 0 m, 0=disabled).\n"
139 virtual bool callback(
const std::string & msg)
const
142 printf(
"%s\n",
msg.c_str());
160 bool texture =
false;
162 bool doGainCompensationRGB =
true;
164 bool doBlending =
true;
166 int minCluster = 200;
167 int poissonDepth = 0;
168 float poissonSize = 0.03;
169 int maxPolygons = 300000;
171 float minRange = 0.0f;
172 float maxRange = -1.0f;
173 float voxelSize = -1.0f;
174 float groundNormalsUp = 0.0f;
175 float noiseRadius = 0.0f;
176 int noiseMinNeighbors = 5;
177 float proportionalRadiusFactor = 0.0f;
178 float proportionalRadiusScale = 2.0f;
179 int randomSamples = 0;
180 int textureSize = 8192;
181 int textureCount = 1;
182 float textureRange = 0;
183 float textureAngle = 0;
184 float textureDepthError = 0;
185 std::vector<float> textureRoiRatios;
186 bool distanceToCamPolicy =
false;
187 int laplacianThr = 0;
188 bool multiband =
false;
189 int multibandDownScale = 2;
190 std::string multibandNbContrib =
"1 5 10 0";
191 int multibandUnwrap = 0;
192 bool multibandFillHoles =
false;
193 int multibandPadding = 5;
194 double multibandBestScoreThr = 0.1;
195 double multibandAngleHardthr = 90;
196 bool multibandForceVisible =
false;
197 float colorRadius = -1.0f;
198 bool cloudFromScan =
false;
199 bool saveInDb =
false;
200 int lowBrightnessGain = 0;
201 int highBrightnessGain = 10;
202 bool camProjection =
false;
203 bool camProjectionKeepAll =
false;
204 int cameraProjDecimation = 1;
205 std::string cameraProjMask;
207 bool exportPosesCamera =
false;
208 bool exportPosesScan =
false;
209 int exportPosesFormat = 11;
210 bool exportImages =
false;
211 bool exportImagesId =
false;
212 std::string outputName;
213 std::string outputDir;
215 float filter_ceiling = 0.0f;
216 float filter_floor = 0.0f;
217 for(
int i=1;
i<argc; ++
i)
219 if(std::strcmp(
argv[
i],
"--help") == 0)
223 else if(std::strcmp(
argv[
i],
"--output") == 0)
228 outputName =
argv[
i];
235 else if(std::strcmp(
argv[
i],
"--output_dir") == 0)
247 else if(std::strcmp(
argv[
i],
"--bin") == 0)
249 printf(
"No need to set --bin anymore, ply are now automatically exported in binary by default. Set --ascii to export as text.\n");
251 else if(std::strcmp(
argv[
i],
"--ascii") == 0)
255 else if(std::strcmp(
argv[
i],
"--las") == 0)
260 printf(
"\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n");
264 else if(std::strcmp(
argv[
i],
"--mesh") == 0)
268 else if(std::strcmp(
argv[
i],
"--texture") == 0)
272 else if(std::strcmp(
argv[
i],
"--texture_size") == 0)
285 else if(std::strcmp(
argv[
i],
"--texture_count") == 0)
297 else if(std::strcmp(
argv[
i],
"--texture_range") == 0)
309 else if(std::strcmp(
argv[
i],
"--texture_angle") == 0)
321 else if(std::strcmp(
argv[
i],
"--texture_depth_error") == 0)
333 else if(std::strcmp(
argv[
i],
"--texture_roi_ratios") == 0)
338 std::list<std::string> strValues =
uSplit(
argv[
i],
' ');
339 if(strValues.size() != 4)
341 printf(
"The number of values must be 4 (roi=\"%s\")\n",
argv[
i]);
346 std::vector<float> tmpValues(4);
348 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
354 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0
f-tmpValues[1] &&
355 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0
f-tmpValues[0] &&
356 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0
f-tmpValues[3] &&
357 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0
f-tmpValues[2])
359 textureRoiRatios = tmpValues;
363 printf(
"The roi ratios are not valid (roi=\"%s\")\n",
argv[
i]);
373 else if(std::strcmp(
argv[
i],
"--texture_d2c") == 0)
375 distanceToCamPolicy =
true;
377 else if(std::strcmp(
argv[
i],
"--texture_blur") == 0)
389 else if(std::strcmp(
argv[
i],
"--cam_projection") == 0)
391 camProjection =
true;
393 else if(std::strcmp(
argv[
i],
"--cam_projection_keep_all") == 0)
395 camProjectionKeepAll =
true;
397 else if(std::strcmp(
argv[
i],
"--cam_projection_decimation") == 0)
403 if(cameraProjDecimation<1)
405 printf(
"--cam_projection_decimation cannot be <1! value=\"%s\"\n",
argv[
i]);
414 else if(std::strcmp(
argv[
i],
"--cam_projection_mask") == 0)
419 cameraProjMask =
argv[
i];
422 printf(
"--cam_projection_mask is set with a file not existing or don't have permissions to open it. Path=\"%s\"\n",
argv[
i]);
431 else if(std::strcmp(
argv[
i],
"--poses") == 0)
435 else if(std::strcmp(
argv[
i],
"--poses_camera") == 0)
437 exportPosesCamera =
true;
439 else if(std::strcmp(
argv[
i],
"--poses_scan") == 0)
441 exportPosesScan =
true;
443 else if(std::strcmp(
argv[
i],
"--poses_format") == 0)
455 else if(std::strcmp(
argv[
i],
"--images") == 0)
459 else if(std::strcmp(
argv[
i],
"--images_id") == 0)
462 exportImagesId =
true;
464 else if(std::strcmp(
argv[
i],
"--ba") == 0)
468 else if(std::strcmp(
argv[
i],
"--gain_gray") == 0)
470 doGainCompensationRGB =
false;
472 else if(std::strcmp(
argv[
i],
"--gain") == 0)
485 else if(std::strcmp(
argv[
i],
"--no_blending") == 0)
489 else if(std::strcmp(
argv[
i],
"--no_clean") == 0)
493 else if(std::strcmp(
argv[
i],
"--min_cluster") == 0)
505 else if(std::strcmp(
argv[
i],
"--multiband") == 0)
507 #ifdef RTABMAP_ALICE_VISION
510 printf(
"\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
513 else if(std::strcmp(
argv[
i],
"--multiband_fillholes") == 0)
515 multibandFillHoles =
true;
517 else if(std::strcmp(
argv[
i],
"--multiband_downscale") == 0)
529 else if(std::strcmp(
argv[
i],
"--multiband_contrib") == 0)
536 printf(
"--multiband_contrib has wrong format! value=\"%s\"\n",
argv[
i]);
539 multibandNbContrib =
argv[
i];
546 else if(std::strcmp(
argv[
i],
"--multiband_unwrap") == 0)
558 else if(std::strcmp(
argv[
i],
"--multiband_padding") == 0)
570 else if(std::strcmp(
argv[
i],
"--multiband_forcevisible") == 0)
572 multibandForceVisible =
true;
574 else if(std::strcmp(
argv[
i],
"--multiband_scorethr") == 0)
586 else if(std::strcmp(
argv[
i],
"--multiband_anglethr") == 0)
598 else if(std::strcmp(
argv[
i],
"--poisson_depth") == 0)
610 else if(std::strcmp(
argv[
i],
"--poisson_size") == 0)
622 else if(std::strcmp(
argv[
i],
"--max_polygons") == 0)
634 else if(std::strcmp(
argv[
i],
"--min_range") == 0)
646 else if(std::strcmp(
argv[
i],
"--max_range") == 0)
658 else if(std::strcmp(
argv[
i],
"--decimation") == 0)
670 else if(std::strcmp(
argv[
i],
"--voxel") == 0)
682 else if(std::strcmp(
argv[
i],
"--ground_normals_up") == 0)
694 else if(std::strcmp(
argv[
i],
"--noise_radius") == 0)
706 else if(std::strcmp(
argv[
i],
"--noise_k") == 0)
718 else if(std::strcmp(
argv[
i],
"--prop_radius_factor") == 0)
730 else if(std::strcmp(
argv[
i],
"--prop_radius_scale") == 0)
736 UASSERT_MSG(proportionalRadiusScale>=1.0
f,
"--prop_radius_scale should be >= 1.0");
743 else if(std::strcmp(
argv[
i],
"--random_samples") == 0)
755 else if(std::strcmp(
argv[
i],
"--color_radius") == 0)
767 else if(std::strcmp(
argv[
i],
"--scan") == 0)
769 cloudFromScan =
true;
771 else if(std::strcmp(
argv[
i],
"--save_in_db") == 0)
775 else if(std::strcmp(
argv[
i],
"--low_gain") == 0)
787 else if(std::strcmp(
argv[
i],
"--high_gain") == 0)
799 else if(std::strcmp(
argv[
i],
"--xmin") == 0)
811 else if(std::strcmp(
argv[
i],
"--xmax") == 0)
823 else if(std::strcmp(
argv[
i],
"--ymin") == 0)
835 else if(std::strcmp(
argv[
i],
"--ymax") == 0)
847 else if(std::strcmp(
argv[
i],
"--zmin") == 0)
859 else if(std::strcmp(
argv[
i],
"--zmax") == 0)
871 else if(std::strcmp(
argv[
i],
"--filter_ceiling") == 0)
877 if(filter_floor!=0.0
f && filter_ceiling != 0.0
f && filter_ceiling<filter_floor)
879 printf(
"Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
888 else if(std::strcmp(
argv[
i],
"--filter_floor") == 0)
894 if(filter_floor!=0.0
f && filter_ceiling != 0.0
f && filter_ceiling<filter_floor)
896 printf(
"Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
910 decimation = cloudFromScan?1:4;
914 maxRange = cloudFromScan?0:4;
918 voxelSize = cloudFromScan?0:0.01f;
920 if(colorRadius < 0.0
f)
922 colorRadius = cloudFromScan?0:0.05f;
929 printf(
"Option --multiband is not supported with --save_in_db option, disabling multiband...\n");
934 printf(
"Option --texture_count > 1 is not supported with --save_in_db option, setting texture_count to 1...\n");
941 std::string dbPath =
argv[argc-1];
945 UERROR(
"File \"%s\" doesn't exist!", dbPath.c_str());
959 UERROR(
"Cannot open database %s!", dbPath.c_str());
967 printf(
"Added custom parameter %s=%s\n",
iter->first.c_str(),
iter->second.c_str());
972 printf(
"Loading database \"%s\"...\n", dbPath.c_str());
976 rtabmap.init(parameters, dbPath);
977 printf(
"Loading database \"%s\"... done (%fs).\n", dbPath.c_str(),
timer.ticks());
979 std::map<int, Signature>
nodes;
980 std::map<int, Transform> optimizedPoses;
981 std::multimap<int, Link> links;
982 printf(
"Optimizing the map...\n");
983 rtabmap.getGraph(optimizedPoses, links,
true,
true, &
nodes,
true,
true,
true,
true);
984 printf(
"Optimizing the map... done (%fs, poses=%d).\n",
timer.ticks(), (
int)optimizedPoses.size());
986 if(optimizedPoses.empty())
988 printf(
"The optimized graph is empty!? Aborting...\n");
996 printf(
"Filtering poses (range: x=%f<->%f, y=%f<->%f, z=%f<->%f, map size=%f x %f x %f)...\n",
998 maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
999 std::map<int, Transform> posesFiltered;
1000 for(std::map<int, Transform>::const_iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
1002 bool ignore =
false;
1017 posesFiltered.insert(*
iter);
1021 printf(
"Filtering poses... done! %d/%d remaining (new map size=%f x %f x %f).\n", (
int)posesFiltered.size(), (
int)optimizedPoses.size(), maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
1022 optimizedPoses = posesFiltered;
1023 if(optimizedPoses.empty())
1029 std::string outputDirectory = outputDir.empty()?
UDirectory::getDir(dbPath):outputDir;
1034 std::string baseName = outputName.empty()?
uSplit(
UFile::getName(dbPath),
'.').front():outputName;
1038 printf(
"Global bundle adjustment...\n");
1040 optimizedPoses = ((
Optimizer*)&
g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links,
nodes,
true);
1041 printf(
"Global bundle adjustment... done (%fs).\n",
timer.ticks());
1045 printf(
"Create and assemble the clouds...\n");
1046 pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
1047 pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(
new pcl::PointCloud<pcl::PointXYZI>);
1048 std::map<int, rtabmap::Transform> robotPoses;
1049 std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
1050 std::map<int, rtabmap::Transform> scanPoses;
1051 std::map<int, double> cameraStamps;
1052 std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
1053 std::map<int, cv::Mat> cameraDepths;
1054 int imagesExported = 0;
1055 std::vector<int> rawViewpointIndices;
1056 std::map<int, Transform> rawViewpoints;
1057 for(std::map<int, Transform>::iterator
iter=optimizedPoses.lower_bound(1);
iter!=optimizedPoses.end(); ++
iter)
1067 pcl::IndicesPtr
indices(
new std::vector<int>);
1068 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1069 pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
1079 printf(
"Node %d doesn't have scan data, empty cloud is created.\n",
iter->first);
1081 if(decimation>1 || minRange>0.0
f || maxRange)
1088 if(noiseRadius>0.0
f && noiseMinNeighbors>0)
1096 if(noiseRadius>0.0
f && noiseMinNeighbors>0)
1107 printf(
"Node %d doesn't have depth or stereo data, empty cloud is "
1108 "created (if you want to create point cloud from scan, use --scan option).\n",
iter->first);
1116 if(noiseRadius>0.0
f && noiseMinNeighbors>0)
1123 if(exportImages && !rgb.empty())
1125 std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1 && !depth.empty())?
"left":
"rgb";
1126 std::string dir = outputDirectory+
"/"+baseName+
"_"+dirSuffix;
1131 cv::imwrite(outputPath, rgb);
1136 cv::Mat depthExported = depth;
1137 if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1)
1140 dir = outputDirectory+
"/"+baseName+
"_right";
1145 dir = outputDirectory+
"/"+baseName+
"_depth";
1146 if(depth.type() == CV_32FC1)
1156 cv::imwrite(outputPath, depthExported);
1160 for(
size_t i=0;
i<models.size(); ++
i)
1164 if(models.size() > 1) {
1167 model.setName(modelName);
1168 std::string dir = outputDirectory+
"/"+baseName+
"_calib";
1174 for(
size_t i=0;
i<stereoModels.size(); ++
i)
1178 if(stereoModels.size() > 1) {
1181 model.setName(modelName,
"left",
"right");
1182 std::string dir = outputDirectory+
"/"+baseName+
"_calib";
1192 if(cloud.get() && !cloud->empty())
1194 else if(cloudI.get() && !cloudI->empty())
1197 if(cloud.get() && !cloud->empty())
1199 else if(cloudI.get() && !cloudI->empty())
1202 if(filter_ceiling != 0.0 || filter_floor != 0.0
f)
1204 if(cloud.get() && !cloud->empty())
1208 if(cloudI.get() && !cloudI->empty())
1217 rawViewpoints.insert(std::make_pair(
iter->first, lidarViewpoint));
1222 rawViewpoints.insert(std::make_pair(
iter->first, cameraViewpoint));
1227 rawViewpoints.insert(std::make_pair(
iter->first, cameraViewpoint));
1231 rawViewpoints.insert(*
iter);
1234 if(cloud.get() && !cloud->empty())
1236 if(assembledCloud->empty())
1238 *assembledCloud = *cloud;
1242 *assembledCloud += *cloud;
1244 rawViewpointIndices.resize(assembledCloud->size(),
iter->first);
1246 else if(cloudI.get() && !cloudI->empty())
1248 if(assembledCloudI->empty())
1250 *assembledCloudI = *cloudI;
1254 *assembledCloudI += *cloudI;
1256 rawViewpointIndices.resize(assembledCloudI->size(),
iter->first);
1267 robotPoses.insert(std::make_pair(
iter->first,
iter->second));
1268 cameraStamps.insert(std::make_pair(
iter->first, node.
getStamp()));
1269 if(models.empty() && node.
getWeight() == -1 && !cameraModels.empty())
1272 models = cameraModels.rbegin()->second;
1278 cameraModels.insert(std::make_pair(
iter->first, models));
1280 if(exportPosesCamera)
1282 if(cameraPoses.empty())
1284 cameraPoses.resize(models.size());
1286 UASSERT_MSG(models.size() == cameraPoses.size(),
"Not all nodes have same number of cameras to export camera poses.");
1287 for(
size_t i=0;
i<models.size(); ++
i)
1289 cameraPoses[
i].insert(std::make_pair(
iter->first,
iter->second*models[
i].localTransform()));
1293 if(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
1295 cameraDepths.insert(std::make_pair(
iter->first, depth));
1302 printf(
"Create and assemble the clouds... done (%fs, %d points).\n",
timer.ticks(), !assembledCloud->empty()?(
int)assembledCloud->size():(
int)assembledCloudI->size());
1304 if(imagesExported>0)
1305 printf(
"%d images exported!\n", imagesExported);
1309 if(!assembledCloud->empty() || !assembledCloudI->empty())
1323 std::string posesExt = (exportPosesFormat==3?
"toro":exportPosesFormat==4?
"g2o":
"txt");
1326 std::string outputPath=outputDirectory+
"/"+baseName+
"_poses." + posesExt;
1328 printf(
"Poses exported to \"%s\".\n", outputPath.c_str());
1330 if(exportPosesCamera)
1332 for(
size_t i=0;
i<cameraPoses.size(); ++
i)
1334 std::string outputPath;
1335 if(cameraPoses.size()==1)
1336 outputPath = outputDirectory+
"/"+baseName+
"_camera_poses." + posesExt;
1338 outputPath = outputDirectory+
"/"+baseName+
"_camera_poses_"+
uNumber2Str((
int)
i)+
"." + posesExt;
1340 printf(
"Camera poses exported to \"%s\".\n", outputPath.c_str());
1345 std::string outputPath=outputDirectory+
"/"+baseName+
"_scan_poses." + posesExt;
1347 printf(
"Scan poses exported to \"%s\".\n", outputPath.c_str());
1351 if(proportionalRadiusFactor>0.0
f && proportionalRadiusScale>=1.0
f)
1353 printf(
"Proportional radius filtering of the assembled cloud... (factor=%f scale=%f, %d points)\n", proportionalRadiusFactor, proportionalRadiusScale, !assembledCloud->empty()?(
int)assembledCloud->size():(
int)assembledCloudI->size());
1355 if(!assembledCloud->empty())
1358 pcl::PointCloud<pcl::PointXYZRGB> tmp;
1359 pcl::copyPointCloud(*assembledCloud, *
indices, tmp);
1360 *assembledCloud = tmp;
1362 else if(!assembledCloudI->empty())
1365 pcl::PointCloud<pcl::PointXYZI> tmp;
1366 pcl::copyPointCloud(*assembledCloudI, *
indices, tmp);
1367 *assembledCloudI = tmp;
1371 std::vector<int> rawCameraIndicesTmp(
indices->size());
1372 for (std::size_t
i = 0;
i <
indices->size(); ++
i)
1373 rawCameraIndicesTmp[
i] = rawViewpointIndices[
indices->at(
i)];
1374 rawViewpointIndices = rawCameraIndicesTmp;
1376 printf(
"Proportional radius filtering of the assembled cloud.... done! (%fs, %d points)\n",
timer.ticks(), !assembledCloud->empty()?(
int)assembledCloud->size():(
int)assembledCloudI->size());
1379 pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(
new pcl::PointCloud<pcl::PointXYZ>);
1380 if(!assembledCloud->empty())
1381 pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud);
1382 else if(!assembledCloudI->empty())
1383 pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud);
1385 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals = rawAssembledCloud;
1389 printf(
"Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(
int)assembledCloud->size():(
int)assembledCloudI->size());
1390 if(!assembledCloud->empty())
1393 cloudWithoutNormals.reset(
new pcl::PointCloud<pcl::PointXYZ>);
1394 pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
1396 else if(!assembledCloudI->empty())
1399 cloudWithoutNormals.reset(
new pcl::PointCloud<pcl::PointXYZ>);
1400 pcl::copyPointCloud(*assembledCloudI, *cloudWithoutNormals);
1402 printf(
"Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n",
timer.ticks(), !assembledCloud->empty()?(
int)assembledCloud->size():(
int)assembledCloudI->size());
1405 printf(
"Computing normals of the assembled cloud... (k=20, %d points)\n", !assembledCloud->empty()?(
int)assembledCloud->size():(
int)assembledCloudI->size());
1408 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
1409 pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport(
new pcl::PointCloud<pcl::PointXYZINormal>);
1410 if(!assembledCloud->empty())
1412 UASSERT(assembledCloud->size() == normals->size());
1413 pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport);
1414 printf(
"Computing normals of the assembled cloud... done! (%fs, %d points)\n",
timer.ticks(), (
int)assembledCloud->size());
1415 assembledCloud->clear();
1418 printf(
"Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (
int)cloudToExport->size());
1422 rawViewpointIndices,
1425 printf(
"Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n",
timer.ticks(), (
int)cloudToExport->size());
1427 else if(!assembledCloudI->empty())
1429 UASSERT(assembledCloudI->size() == normals->size());
1430 pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport);
1431 printf(
"Computing normals of the assembled cloud... done! (%fs, %d points)\n",
timer.ticks(), (
int)assembledCloudI->size());
1432 assembledCloudI->clear();
1435 printf(
"Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (
int)cloudIToExport->size());
1439 rawViewpointIndices,
1442 printf(
"Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n",
timer.ticks(), (
int)cloudIToExport->size());
1444 cloudWithoutNormals->clear();
1445 rawAssembledCloud->clear();
1449 printf(
"Random samples filtering of the assembled cloud... (samples=%d, %d points)\n", randomSamples, !cloudToExport->empty()?(
int)cloudToExport->size():(
int)cloudIToExport->size());
1450 if(!cloudToExport->empty())
1454 else if(!cloudIToExport->empty())
1458 printf(
"Random samples filtering of the assembled cloud.... done! (%fs, %d points)\n",
timer.ticks(), !cloudToExport->empty()?(
int)cloudToExport->size():(
int)cloudIToExport->size());
1461 std::vector<int> pointToCamId;
1462 std::vector<float> pointToCamIntensity;
1463 if(camProjection && !robotPoses.empty())
1465 printf(
"Camera projection...\n");
1466 std::map<int, std::vector<rtabmap::CameraModel> > cameraModelsProj;
1467 if(cameraProjDecimation>1)
1469 for(std::map<
int, std::vector<rtabmap::CameraModel> >::
iterator iter=cameraModels.begin();
1470 iter!=cameraModels.end();
1473 std::vector<rtabmap::CameraModel> models;
1474 for(
size_t i=0;
i<
iter->second.size(); ++
i)
1476 models.push_back(
iter->second[
i].scaled(1.0/
double(cameraProjDecimation)));
1478 cameraModelsProj.insert(std::make_pair(
iter->first, models));
1483 cameraModelsProj = cameraModels;
1488 printf(
"Camera projection... projecting cloud to individual cameras (--images option)\n");
1490 pcl::PCLPointCloud2::Ptr cloud2(
new pcl::PCLPointCloud2);
1491 if(!cloudToExport->empty())
1493 pcl::toPCLPointCloud2(*cloudToExport, *cloud2);
1495 else if(!cloudIToExport->empty())
1497 pcl::toPCLPointCloud2(*cloudIToExport, *cloud2);
1500 std::string dir = outputDirectory+
"/"+baseName+
"_depth_from_scan";
1505 for(std::map<
int, std::vector<rtabmap::CameraModel> >::
iterator iter=cameraModelsProj.begin();
1506 iter!=cameraModelsProj.end();
1509 cv::Mat depth(
iter->second.front().imageHeight(),
iter->second.front().imageWidth()*
iter->second.size(), CV_32FC1);
1510 for(
size_t i=0;
i<
iter->second.size(); ++
i)
1513 iter->second.at(
i).imageSize(),
1514 iter->second.at(
i).K(),
1516 robotPoses.at(
iter->first) *
iter->second.at(
i).localTransform());
1517 subDepth.copyTo(depth(
cv::Range::all(), cv::Range(
i*
iter->second.front().imageWidth(), (
i+1)*
iter->second.front().imageWidth())));
1522 cv::imwrite(outputPath, depth);
1527 if(!cameraProjMask.empty())
1529 projMask = cv::imread(cameraProjMask, cv::IMREAD_GRAYSCALE);
1530 if(cameraProjDecimation>1)
1532 cv::Mat
out = projMask;
1533 cv::resize(projMask,
out, cv::Size(), 1.0
f/
float(cameraProjDecimation), 1.0
f/
float(cameraProjDecimation), cv::INTER_NEAREST);
1538 printf(
"Camera projection... projecting cloud to all cameras\n");
1539 pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
1540 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
1541 if(!cloudToExport->empty())
1551 distanceToCamPolicy,
1554 else if(!cloudIToExport->empty())
1564 distanceToCamPolicy,
1566 pointToCamIntensity.resize(pointToPixel.size());
1569 printf(
"Camera projection... coloring the cloud\n");
1571 UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size());
1572 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints(
new pcl::PointCloud<pcl::PointXYZRGBNormal>());
1573 assembledCloudValidPoints->resize(pointToCamId.size());
1576 for(std::map<int, rtabmap::Transform>::iterator
iter=robotPoses.begin();
iter!=robotPoses.end(); ++
iter)
1578 int nodeID =
iter->first;
1582 nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0);
1586 if(cameraProjDecimation>1)
1590 UASSERT(cameraModelsProj.find(nodeID) != cameraModelsProj.end());
1591 int modelsSize = cameraModelsProj.at(nodeID).size();
1592 for(
size_t i=0;
i<pointToPixel.size(); ++
i)
1594 int cameraIndex = pointToPixel[
i].first.second;
1595 if(nodeID == pointToPixel[
i].
first.first && cameraIndex>=0)
1597 pcl::PointXYZRGBNormal pt;
1598 float intensity = 0;
1599 if(!cloudToExport->empty())
1601 pt = cloudToExport->at(
i);
1603 else if(!cloudIToExport->empty())
1605 pt.x = cloudIToExport->at(
i).x;
1606 pt.y = cloudIToExport->at(
i).y;
1607 pt.z = cloudIToExport->at(
i).z;
1608 pt.normal_x = cloudIToExport->at(
i).normal_x;
1609 pt.normal_y = cloudIToExport->at(
i).normal_y;
1610 pt.normal_z = cloudIToExport->at(
i).normal_z;
1611 intensity = cloudIToExport->at(
i).intensity;
1614 int subImageWidth = image.cols / modelsSize;
1615 cv::Mat subImage = image(
cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
1617 int x = pointToPixel[
i].second.x * (
float)subImage.cols;
1618 int y = pointToPixel[
i].second.y * (
float)subImage.rows;
1622 if(subImage.type()==CV_8UC3)
1624 cv::Vec3b bgr = subImage.at<cv::Vec3b>(
y,
x);
1631 UASSERT(subImage.type()==CV_8UC1);
1632 pt.r = pt.g = pt.b = subImage.at<
unsigned char>(pointToPixel[
i].second.y * subImage.rows, pointToPixel[
i].second.x * subImage.cols);
1635 int exportedId = nodeID;
1636 pointToCamId[
i] = exportedId;
1637 if(!pointToCamIntensity.empty())
1639 pointToCamIntensity[
i] = intensity;
1641 assembledCloudValidPoints->at(
i) = pt;
1645 UINFO(
"Processed %d/%d images", imagesDone++, (
int)robotPoses.size());
1648 pcl::IndicesPtr validIndices(
new std::vector<int>(pointToPixel.size()));
1650 for(
size_t i=0;
i<pointToPixel.size(); ++
i)
1652 if(pointToPixel[
i].
first.first <=0)
1654 if(camProjectionKeepAll)
1656 pcl::PointXYZRGBNormal pt;
1657 float intensity = 0;
1658 if(!cloudToExport->empty())
1660 pt = cloudToExport->at(
i);
1662 else if(!cloudIToExport->empty())
1664 pt.x = cloudIToExport->at(
i).x;
1665 pt.y = cloudIToExport->at(
i).y;
1666 pt.z = cloudIToExport->at(
i).z;
1667 pt.normal_x = cloudIToExport->at(
i).normal_x;
1668 pt.normal_y = cloudIToExport->at(
i).normal_y;
1669 pt.normal_z = cloudIToExport->at(
i).normal_z;
1670 intensity = cloudIToExport->at(
i).intensity;
1673 pointToCamId[
i] = 0;
1677 if(!pointToCamIntensity.empty())
1679 pointToCamIntensity[
i] = intensity;
1681 assembledCloudValidPoints->at(
i) = pt;
1682 validIndices->at(oi++) =
i;
1687 validIndices->at(oi++) =
i;
1691 if(oi != validIndices->size())
1693 validIndices->resize(oi);
1694 assembledCloudValidPoints =
util3d::extractIndices(assembledCloudValidPoints, validIndices,
false,
false);
1695 std::vector<int> pointToCamIdTmp(validIndices->size());
1696 std::vector<float> pointToCamIntensityTmp(validIndices->size());
1697 for(
size_t i=0;
i<validIndices->size(); ++
i)
1699 pointToCamIdTmp[
i] = pointToCamId[validIndices->at(
i)];
1700 pointToCamIntensityTmp[
i] = pointToCamIntensity[validIndices->at(
i)];
1702 pointToCamId = pointToCamIdTmp;
1703 pointToCamIntensity = pointToCamIntensityTmp;
1704 pointToCamIdTmp.clear();
1705 pointToCamIntensityTmp.clear();
1708 cloudToExport = assembledCloudValidPoints;
1709 cloudIToExport->clear();
1711 printf(
"Camera projection... done! (%fs)\n",
timer.ticks());
1714 if(!(mesh || texture))
1718 printf(
"Saving in db... (%d points)\n", !cloudToExport->empty()?(
int)cloudToExport->size():(
int)cloudIToExport->size());
1719 if(!cloudToExport->empty())
1721 else if(!cloudIToExport->empty())
1723 printf(
"Saving in db... done!\n");
1727 std::string ext = las?
"las":
"ply";
1728 std::string outputPath=outputDirectory+
"/"+baseName+
"_cloud."+ext;
1729 printf(
"Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(
int)cloudToExport->size():(
int)cloudIToExport->size());
1731 if(las || !pointToCamId.empty() || !pointToCamIntensity.empty())
1733 if(!cloudToExport->empty())
1735 if(!pointToCamIntensity.empty())
1744 else if(!cloudIToExport->empty())
1750 if(!pointToCamId.empty())
1752 if(!pointToCamIntensity.empty())
1754 printf(
"Option --cam_projection is enabled but rtabmap is not built "
1755 "with PDAL support, so camera IDs and lidar intensities won't be exported in the output cloud.\n");
1759 printf(
"Option --cam_projection is enabled but rtabmap is not built "
1760 "with PDAL support, so camera IDs won't be exported in the output cloud.\n");
1763 if(!cloudToExport->empty())
1764 pcl::io::savePLYFile(outputPath, *cloudToExport,
binary);
1765 else if(!cloudIToExport->empty())
1766 pcl::io::savePLYFile(outputPath, *cloudIToExport,
binary);
1768 printf(
"Saving %s... done!\n", outputPath.c_str());
1775 if(!cloudIToExport->empty())
1777 pcl::copyPointCloud(*cloudIToExport, *cloudToExport);
1778 cloudIToExport->clear();
1784 int optimizedDepth = 12;
1785 for(
int i=6;
i<12; ++
i)
1787 if(mapLength/
float(1<<
i) < poissonSize)
1795 optimizedDepth = poissonDepth;
1799 printf(
"Mesh reconstruction... depth=%d\n", optimizedDepth);
1800 pcl::PolygonMesh::Ptr mesh(
new pcl::PolygonMesh);
1801 pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
1802 poisson.setDepth(optimizedDepth);
1803 poisson.setInputCloud(cloudToExport);
1804 poisson.reconstruct(*mesh);
1805 printf(
"Mesh reconstruction... done (%fs, %d polygons).\n",
timer.ticks(), (
int)mesh->polygons.size());
1807 if(mesh->polygons.size())
1809 printf(
"Mesh color transfer (max polygons=%d, color radius=%f, clean=%s)...\n",
1812 doClean?
"true":
"false");
1813 rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
1822 printf(
"Mesh color transfer... done (%fs).\n",
timer.ticks());
1828 printf(
"Saving mesh in db...\n");
1829 std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
1834 printf(
"Saving mesh in db... done!\n");
1838 std::string outputPath=outputDirectory+
"/"+baseName+
"_mesh.ply";
1839 printf(
"Saving %s...\n", outputPath.c_str());
1841 pcl::io::savePLYFileBinary(outputPath, *mesh);
1843 pcl::io::savePLYFile(outputPath, *mesh);
1844 printf(
"Saving %s... done!\n", outputPath.c_str());
1850 std::map<int, rtabmap::Transform> robotPosesFiltered;
1853 printf(
"Filtering %ld images from texturing...\n", robotPoses.size());
1854 for(std::map<int, rtabmap::Transform>::iterator
iter=robotPoses.begin();
iter!=robotPoses.end(); ++
iter)
1858 nodes.find(
iter->first)->second.sensorData().uncompressDataConst(&
img, 0);
1861 cv::Mat imgLaplacian;
1862 cv::Laplacian(
img, imgLaplacian, CV_16S);
1864 cv::meanStdDev(imgLaplacian,
m,
s);
1865 double stddev_pxl =
s.at<
double>(0);
1866 double var = stddev_pxl*stddev_pxl;
1867 if(var < (
double)laplacianThr)
1869 printf(
"Camera's image %d is detected as blurry (var=%f < thr=%d), camera is ignored for texturing.\n",
iter->first, var, laplacianThr);
1873 robotPosesFiltered.insert(*
iter);
1877 printf(
"Filtered %ld/%ld images from texturing", robotPosesFiltered.size(), robotPoses.size());
1881 robotPosesFiltered = robotPoses;
1884 printf(
"Texturing %d polygons... robotPoses=%d, cameraModels=%d, cameraDepths=%d\n", (
int)mesh->polygons.size(), (
int)robotPosesFiltered.size(), (
int)cameraModels.size(), (
int)cameraDepths.size());
1885 std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
1898 distanceToCamPolicy);
1899 printf(
"Texturing... done (%fs).\n",
timer.ticks());
1902 if(doClean && textureMesh->tex_coordinates.size())
1904 printf(
"Cleanup mesh...\n");
1906 printf(
"Cleanup mesh... done (%fs).\n",
timer.ticks());
1909 if(textureMesh->tex_materials.size())
1913 printf(
"Merging %d texture(s) to single one (multiband enabled)...\n", (
int)textureMesh->tex_materials.size());
1917 printf(
"Merging %d texture(s)... (%d max textures)\n", (
int)textureMesh->tex_materials.size(), textureCount);
1919 std::map<int, std::map<int, cv::Vec4d> > gains;
1920 std::map<int, std::map<int, cv::Mat> > blendingGains;
1921 std::pair<float, float> contrastValues(0,0);
1924 std::map<int, cv::Mat>(),
1925 std::map<
int, std::vector<rtabmap::CameraModel> >(),
1929 multiband?1:textureCount,
1931 gainValue>0.0f, gainValue, doGainCompensationRGB,
1933 lowBrightnessGain, highBrightnessGain,
1940 printf(
"Merging to %d texture(s)... done (%fs).\n", (
int)textureMesh->tex_materials.size(),
timer.ticks());
1944 printf(
"Saving texture mesh in db...\n");
1948 textureMesh->tex_coordinates,
1950 printf(
"Saving texture mesh in db... done!\n");
1955 bool success =
false;
1957 for(
size_t i=0;
i<textureMesh->tex_materials.size(); ++
i)
1959 textureMesh->tex_materials[
i].tex_file +=
".jpg";
1960 printf(
"Saving texture to %s.\n", textureMesh->tex_materials[
i].tex_file.c_str());
1961 UASSERT(textures.cols % textures.rows == 0);
1962 success = cv::imwrite(outputDirectory+
"/"+textureMesh->tex_materials[
i].tex_file, cv::Mat(textures,
cv::Range::all(), cv::Range(textures.rows*
i, textures.rows*(
i+1))));
1965 UERROR(
"Failed saving %s!", textureMesh->tex_materials[
i].tex_file.c_str());
1969 printf(
"Saved %s.\n", textureMesh->tex_materials[
i].tex_file.c_str());
1975 std::string outputPath=outputDirectory+
"/"+baseName+
"_mesh.obj";
1976 printf(
"Saving obj (%d vertices) to %s.\n", (
int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
1977 success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
1981 printf(
"Saved obj to %s!\n", outputPath.c_str());
1985 UERROR(
"Failed saving obj to %s!", outputPath.c_str());
1993 std::string outputPath=outputDirectory+
"/"+baseName+
"_mesh_multiband.obj";
1994 printf(
"MultiBand texturing (size=%d, downscale=%d, unwrap method=%s, fill holes=%s, padding=%d, best score thr=%f, angle thr=%f, force visible=%s)... \"%s\"\n",
1997 multibandUnwrap==1?
"ABF":multibandUnwrap==2?
"LSCM":
"Basic",
1998 multibandFillHoles?
"true":
"false",
2000 multibandBestScoreThr,
2001 multibandAngleHardthr,
2002 multibandForceVisible?
"false":
"true",
2003 outputPath.c_str());
2006 textureMesh->tex_polygons[0],
2009 std::map<int, cv::Mat >(),
2010 std::map<
int, std::vector<CameraModel> >(),
2020 doGainCompensationRGB,
2024 multibandBestScoreThr,
2025 multibandAngleHardthr,
2026 multibandForceVisible))
2028 printf(
"MultiBand texturing...done (%fs).\n",
timer.ticks());
2032 printf(
"MultiBand texturing...failed! (%fs)\n",
timer.ticks());
2042 printf(
"Export failed! The cloud is empty.\n");