tools/Export/main.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/DBDriver.h>
29 #include <rtabmap/core/Rtabmap.h>
30 #include <rtabmap/core/util3d.h>
31 #include <rtabmap/core/util3d_filtering.h>
32 #include <rtabmap/core/util3d_transforms.h>
33 #include <rtabmap/core/util3d_surface.h>
34 #include <rtabmap/core/util2d.h>
35 #include <rtabmap/core/optimizer/OptimizerG2O.h>
36 #include <rtabmap/core/Graph.h>
37 #include <rtabmap/utilite/UMath.h>
38 #include <rtabmap/utilite/UTimer.h>
39 #include <rtabmap/utilite/UFile.h>
40 #include <rtabmap/utilite/UStl.h>
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>
46 #include <stdio.h>
47 
48 #ifdef RTABMAP_PDAL
49 #include <rtabmap/core/PDALWriter.h>
50 #endif
51 
52 #ifdef RTABMAP_LIBLAS
53 #include <rtabmap/core/LASWriter.h>
54 #endif
55 
56 using namespace rtabmap;
57 
58 void showUsage()
59 {
60  printf("\nUsage:\n"
61  "rtabmap-export [options] database.db\n"
62  " New version: we now have to explicitly set what we want to export to give a more fine-grained control.\n"
63  " At least one of these options is required:\n"
64  " --cloud (old version used this by default)\n"
65  " --mesh\n"
66  " --poses\n"
67  " --poses_camera\n"
68  " --poses_scan\n"
69  " --poses_gps\n"
70  " --poses_gt\n"
71  " --gps\n"
72  " --images\n"
73  " --images_id\n"
74  "Options:\n"
75  " --output \"\" Output name (default: name of the database is used).\n"
76  " --output_dir \"\" Output directory (default: same directory than the database).\n"
77  " --ascii Export PLY in ascii format.\n"
78  " --las Export cloud in LAS instead of PLY (PDAL or libLAS dependency required).\n"
79  " --cloud Export assembled cloud.\n"
80  " --mesh Export assembled mesh.\n"
81  " --texture Texture the mesh. Used with --mesh option.\n"
82  " --texture_size # Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n"
83  " --texture_count # Maximum textures generated (default 1). Ignored by --multiband option (adjust --multiband_contrib instead).\n"
84  " --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
85  " --texture_angle # Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
86  " --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face\n"
87  " (-1=disabled, 0=edge length is used, default=0).\n"
88  " --texture_color_policy # How vertices are colored when texturing: \n"
89  " 0=colorless (standard OBJ), \n"
90  " 1=keep color only on textureless polygons, \n"
91  " 2=keep all color.\n"
92  " --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format\n"
93  " is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%%\n"
94  " of the image bottom not used).\n"
95  " --texture_d2c Distance to camera policy.\n"
96  " --texture_blur # Motion blur threshold (default 0: disabled). Below this threshold, the image is\n"
97  " considered blurred. 0 means disabled. 50 can be good default.\n"
98  " --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
99  " --cam_projection_keep_all Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
100  " --cam_projection_decimation Decimate images before projecting the points.\n"
101  " --cam_projection_mask \"\" File path for a mask. Format should be 8-bits grayscale. The mask should\n"
102  " cover all cameras in case multi-camera is used and have the same resolution.\n"
103  " --opt # Optimization approach:\n"
104  " 0=Full Global Optimization (default)\n"
105  " 1=Iterative Global Optimization\n"
106  " 2=Use optimized poses already computed in the database instead\n"
107  " of re-computing them (fallback to default if optimized poses don't exist).\n"
108  " 3=No optimization, use odometry poses directly.\n"
109  " --poses Export optimized poses of the robot frame (e.g., base_link).\n"
110  " --poses_camera Export optimized poses of the camera frame (e.g., optical frame).\n"
111  " --poses_scan Export optimized poses of the scan frame.\n"
112  " --poses_gt Export ground truth poses of the robot frame (e.g., base_link).\n"
113  " --poses_gps Export GPS poses of the GPS frame in local coordinates.\n"
114  " --poses_format # Format used for exported poses (default is 11):\n"
115  " 0=Raw 3x4 transformation matrix (r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz)\n"
116  " 1=RGBD-SLAM (in motion capture coordinate frame)\n"
117  " 2=KITTI (same as raw but in optical frame)\n"
118  " 3=TORO\n"
119  " 4=g2o\n"
120  " 10=RGBD-SLAM in ROS coordinate frame (stamp x y z qx qy qz qw)\n"
121  " 11=RGBD-SLAM in ROS coordinate frame + ID (stamp x y z qx qy qz qw id)\n"
122  " --gps # Export GPS values of the GPS frame in world coordinates. Formats:\n"
123  " 0=Raw (stamp longitude latitude altitude error bearing)\n"
124  " 1=KML (Google Earth)\n"
125  " --images Export images with stamp as file name.\n"
126  " --images_id Export images with node id as file name.\n"
127  " --ba Do global bundle adjustment before assembling the clouds.\n"
128  " --gain # Gain compensation value (default 1, set 0 to disable).\n"
129  " --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n"
130  " --no_blending Disable blending when texturing.\n"
131  " --no_clean Disable cleaning colorless polygons.\n"
132  " --min_cluster # When meshing, filter clusters of polygons with size less than this\n"
133  " threshold (default 200, -1 means keep only biggest contiguous surface).\n"
134  " --low_gain # Low brightness gain 0-100 (default 0).\n"
135  " --high_gain # High brightness gain 0-100 (default 10).\n"
136  " --multiband Enable multiband texturing (AliceVision dependency required). Used with --texture option.\n"
137  " --multiband_downscale # Downscaling reduce the texture quality but speed up the computation time (default 2).\n"
138  " --multiband_contrib \"# # # # \" Number of contributions per frequency band for the\n"
139  " multi-band blending, should be 4 values! (default \"1 5 10 0\").\n"
140  " --multiband_unwrap # Method to unwrap input mesh:\n"
141  " 0=basic (default, >600k faces, fast)\n"
142  " 1=ABF (<=300k faces, generate 1 atlas)\n"
143  " 2=LSCM (<=600k faces, optimize space).\n"
144  " --multiband_fillholes Fill Texture holes with plausible values.\n"
145  " --multiband_padding # Texture edge padding size in pixel (0-100) (default 5).\n"
146  " --multiband_scorethr # 0 to disable filtering based on threshold to relative best score (0.0-1.0). (default 0.1).\n"
147  " --multiband_anglethr # 0 to disable angle hard threshold filtering (0.0, 180.0) (default 90.0).\n"
148  " --multiband_forcevisible Triangle visibility is based on the union of vertices visibility.\n"
149  " --poisson_depth # Set Poisson depth for mesh reconstruction.\n"
150  " --poisson_size # Set target polygon size when computing Poisson's depth for mesh reconstruction (default 0.03 m).\n"
151  " --max_polygons # Maximum polygons when creating a mesh (default 300000, set 0 for no limit).\n"
152  " --min_range # Minimum range of the created clouds (default 0 m).\n"
153  " --max_range # Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
154  " --decimation # Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
155  " --voxel # Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
156  " --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"
157  " --noise_radius # Noise filtering search radius (default 0, 0=disabled).\n"
158  " --noise_k # Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
159  " --prop_radius_factor # Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
160  " --prop_radius_scale # Proportional radius filter neighbor scale (default 2).\n"
161  " --random_samples # Number of output samples using a random filter (default 0, 0=disabled).\n"
162  " --color_radius # Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
163  " --scan Use laser scan for the point cloud.\n"
164  " --save_in_db Save resulting assembled point cloud or mesh in the database.\n"
165  " --xmin # Minimum range on X axis to keep nodes to export.\n"
166  " --xmax # Maximum range on X axis to keep nodes to export.\n"
167  " --ymin # Minimum range on Y axis to keep nodes to export.\n"
168  " --ymax # Maximum range on Y axis to keep nodes to export.\n"
169  " --zmin # Minimum range on Z axis to keep nodes to export.\n"
170  " --zmax # Maximum range on Z axis to keep nodes to export.\n"
171  " --density_radius # Filter poses in a fixed radius (m) to keep only one to be exported in the assembled cloud.\n"
172  " --density_angle # Filter poses up to angle (deg) in the --density_radius.\n"
173  " --filter_ceiling # Filter points over a custom height (default 0 m, 0=disabled).\n"
174  " --filter_floor # Filter points below a custom height (default 0 m, 0=disabled).\n"
175 
176  "\n%s", Parameters::showUsage());
177  ;
178  exit(1);
179 }
180 
181 class ConsoleProgessState : public ProgressState
182 {
183  virtual bool callback(const std::string & msg) const
184  {
185  if(!msg.empty())
186  printf("%s\n", msg.c_str());
187  return true;
188  }
189 };
190 
191 int main(int argc, char * argv[])
192 {
193  ULogger::setType(ULogger::kTypeConsole);
194  ULogger::setLevel(ULogger::kError);
195 
196  if(argc < 2)
197  {
198  showUsage();
199  }
200 
201  bool binary = true;
202  bool las = false;
203  bool exportCloud = false;
204  bool exportMesh = false;
205  bool texture = false;
206  bool ba = false;
207  bool doGainCompensationRGB = true;
208  float gainValue = 1;
209  bool doBlending = true;
210  bool doClean = true;
211  int minCluster = 200;
212  int poissonDepth = 0;
213  float poissonSize = 0.03;
214  int maxPolygons = 300000;
215  int decimation = -1;
216  float minRange = 0.0f;
217  float maxRange = -1.0f;
218  float voxelSize = -1.0f;
219  float groundNormalsUp = 0.0f;
220  float noiseRadius = 0.0f;
221  int noiseMinNeighbors = 5;
222  float proportionalRadiusFactor = 0.0f;
223  float proportionalRadiusScale = 2.0f;
224  int randomSamples = 0;
225  int textureSize = 8192;
226  int textureCount = 1;
227  float textureRange = 0;
228  float textureAngle = 0;
229  float textureDepthError = 0;
230  std::vector<float> textureRoiRatios;
231  bool distanceToCamPolicy = false;
232  int laplacianThr = 0;
233  bool multiband = false;
234  int multibandDownScale = 2;
235  std::string multibandNbContrib = "1 5 10 0";
236  int multibandUnwrap = 0;
237  bool multibandFillHoles = false;
238  int multibandPadding = 5;
239  double multibandBestScoreThr = 0.1;
240  double multibandAngleHardthr = 90;
241  bool multibandForceVisible = false;
242  float colorRadius = -1.0f;
243  int textureVertexColorPolicy = 0;
244  bool cloudFromScan = false;
245  bool saveInDb = false;
246  int lowBrightnessGain = 0;
247  int highBrightnessGain = 10;
248  bool camProjection = false;
249  bool camProjectionKeepAll = false;
250  int cameraProjDecimation = 1;
251  std::string cameraProjMask;
252  bool exportPoses = false;
253  bool exportPosesCamera = false;
254  bool exportPosesScan = false;
255  bool exportPosesGt = false;
256  bool exportPosesGps = false;
257  int exportPosesFormat = 11;
258  int exportGps = -1;
259  bool exportImages = false;
260  bool exportImagesId = false;
261  int optimizationApproach = 0;
262  std::string outputName;
263  std::string outputDir;
264  cv::Vec3f min, max;
265  float densityRadius = 0.0f;
266  float densityAngle = 0.0f;
267  float filter_ceiling = 0.0f;
268  float filter_floor = 0.0f;
269  for(int i=1; i<argc; ++i)
270  {
271  if(std::strcmp(argv[i], "--help") == 0)
272  {
273  showUsage();
274  }
275  else if(std::strcmp(argv[i], "--output") == 0)
276  {
277  ++i;
278  if(i<argc-1)
279  {
280  outputName = argv[i];
281  }
282  else
283  {
284  showUsage();
285  }
286  }
287  else if(std::strcmp(argv[i], "--output_dir") == 0)
288  {
289  ++i;
290  if(i<argc-1)
291  {
292  outputDir = argv[i];
293  }
294  else
295  {
296  showUsage();
297  }
298  }
299  else if(std::strcmp(argv[i], "--bin") == 0)
300  {
301  printf("No need to set --bin anymore, ply are now automatically exported in binary by default. Set --ascii to export as text.\n");
302  }
303  else if(std::strcmp(argv[i], "--ascii") == 0)
304  {
305  binary = false;
306  }
307  else if(std::strcmp(argv[i], "--las") == 0)
308  {
309 #ifdef RTABMAP_PDAL
310  las = true;
311 #elif defined(RTABMAP_LIBLAS)
312  printf("\"--las\" option cannot be used with libLAS because the cloud has normals, build RTAB-Map with PDAL support to export in las with normals. Will export in PLY...\n");
313 #else
314  printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n");
315 #endif
316 
317  }
318  else if(std::strcmp(argv[i], "--cloud") == 0)
319  {
320  exportCloud = true;
321  }
322  else if(std::strcmp(argv[i], "--mesh") == 0)
323  {
324  exportMesh = true;
325  }
326  else if(std::strcmp(argv[i], "--texture") == 0)
327  {
328  texture = true;
329  }
330  else if(std::strcmp(argv[i], "--texture_size") == 0)
331  {
332  ++i;
333  if(i<argc-1)
334  {
335  textureSize = uStr2Int(argv[i]);
336  UASSERT(textureSize%256==0);
337  }
338  else
339  {
340  showUsage();
341  }
342  }
343  else if(std::strcmp(argv[i], "--texture_count") == 0)
344  {
345  ++i;
346  if(i<argc-1)
347  {
348  textureCount = uStr2Int(argv[i]);
349  }
350  else
351  {
352  showUsage();
353  }
354  }
355  else if(std::strcmp(argv[i], "--texture_range") == 0)
356  {
357  ++i;
358  if(i<argc-1)
359  {
360  textureRange = uStr2Float(argv[i]);
361  }
362  else
363  {
364  showUsage();
365  }
366  }
367  else if(std::strcmp(argv[i], "--texture_angle") == 0)
368  {
369  ++i;
370  if(i<argc-1)
371  {
372  textureAngle = uStr2Float(argv[i])*M_PI/180.0f;
373  }
374  else
375  {
376  showUsage();
377  }
378  }
379  else if(std::strcmp(argv[i], "--texture_depth_error") == 0)
380  {
381  ++i;
382  if(i<argc-1)
383  {
384  textureDepthError = uStr2Float(argv[i]);
385  }
386  else
387  {
388  showUsage();
389  }
390  }
391  else if(std::strcmp(argv[i], "--texture_roi_ratios") == 0)
392  {
393  ++i;
394  if(i<argc-1)
395  {
396  std::list<std::string> strValues = uSplit(argv[i], ' ');
397  if(strValues.size() != 4)
398  {
399  printf("The number of values must be 4 (roi=\"%s\")\n", argv[i]);
400  showUsage();
401  }
402  else
403  {
404  std::vector<float> tmpValues(4);
405  unsigned int i=0;
406  for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
407  {
408  tmpValues[i] = uStr2Float(*jter);
409  ++i;
410  }
411 
412  if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
413  tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
414  tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
415  tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
416  {
417  textureRoiRatios = tmpValues;
418  }
419  else
420  {
421  printf("The roi ratios are not valid (roi=\"%s\")\n", argv[i]);
422  showUsage();
423  }
424  }
425  }
426  else
427  {
428  showUsage();
429  }
430  }
431  else if(std::strcmp(argv[i], "--texture_d2c") == 0)
432  {
433  distanceToCamPolicy = true;
434  }
435  else if(std::strcmp(argv[i], "--texture_blur") == 0)
436  {
437  ++i;
438  if(i<argc-1)
439  {
440  laplacianThr = uStr2Int(argv[i]);
441  }
442  else
443  {
444  showUsage();
445  }
446  }
447  else if(std::strcmp(argv[i], "--cam_projection") == 0)
448  {
449  camProjection = true;
450  }
451  else if(std::strcmp(argv[i], "--cam_projection_keep_all") == 0)
452  {
453  camProjectionKeepAll = true;
454  }
455  else if(std::strcmp(argv[i], "--cam_projection_decimation") == 0)
456  {
457  ++i;
458  if(i<argc-1)
459  {
460  cameraProjDecimation = uStr2Int(argv[i]);
461  if(cameraProjDecimation<1)
462  {
463  printf("--cam_projection_decimation cannot be <1! value=\"%s\"\n", argv[i]);
464  showUsage();
465  }
466  }
467  else
468  {
469  showUsage();
470  }
471  }
472  else if(std::strcmp(argv[i], "--cam_projection_mask") == 0)
473  {
474  ++i;
475  if(i<argc-1)
476  {
477  cameraProjMask = argv[i];
478  if(!UFile::exists(cameraProjMask))
479  {
480  printf("--cam_projection_mask is set with a file not existing or don't have permissions to open it. Path=\"%s\"\n", argv[i]);
481  showUsage();
482  }
483  }
484  else
485  {
486  showUsage();
487  }
488  }
489  else if(std::strcmp(argv[i], "--poses") == 0)
490  {
491  exportPoses = true;
492  }
493  else if(std::strcmp(argv[i], "--poses_camera") == 0)
494  {
495  exportPosesCamera = true;
496  }
497  else if(std::strcmp(argv[i], "--poses_scan") == 0)
498  {
499  exportPosesScan = true;
500  }
501  else if(std::strcmp(argv[i], "--poses_gt") == 0)
502  {
503  exportPosesGt = true;
504  }
505  else if(std::strcmp(argv[i], "--poses_gps") == 0)
506  {
507  exportPosesGps = true;
508  }
509  else if(std::strcmp(argv[i], "--poses_format") == 0)
510  {
511  ++i;
512  if(i<argc-1)
513  {
514  exportPosesFormat = uStr2Int(argv[i]);
515  }
516  else
517  {
518  showUsage();
519  }
520  }
521  else if(std::strcmp(argv[i], "--gps") == 0)
522  {
523  ++i;
524  if(i<argc-1)
525  {
526  exportGps = uStr2Int(argv[i]);
527  if(exportGps<0 || exportGps>1)
528  {
529  printf("Wrong GPS format (%d), should be 0 or 1\n", exportGps);
530  showUsage();
531  }
532  }
533  else
534  {
535  showUsage();
536  }
537  }
538  else if(std::strcmp(argv[i], "--opt") == 0)
539  {
540  ++i;
541  if(i<argc-1)
542  {
543  optimizationApproach = uStr2Int(argv[i]);
544  if(optimizationApproach<0 || optimizationApproach >3)
545  {
546  printf("Invalid --opt (%d)\n", optimizationApproach);
547  showUsage();
548  }
549  }
550  else
551  {
552  showUsage();
553  }
554  }
555  else if(std::strcmp(argv[i], "--images") == 0)
556  {
557  exportImages = true;
558  }
559  else if(std::strcmp(argv[i], "--images_id") == 0)
560  {
561  exportImages = true;
562  exportImagesId = true;
563  }
564  else if(std::strcmp(argv[i], "--ba") == 0)
565  {
566  ba = true;
567  }
568  else if(std::strcmp(argv[i], "--gain_gray") == 0)
569  {
570  doGainCompensationRGB = false;
571  }
572  else if(std::strcmp(argv[i], "--gain") == 0)
573  {
574  ++i;
575  if(i<argc-1)
576  {
577  gainValue = uStr2Float(argv[i]);
578  UASSERT(gainValue>=0.0f);
579  }
580  else
581  {
582  showUsage();
583  }
584  }
585  else if(std::strcmp(argv[i], "--no_blending") == 0)
586  {
587  doBlending = false;
588  }
589  else if(std::strcmp(argv[i], "--no_clean") == 0)
590  {
591  doClean = false;
592  }
593  else if(std::strcmp(argv[i], "--min_cluster") == 0)
594  {
595  ++i;
596  if(i<argc-1)
597  {
598  minCluster = uStr2Int(argv[i]);
599  }
600  else
601  {
602  showUsage();
603  }
604  }
605  else if(std::strcmp(argv[i], "--multiband") == 0)
606  {
607 #ifdef RTABMAP_ALICE_VISION
608  multiband = true;
609 #else
610  printf("\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
611 #endif
612  }
613  else if(std::strcmp(argv[i], "--multiband_fillholes") == 0)
614  {
615  multibandFillHoles = true;
616  }
617  else if(std::strcmp(argv[i], "--multiband_downscale") == 0)
618  {
619  ++i;
620  if(i<argc-1)
621  {
622  multibandDownScale = uStr2Int(argv[i]);
623  }
624  else
625  {
626  showUsage();
627  }
628  }
629  else if(std::strcmp(argv[i], "--multiband_contrib") == 0)
630  {
631  ++i;
632  if(i<argc-1)
633  {
634  if(uSplit(argv[i], ' ').size() != 4)
635  {
636  printf("--multiband_contrib has wrong format! value=\"%s\"\n", argv[i]);
637  showUsage();
638  }
639  multibandNbContrib = argv[i];
640  }
641  else
642  {
643  showUsage();
644  }
645  }
646  else if(std::strcmp(argv[i], "--multiband_unwrap") == 0)
647  {
648  ++i;
649  if(i<argc-1)
650  {
651  multibandUnwrap = uStr2Int(argv[i]);
652  }
653  else
654  {
655  showUsage();
656  }
657  }
658  else if(std::strcmp(argv[i], "--multiband_padding") == 0)
659  {
660  ++i;
661  if(i<argc-1)
662  {
663  multibandPadding = uStr2Int(argv[i]);
664  }
665  else
666  {
667  showUsage();
668  }
669  }
670  else if(std::strcmp(argv[i], "--multiband_forcevisible") == 0)
671  {
672  multibandForceVisible = true;
673  }
674  else if(std::strcmp(argv[i], "--multiband_scorethr") == 0)
675  {
676  ++i;
677  if(i<argc-1)
678  {
679  multibandBestScoreThr = uStr2Float(argv[i]);
680  }
681  else
682  {
683  showUsage();
684  }
685  }
686  else if(std::strcmp(argv[i], "--multiband_anglethr") == 0)
687  {
688  ++i;
689  if(i<argc-1)
690  {
691  multibandAngleHardthr = uStr2Float(argv[i]);
692  }
693  else
694  {
695  showUsage();
696  }
697  }
698  else if(std::strcmp(argv[i], "--poisson_depth") == 0)
699  {
700  ++i;
701  if(i<argc-1)
702  {
703  poissonDepth = uStr2Int(argv[i]);
704  }
705  else
706  {
707  showUsage();
708  }
709  }
710  else if(std::strcmp(argv[i], "--poisson_size") == 0)
711  {
712  ++i;
713  if(i<argc-1)
714  {
715  poissonSize = uStr2Float(argv[i]);
716  }
717  else
718  {
719  showUsage();
720  }
721  }
722  else if(std::strcmp(argv[i], "--max_polygons") == 0)
723  {
724  ++i;
725  if(i<argc-1)
726  {
727  maxPolygons = uStr2Int(argv[i]);
728  }
729  else
730  {
731  showUsage();
732  }
733  }
734  else if(std::strcmp(argv[i], "--min_range") == 0)
735  {
736  ++i;
737  if(i<argc-1)
738  {
739  minRange = uStr2Float(argv[i]);
740  }
741  else
742  {
743  showUsage();
744  }
745  }
746  else if(std::strcmp(argv[i], "--max_range") == 0)
747  {
748  ++i;
749  if(i<argc-1)
750  {
751  maxRange = uStr2Float(argv[i]);
752  }
753  else
754  {
755  showUsage();
756  }
757  }
758  else if(std::strcmp(argv[i], "--decimation") == 0)
759  {
760  ++i;
761  if(i<argc-1)
762  {
763  decimation = uStr2Int(argv[i]);
764  }
765  else
766  {
767  showUsage();
768  }
769  }
770  else if(std::strcmp(argv[i], "--voxel") == 0)
771  {
772  ++i;
773  if(i<argc-1)
774  {
775  voxelSize = uStr2Float(argv[i]);
776  }
777  else
778  {
779  showUsage();
780  }
781  }
782  else if(std::strcmp(argv[i], "--ground_normals_up") == 0)
783  {
784  ++i;
785  if(i<argc-1)
786  {
787  groundNormalsUp = uStr2Float(argv[i]);
788  }
789  else
790  {
791  showUsage();
792  }
793  }
794  else if(std::strcmp(argv[i], "--noise_radius") == 0)
795  {
796  ++i;
797  if(i<argc-1)
798  {
799  noiseRadius = uStr2Float(argv[i]);
800  }
801  else
802  {
803  showUsage();
804  }
805  }
806  else if(std::strcmp(argv[i], "--noise_k") == 0)
807  {
808  ++i;
809  if(i<argc-1)
810  {
811  noiseMinNeighbors = uStr2Int(argv[i]);
812  }
813  else
814  {
815  showUsage();
816  }
817  }
818  else if(std::strcmp(argv[i], "--prop_radius_factor") == 0)
819  {
820  ++i;
821  if(i<argc-1)
822  {
823  proportionalRadiusFactor = uStr2Float(argv[i]);
824  }
825  else
826  {
827  showUsage();
828  }
829  }
830  else if(std::strcmp(argv[i], "--prop_radius_scale") == 0)
831  {
832  ++i;
833  if(i<argc-1)
834  {
835  proportionalRadiusScale = uStr2Float(argv[i]);
836  UASSERT_MSG(proportionalRadiusScale>=1.0f, "--prop_radius_scale should be >= 1.0");
837  }
838  else
839  {
840  showUsage();
841  }
842  }
843  else if(std::strcmp(argv[i], "--random_samples") == 0)
844  {
845  ++i;
846  if(i<argc-1)
847  {
848  randomSamples = uStr2Int(argv[i]);
849  }
850  else
851  {
852  showUsage();
853  }
854  }
855  else if(std::strcmp(argv[i], "--color_radius") == 0)
856  {
857  ++i;
858  if(i<argc-1)
859  {
860  colorRadius = uStr2Float(argv[i]);
861  }
862  else
863  {
864  showUsage();
865  }
866  }
867  else if(std::strcmp(argv[i], "--texture_color_policy") == 0)
868  {
869  ++i;
870  if(i<argc-1)
871  {
872  textureVertexColorPolicy = uStr2Int(argv[i]);
873  }
874  else
875  {
876  showUsage();
877  }
878  }
879  else if(std::strcmp(argv[i], "--scan") == 0)
880  {
881  cloudFromScan = true;
882  }
883  else if(std::strcmp(argv[i], "--save_in_db") == 0)
884  {
885  saveInDb = true;
886  }
887  else if(std::strcmp(argv[i], "--low_gain") == 0)
888  {
889  ++i;
890  if(i<argc-1)
891  {
892  lowBrightnessGain = uStr2Int(argv[i]);
893  }
894  else
895  {
896  showUsage();
897  }
898  }
899  else if(std::strcmp(argv[i], "--high_gain") == 0)
900  {
901  ++i;
902  if(i<argc-1)
903  {
904  highBrightnessGain = uStr2Int(argv[i]);
905  }
906  else
907  {
908  showUsage();
909  }
910  }
911  else if(std::strcmp(argv[i], "--xmin") == 0)
912  {
913  ++i;
914  if(i<argc-1)
915  {
916  min[0] = uStr2Float(argv[i]);
917  }
918  else
919  {
920  showUsage();
921  }
922  }
923  else if(std::strcmp(argv[i], "--xmax") == 0)
924  {
925  ++i;
926  if(i<argc-1)
927  {
928  max[0] = uStr2Float(argv[i]);
929  }
930  else
931  {
932  showUsage();
933  }
934  }
935  else if(std::strcmp(argv[i], "--ymin") == 0)
936  {
937  ++i;
938  if(i<argc-1)
939  {
940  min[1] = uStr2Float(argv[i]);
941  }
942  else
943  {
944  showUsage();
945  }
946  }
947  else if(std::strcmp(argv[i], "--ymax") == 0)
948  {
949  ++i;
950  if(i<argc-1)
951  {
952  max[1] = uStr2Float(argv[i]);
953  }
954  else
955  {
956  showUsage();
957  }
958  }
959  else if(std::strcmp(argv[i], "--zmin") == 0)
960  {
961  ++i;
962  if(i<argc-1)
963  {
964  min[2] = uStr2Float(argv[i]);
965  }
966  else
967  {
968  showUsage();
969  }
970  }
971  else if(std::strcmp(argv[i], "--zmax") == 0)
972  {
973  ++i;
974  if(i<argc-1)
975  {
976  max[2] = uStr2Float(argv[i]);
977  }
978  else
979  {
980  showUsage();
981  }
982  }
983  else if(std::strcmp(argv[i], "--density_radius") == 0)
984  {
985  ++i;
986  if(i<argc-1)
987  {
988  densityRadius = uStr2Float(argv[i]);
989  UASSERT(densityRadius>=0.0f);
990  }
991  else
992  {
993  showUsage();
994  }
995  }
996  else if(std::strcmp(argv[i], "--density_angle") == 0)
997  {
998  ++i;
999  if(i<argc-1)
1000  {
1001  densityAngle = uStr2Float(argv[i]);
1002  UASSERT(densityAngle>=0.0f);
1003  }
1004  else
1005  {
1006  showUsage();
1007  }
1008  }
1009  else if(std::strcmp(argv[i], "--filter_ceiling") == 0)
1010  {
1011  ++i;
1012  if(i<argc-1)
1013  {
1014  filter_ceiling = uStr2Float(argv[i]);
1015  if(filter_floor!=0.0f && filter_ceiling != 0.0f && filter_ceiling<filter_floor)
1016  {
1017  printf("Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
1018  showUsage();
1019  }
1020  }
1021  else
1022  {
1023  showUsage();
1024  }
1025  }
1026  else if(std::strcmp(argv[i], "--filter_floor") == 0)
1027  {
1028  ++i;
1029  if(i<argc-1)
1030  {
1031  filter_floor = uStr2Float(argv[i]);
1032  if(filter_floor!=0.0f && filter_ceiling != 0.0f && filter_ceiling<filter_floor)
1033  {
1034  printf("Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
1035  showUsage();
1036  }
1037  }
1038  else
1039  {
1040  showUsage();
1041  }
1042  }
1043  }
1044 
1045  if(decimation < 1)
1046  {
1047  decimation = cloudFromScan?1:4;
1048  }
1049  if(maxRange < 0)
1050  {
1051  maxRange = cloudFromScan?0:4;
1052  }
1053  if(voxelSize < 0.0f)
1054  {
1055  voxelSize = cloudFromScan?0:0.01f;
1056  }
1057  if(colorRadius < 0.0f)
1058  {
1059  colorRadius = cloudFromScan?0:0.05f;
1060  }
1061 
1062  if(saveInDb)
1063  {
1064  if(multiband)
1065  {
1066  printf("Option --multiband is not supported with --save_in_db option, disabling multiband...\n");
1067  multiband = false;
1068  }
1069  if(textureCount>1)
1070  {
1071  printf("Option --texture_count > 1 is not supported with --save_in_db option, setting texture_count to 1...\n");
1072  textureCount = 1;
1073  }
1074  }
1075 
1076  if(!(exportCloud ||
1077  exportMesh ||
1078  exportImages ||
1079  exportPoses ||
1080  exportPosesScan ||
1081  exportPosesCamera ||
1082  exportPosesGt ||
1083  exportPosesGps ||
1084  exportGps>=0 ||
1085  texture))
1086  {
1087  printf("Launching the tool without any required option(s) is deprecated. We will add --cloud to keep compatibilty with old behavior.\n");
1088  exportCloud = true;
1089  }
1090 
1091  if(texture && !exportMesh)
1092  {
1093  printf("To use --texture option, --mesh should be also enabled. Enabling --mesh.\n");
1094  exportMesh = true;
1095  }
1096 
1097  ParametersMap params = Parameters::parseArguments(argc, argv, false);
1098 
1099  std::string dbPath = argv[argc-1];
1100 
1101  if(!UFile::exists(dbPath))
1102  {
1103  UERROR("File \"%s\" doesn't exist!", dbPath.c_str());
1104  return -1;
1105  }
1106 
1107  // Get parameters
1108  ParametersMap parameters;
1109  DBDriver * driver = DBDriver::create();
1110  if(driver->openConnection(dbPath))
1111  {
1112  parameters = driver->getLastParameters();
1113  driver->closeConnection(false);
1114  }
1115  else
1116  {
1117  UERROR("Cannot open database %s!", dbPath.c_str());
1118  return -1;
1119  }
1120  delete driver;
1121  driver = 0;
1122 
1123  for(ParametersMap::iterator iter=params.begin(); iter!=params.end(); ++iter)
1124  {
1125  printf("Added custom parameter %s=%s\n",iter->first.c_str(), iter->second.c_str());
1126  }
1127 
1128  UTimer timer;
1129 
1130  printf("Opening database \"%s\"...\n", dbPath.c_str());
1131  uInsert(parameters, params);
1132  std::shared_ptr<DBDriver> dbDriver(DBDriver::create(parameters));
1133  if(!dbDriver->openConnection(dbPath))
1134  {
1135  printf("Failed to open database \"%s\"!\n", dbPath.c_str());
1136  return -1;
1137  }
1138  printf("Opening database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks());
1139 
1140  std::map<int, Transform> optimizedPoses;
1141  std::map<int, Transform> odomPoses;
1142  std::multimap<int, Link> links;
1143  dbDriver->getAllOdomPoses(odomPoses, true);
1144  dbDriver->getAllLinks(links, true, true);
1145  if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan))
1146  {
1147  // Just use odometry poses when exporting only images
1148  optimizedPoses = odomPoses;
1149  if(optimizationApproach == 3)
1150  {
1151  printf("Loaded %d odometry poses from database.\n", (int)odomPoses.size());
1152  }
1153  }
1154  else
1155  {
1156  if(optimizationApproach == 2)
1157  {
1158  printf("Loading optimized poses from database...\n");
1159  optimizedPoses = dbDriver->loadOptimizedPoses();
1160  if(optimizedPoses.empty())
1161  {
1162  printf("The are no saved optimized poses in the database, we will do full global optimization instead.\n");
1163  optimizationApproach = 0;
1164  }
1165  else
1166  {
1167  printf("Loading optimized poses from database... done (%d optimized poses loaded).\n", (int)optimizedPoses.size());
1168  }
1169  }
1170  if(optimizationApproach <= 1)
1171  {
1172  std::string optimizationApproachStr = optimizationApproach==1?"Iterative global optimization":"Full global optimization";
1173  printf("Optimizing the map (%s)...\n", optimizationApproachStr.c_str());
1174  if(odomPoses.empty())
1175  {
1176  printf("The are no odometry poses!? Aborting...\n");
1177  return -1;
1178  }
1179  std::shared_ptr<Optimizer> optimizer(Optimizer::create(parameters));
1180  std::map<int, Transform> posesOut;
1181  std::multimap<int, Link> linksOut;
1182  UASSERT(odomPoses.lower_bound(1) != odomPoses.end());
1183 
1184  // Add landmarks if there are some
1185  // Marker priors parameters
1186  double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
1187  double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
1188  std::map<int, Transform> markerPriors;
1189  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
1190  UASSERT(markerPriorsLinearVariance>0.0f);
1191  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
1192  UASSERT(markerPriorsAngularVariance>0.0f);
1193  std::string markerPriorsStr;
1194  if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
1195  {
1196  std::list<std::string> strList = uSplit(markerPriorsStr, '|');
1197  for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
1198  {
1199  std::string markerStr = *iter;
1200  while(!markerStr.empty() && !uIsDigit(markerStr[0]))
1201  {
1202  markerStr.erase(markerStr.begin());
1203  }
1204  if(!markerStr.empty())
1205  {
1206  std::string idStr = uSplitNumChar(markerStr).front();
1207  int id = uStr2Int(idStr);
1208  Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
1209  if(!prior.isNull() && id>0)
1210  {
1211  markerPriors.insert(std::make_pair(-id, prior));
1212  }
1213  else
1214  {
1215  UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
1216  }
1217  }
1218  else if(!iter->empty())
1219  {
1220  UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
1221  }
1222  }
1223  }
1224  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
1225  {
1226  if(iter->second.type() == Link::kLandmark)
1227  {
1228  UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
1229  int markerId = iter->second.to();
1230  if(odomPoses.find(iter->second.from()) != odomPoses.end() && odomPoses.find(markerId) == odomPoses.end())
1231  {
1232  odomPoses.insert(std::make_pair(markerId, odomPoses.at(iter->second.from())*iter->second.transform()));
1233  // add landmark priors if there are some
1234  if(markerPriors.find(markerId) != markerPriors.end())
1235  {
1236  cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
1237  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
1238  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
1239  links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
1240  printf("Added prior %d : %s (variance: lin=%f ang=%f)\n", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
1241  markerPriorsLinearVariance, markerPriorsAngularVariance);
1242  }
1243  }
1244  }
1245  }
1246 
1247 
1248  optimizer->getConnectedGraph(odomPoses.lower_bound(1)->first, odomPoses, links, posesOut, linksOut);
1249  if(optimizationApproach == 1)
1250  {
1251  std::list<std::map<int, Transform> > intermediateGraphes;
1252  optimizedPoses = optimizer->optimize(odomPoses.lower_bound(1)->first, posesOut, linksOut, &intermediateGraphes);
1253  }
1254  else
1255  {
1256  optimizedPoses = optimizer->optimize(odomPoses.lower_bound(1)->first, posesOut, linksOut);
1257  }
1258  printf("Optimizing the map (%s)... done (%fs, poses=%d, links=%d).\n", optimizationApproachStr.c_str(), timer.ticks(), (int)optimizedPoses.size(), (int)linksOut.size());
1259  }
1260 
1261  if(optimizedPoses.empty())
1262  {
1263  printf("The optimized graph is empty!? Aborting...\n");
1264  return -1;
1265  }
1266 
1267  if(min[0] != max[0] || min[1] != max[1] || min[2] != max[2])
1268  {
1269  cv::Vec3f minP,maxP;
1270  graph::computeMinMax(optimizedPoses, minP, maxP);
1271  printf("Filtering poses (range: x=%.1f<->%.1f, y=%.1f<->%.1f, z=%.1f<->%.1f, map size=%.1f x %.1f x %.1f, map min/max: [%.1f, %.1f, %.1f] [%.1f, %.1f, %.1f])...\n",
1272  min[0],max[0],min[1],max[1],min[2],max[2],
1273  maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2],
1274  minP[0],minP[1],minP[2],maxP[0],maxP[1],maxP[2]);
1275  std::map<int, Transform> posesFiltered;
1276  for(std::map<int, Transform>::const_iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
1277  {
1278  bool ignore = false;
1279  if(min[0] != max[0] && (iter->second.x() < min[0] || iter->second.x() > max[0]))
1280  {
1281  ignore = true;
1282  }
1283  if(min[1] != max[1] && (iter->second.y() < min[1] || iter->second.y() > max[1]))
1284  {
1285  ignore = true;
1286  }
1287  if(min[2] != max[2] && (iter->second.z() < min[2] || iter->second.z() > max[2]))
1288  {
1289  ignore = true;
1290  }
1291  if(!ignore)
1292  {
1293  posesFiltered.insert(*iter);
1294  }
1295  }
1296  graph::computeMinMax(posesFiltered, minP, maxP);
1297  printf("Filtering poses... done! %d/%d remaining.\n", (int)posesFiltered.size(), (int)optimizedPoses.size());
1298  optimizedPoses = posesFiltered;
1299  if(optimizedPoses.empty())
1300  {
1301  printf("All poses filtered! Exiting.\n");
1302  return -1;
1303  }
1304  }
1305 
1306  if(ba)
1307  {
1308  printf("Global bundle adjustment...\n");
1309  // TODO: these conversions could be simplified
1310  UASSERT(optimizedPoses.lower_bound(1) != optimizedPoses.end());
1311  OptimizerG2O g2o(parameters);
1312  std::list<int> ids;
1313  for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
1314  {
1315  ids.push_back(iter->first);
1316  }
1317  std::list<Signature *> signatures;
1318  dbDriver->loadSignatures(ids, signatures);
1319  std::map<int, Signature> nodes;
1320  for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
1321  {
1322  nodes.insert(std::make_pair((*iter)->id(), *(*iter)));
1323  }
1324  optimizedPoses = ((Optimizer*)&g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
1325  printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
1326  }
1327  }
1328 
1329  std::string outputDirectory = outputDir.empty()?UDirectory::getDir(dbPath):outputDir;
1330  if(!UDirectory::exists(outputDirectory))
1331  {
1332  UDirectory::makeDir(outputDirectory);
1333  }
1334  std::string baseName = outputName.empty()?uSplit(UFile::getName(dbPath), '.').front():outputName;
1335 
1336  // Construct the cloud
1337  if(exportCloud || exportMesh)
1338  {
1339  printf("Create and assemble the clouds...\n");
1340  }
1341  else if(exportImages || exportImagesId)
1342  {
1343  printf("Export images...\n");
1344  }
1345  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1346  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
1347  std::map<int, rtabmap::Transform> robotPoses;
1348  std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
1349  std::map<int, rtabmap::Transform> scanPoses;
1350  std::map<int, rtabmap::Transform> gtPoses;
1351  std::map<int, rtabmap::Transform> gpsPoses;
1352  std::map<int, double> gpsStamps;
1353  GPS gpsOrigin;
1354  std::map<int, rtabmap::GPS> gpsValues;
1355  std::map<int, double> cameraStamps;
1356  std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
1357  std::map<int, cv::Mat> cameraDepths;
1358  int imagesExported = 0;
1359  std::vector<int> rawViewpointIndices;
1360  std::map<int, Transform> rawViewpoints;
1361  std::map<int, Transform> densityPoses;
1362  if(densityRadius && (exportCloud || exportMesh))
1363  {
1364  densityPoses = graph::radiusPosesFiltering(optimizedPoses, densityRadius, densityAngle*CV_PI/180.0f);
1365  printf("Keeping %d/%d poses after density filtering (--density_radius = %f --density_angle = %f).\n",
1366  (int)densityPoses.size(),
1367  (int)optimizedPoses.size(),
1368  densityRadius,
1369  densityAngle);
1370  }
1371  int processedNodes = 0;
1372  int lastPercent = 0;
1373  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
1374  {
1375  if(iter->first<0)
1376  {
1377  // landmark, just add to list of poses
1378  robotPoses.insert(*iter);
1379  cameraStamps.insert(std::make_pair(iter->first, 0));
1380  continue;
1381  }
1382 
1383  Transform p, gt;
1384  int m;
1385  std::string l;
1386  GPS gps;
1387  std::vector<float> v;
1388  EnvSensors s;
1389  int weight = -1;
1390  double stamp = 0.0;
1391  dbDriver->getNodeInfo(iter->first, p, m, weight, l, stamp, gt, v, gps, s);
1392 
1393  SensorData data;
1394  bool loadImages = ((exportCloud || exportMesh) && (!cloudFromScan || texture || camProjection)) || exportImages;
1395  bool loadScan = ((exportCloud || exportMesh) && cloudFromScan) || exportPosesScan;
1396  if(loadImages || loadScan)
1397  {
1398  dbDriver->getNodeData(
1399  iter->first,
1400  data,
1401  loadImages,
1402  loadScan,
1403  false,
1404  false);
1405  }
1406 
1407  // uncompress data
1408  std::vector<CameraModel> models;
1409  std::vector<StereoCameraModel> stereoModels;
1410  if(loadImages || exportPosesCamera)
1411  {
1412  dbDriver->getCalibration(iter->first, models, stereoModels);
1413  }
1414 
1415  if(exportCloud || exportMesh || exportImages)
1416  {
1417  bool densityFiltered = !densityPoses.empty() && densityPoses.find(iter->first) == densityPoses.end();
1418  cv::Mat rgb;
1419  cv::Mat depth;
1420  pcl::IndicesPtr indices(new std::vector<int>);
1421  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1422  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
1423  if(weight != -1)
1424  {
1425  if(!densityFiltered && cloudFromScan && (exportCloud || exportMesh))
1426  {
1427  LaserScan scan;
1428  data.uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!data.depthOrRightCompressed().empty()?&depth:0, &scan);
1429  if(scan.empty())
1430  {
1431  printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first);
1432  }
1433  if(decimation>1 || minRange>0.0f || maxRange)
1434  {
1435  scan = util3d::commonFiltering(scan, decimation, minRange, maxRange);
1436  }
1437  if(scan.hasRGB())
1438  {
1439  cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
1440  if(noiseRadius>0.0f && noiseMinNeighbors>0)
1441  {
1442  indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors);
1443  }
1444  }
1445  else
1446  {
1447  cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
1448  if(noiseRadius>0.0f && noiseMinNeighbors>0)
1449  {
1450  indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors);
1451  }
1452  }
1453  }
1454  else
1455  {
1456  data.uncompressData(&rgb, &depth);
1457  if(!densityFiltered && (exportCloud || exportMesh))
1458  {
1459  if(depth.empty())
1460  {
1461  printf("Node %d doesn't have depth or stereo data, empty cloud is "
1462  "created (if you want to create point cloud from scan, use --scan option).\n", iter->first);
1463  }
1464  cloud = util3d::cloudRGBFromSensorData(
1465  data,
1466  decimation, // image decimation before creating the clouds
1467  maxRange, // maximum depth of the cloud
1468  minRange,
1469  indices.get());
1470  if(noiseRadius>0.0f && noiseMinNeighbors>0)
1471  {
1472  indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors);
1473  }
1474  }
1475  }
1476  }
1477 
1478  if(exportImages && !rgb.empty())
1479  {
1480  std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1 && !depth.empty())?"left":"rgb";
1481  std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix;
1482  if(!UDirectory::exists(dir)) {
1483  UDirectory::makeDir(dir);
1484  }
1485  std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".jpg";
1486  cv::imwrite(outputPath, rgb);
1487  ++imagesExported;
1488  if(!depth.empty())
1489  {
1490  std::string ext;
1491  cv::Mat depthExported = depth;
1492  if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1)
1493  {
1494  ext = ".jpg";
1495  dir = outputDirectory+"/"+baseName+"_right";
1496  }
1497  else
1498  {
1499  ext = ".png";
1500  dir = outputDirectory+"/"+baseName+"_depth";
1501  if(depth.type() == CV_32FC1)
1502  {
1503  depthExported = rtabmap::util2d::cvtDepthFromFloat(depth);
1504  }
1505  }
1506  if(!UDirectory::exists(dir)) {
1507  UDirectory::makeDir(dir);
1508  }
1509 
1510  outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+ext;
1511  cv::imwrite(outputPath, depthExported);
1512  }
1513 
1514  // save calibration per image (calibration can change over time, e.g. camera has auto focus)
1515  for(size_t i=0; i<models.size(); ++i)
1516  {
1517  CameraModel model = models[i];
1518  std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp));
1519  if(models.size() > 1) {
1520  modelName += "_" + uNumber2Str((int)i);
1521  }
1522  model.setName(modelName);
1523  std::string dir = outputDirectory+"/"+baseName+"_calib";
1524  if(!UDirectory::exists(dir)) {
1525  UDirectory::makeDir(dir);
1526  }
1527  model.save(dir);
1528  }
1529  for(size_t i=0; i<stereoModels.size(); ++i)
1530  {
1531  StereoCameraModel model = stereoModels[i];
1532  std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp));
1533  if(stereoModels.size() > 1) {
1534  modelName += "_" + uNumber2Str((int)i);
1535  }
1536  model.setName(modelName, "left", "right");
1537  std::string dir = outputDirectory+"/"+baseName+"_calib";
1538  if(!UDirectory::exists(dir)) {
1539  UDirectory::makeDir(dir);
1540  }
1541  model.save(dir);
1542  }
1543  }
1544 
1545  if(exportCloud || exportMesh)
1546  {
1547  if(voxelSize>0.0f)
1548  {
1549  if(cloud.get() && !cloud->empty())
1550  cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
1551  else if(cloudI.get() && !cloudI->empty())
1552  cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
1553  }
1554  if(cloud.get() && !cloud->empty())
1555  cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
1556  else if(cloudI.get() && !cloudI->empty())
1557  cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
1558 
1559  if(filter_ceiling != 0.0 || filter_floor != 0.0f)
1560  {
1561  if(cloud.get() && !cloud->empty())
1562  {
1563  cloud = util3d::passThrough(cloud, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
1564  }
1565  if(cloudI.get() && !cloudI->empty())
1566  {
1567  cloudI = util3d::passThrough(cloudI, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
1568  }
1569  }
1570 
1571  if(cloudFromScan)
1572  {
1573  Transform lidarViewpoint = iter->second * data.laserScanRaw().localTransform();
1574  rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
1575  }
1576  else if(!models.empty() && !models[0].localTransform().isNull())
1577  {
1578  Transform cameraViewpoint = iter->second * models[0].localTransform(); // take the first camera
1579  rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
1580  }
1581  else if(!stereoModels.empty() && !stereoModels[0].localTransform().isNull())
1582  {
1583  Transform cameraViewpoint = iter->second * stereoModels[0].localTransform();
1584  rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
1585  }
1586  else
1587  {
1588  rawViewpoints.insert(*iter);
1589  }
1590 
1591  if(cloud.get() && !cloud->empty())
1592  {
1593  if(assembledCloud->empty())
1594  {
1595  *assembledCloud = *cloud;
1596  }
1597  else
1598  {
1599  *assembledCloud += *cloud;
1600  }
1601  rawViewpointIndices.resize(assembledCloud->size(), iter->first);
1602  }
1603  else if(cloudI.get() && !cloudI->empty())
1604  {
1605  if(assembledCloudI->empty())
1606  {
1607  *assembledCloudI = *cloudI;
1608  }
1609  else
1610  {
1611  *assembledCloudI += *cloudI;
1612  }
1613  rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
1614  }
1615  if(texture && !depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
1616  {
1617  cameraDepths.insert(std::make_pair(iter->first, depth));
1618  }
1619  }
1620  }
1621 
1622  if(models.empty())
1623  {
1624  for(size_t i=0; i<stereoModels.size(); ++i)
1625  {
1626  models.push_back(stereoModels[i].left());
1627  }
1628  }
1629 
1630  robotPoses.insert(std::make_pair(iter->first, iter->second));
1631  cameraStamps.insert(std::make_pair(iter->first, stamp));
1632  if(models.empty() && weight == -1 && !cameraModels.empty())
1633  {
1634  // For intermediate nodes, use latest models
1635  models = cameraModels.rbegin()->second;
1636  }
1637  if(!models.empty())
1638  {
1639  if(!data.imageCompressed().empty())
1640  {
1641  cameraModels.insert(std::make_pair(iter->first, models));
1642  }
1643  if(exportPosesCamera)
1644  {
1645  if(cameraPoses.empty())
1646  {
1647  cameraPoses.resize(models.size());
1648  }
1649  UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
1650  for(size_t i=0; i<models.size(); ++i)
1651  {
1652  cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
1653  }
1654  }
1655  }
1656  if(exportPosesScan && !data.laserScanCompressed().empty())
1657  {
1658  scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
1659  }
1660 
1661  if(exportPosesGps || exportGps>=0)
1662  {
1663  if(gps.stamp() > 0.0)
1664  {
1665  if(exportPosesGps)
1666  {
1667  cv::Point3f p(0.0f,0.0f,0.0f);
1668  if(!gpsPoses.empty())
1669  {
1670  GeodeticCoords coords = gps.toGeodeticCoords();
1671  p = coords.toENU_WGS84(gpsOrigin.toGeodeticCoords());
1672  }
1673  else
1674  {
1675  gpsOrigin = gps;
1676  }
1677  Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
1678  gpsPoses.insert(std::make_pair(iter->first, pose));
1679  }
1680  if(exportGps>=0)
1681  {
1682  gpsValues.insert(std::make_pair(iter->first, gps));
1683  }
1684  gpsStamps.insert(std::make_pair(iter->first, gps.stamp()));
1685  }
1686  }
1687 
1688  if(exportPosesGt && !gt.isNull())
1689  {
1690  gtPoses.insert(std::make_pair(iter->first, gt));
1691  }
1692 
1693  if(optimizedPoses.size() >= 500)
1694  {
1695  ++processedNodes;
1696  int percent = processedNodes*100/(int)optimizedPoses.size();
1697  if(percent != lastPercent)
1698  {
1699  printf("Processed %d/%d (%d%%) nodes...\n",
1700  processedNodes,
1701  (int)optimizedPoses.size(),
1702  percent);
1703  lastPercent = percent;
1704  }
1705  }
1706  }
1707  if(exportCloud || exportMesh)
1708  {
1709  printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1710  }
1711 
1712  if(exportImages || exportImagesId)
1713  {
1714  printf("%d images exported!\n", imagesExported);
1715  if(!(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan)) {
1716  //images exported, early exit.
1717  return 0;
1718  }
1719  }
1720 
1721  ConsoleProgessState progressState;
1722 
1723  if(saveInDb)
1724  {
1725  driver = DBDriver::create();
1726  UASSERT(driver->openConnection(dbPath, false));
1727  Transform lastlocalizationPose;
1728  driver->loadOptimizedPoses(&lastlocalizationPose);
1729  //optimized poses have changed, reset 2d map
1730  driver->save2DMap(cv::Mat(), 0, 0, 0);
1731  driver->saveOptimizedPoses(robotPoses, lastlocalizationPose);
1732  cv::Vec3f vmin, vmax;
1733  graph::computeMinMax(robotPoses, vmin, vmax);
1734  printf("Saved %d poses to database! (min=[%f,%f,%f] max=[%f,%f,%f])\n",
1735  (int)robotPoses.size(),
1736  vmin[0], vmin[1], vmin[2],
1737  vmax[0], vmax[1], vmax[2]);
1738  }
1739  else
1740  {
1741  std::string posesExt = (exportPosesFormat==3?"toro":exportPosesFormat==4?"g2o":"txt");
1742  if(exportPoses)
1743  {
1744  std::string outputPath=outputDirectory+"/"+baseName+"_poses." + posesExt;
1745  if(robotPoses.begin() != robotPoses.lower_bound(1) && !(exportPosesFormat == 4 || exportPosesFormat == 11))
1746  {
1747  printf("Note that landmarks won't be exported because --poses_format is not 4 or 11 (currently using %d).\n", exportPosesFormat);
1748  rtabmap::graph::exportPoses(
1749  outputPath,
1750  exportPosesFormat,
1751  std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()),
1752  links,
1753  std::map<int, double>(cameraStamps.lower_bound(1), cameraStamps.end()));
1754  }
1755  else
1756  {
1757  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps);
1758  }
1759  cv::Vec3f vmin, vmax;
1760  graph::computeMinMax(robotPoses, vmin, vmax);
1761  printf("%d poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
1762  (int)robotPoses.size(),
1763  outputPath.c_str(),
1764  vmin[0], vmin[1], vmin[2],
1765  vmax[0], vmax[1], vmax[2]);
1766  }
1767  if(exportPosesCamera)
1768  {
1769  for(size_t i=0; i<cameraPoses.size(); ++i)
1770  {
1771  std::string outputPath;
1772  if(cameraPoses.size()==1)
1773  outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
1774  else
1775  outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
1776  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
1777  cv::Vec3f vmin, vmax;
1778  graph::computeMinMax(cameraPoses[i], vmin, vmax);
1779  printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
1780  (int)cameraPoses[i].size(),
1781  outputPath.c_str(),
1782  vmin[0], vmin[1], vmin[2],
1783  vmax[0], vmax[1], vmax[2]);
1784  }
1785  }
1786  if(exportPosesScan)
1787  {
1788  std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
1789  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), cameraStamps);
1790  cv::Vec3f min, max;
1791  graph::computeMinMax(scanPoses, min, max);
1792  printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
1793  (int)scanPoses.size(),
1794  outputPath.c_str(),
1795  min[0], min[1], min[2],
1796  max[0], max[1], max[2]);
1797  }
1798  if(exportPosesGps)
1799  {
1800  std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt;
1801  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap<int, Link>(), gpsStamps);
1802  printf("%d GPS poses exported to \"%s\".\n",
1803  (int)gpsPoses.size(),
1804  outputPath.c_str());
1805  }
1806  if(exportPosesGt)
1807  {
1808  std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt;
1809  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), cameraStamps);
1810  printf("%d scan poses exported to \"%s\".\n",
1811  (int)gtPoses.size(),
1812  outputPath.c_str());
1813  }
1814  if(exportGps>=0)
1815  {
1816  std::string outputPath=outputDirectory+"/"+baseName+"_gps." + (exportGps==0?"txt":"kml");
1817  rtabmap::graph::exportGPS(outputPath, gpsValues);
1818  printf("%d GPS values exported to \"%s\".\n",
1819  (int)gpsValues.size(),
1820  outputPath.c_str());
1821  }
1822  }
1823 
1824  if(!(exportCloud || exportMesh))
1825  {
1826  // poses exported, early exit
1827  return 0;
1828  }
1829 
1830  if(!assembledCloud->empty() || !assembledCloudI->empty())
1831  {
1832  if(proportionalRadiusFactor>0.0f && proportionalRadiusScale>=1.0f)
1833  {
1834  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());
1835  pcl::IndicesPtr indices;
1836  if(!assembledCloud->empty())
1837  {
1838  indices = util3d::proportionalRadiusFiltering(assembledCloud, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
1839  pcl::PointCloud<pcl::PointXYZRGB> tmp;
1840  pcl::copyPointCloud(*assembledCloud, *indices, tmp);
1841  *assembledCloud = tmp;
1842  }
1843  else if(!assembledCloudI->empty())
1844  {
1845  indices = util3d::proportionalRadiusFiltering(assembledCloudI, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
1846  pcl::PointCloud<pcl::PointXYZI> tmp;
1847  pcl::copyPointCloud(*assembledCloudI, *indices, tmp);
1848  *assembledCloudI = tmp;
1849  }
1850  if(indices.get())
1851  {
1852  std::vector<int> rawCameraIndicesTmp(indices->size());
1853  for (std::size_t i = 0; i < indices->size(); ++i)
1854  rawCameraIndicesTmp[i] = rawViewpointIndices[indices->at(i)];
1855  rawViewpointIndices = rawCameraIndicesTmp;
1856  }
1857  printf("Proportional radius filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1858  }
1859 
1860  pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
1861  if(voxelSize>0.0f)
1862  {
1863  printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1864  if(!assembledCloud->empty()) {
1865  pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation
1866  assembledCloud = util3d::voxelize(assembledCloud, voxelSize);
1867  }
1868  else if(!assembledCloudI->empty()) {
1869  pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation
1870  assembledCloudI = util3d::voxelize(assembledCloudI, voxelSize);
1871  }
1872  printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1873  }
1874 
1875  printf("Computing normals of the assembled cloud... (k=20, %d points)\n", (int)assembledCloud->size()?(int)assembledCloud->size():(int)assembledCloudI->size());
1876  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
1877  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport(new pcl::PointCloud<pcl::PointXYZINormal>);
1878  if(!assembledCloud->empty()) {
1879  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(assembledCloud, 20, 0);
1880  UASSERT(assembledCloud->size() == normals->size());
1881  pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport);
1882  }
1883  else if(!assembledCloudI->empty()) {
1884  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(assembledCloudI, 20, 0);
1885  UASSERT(assembledCloudI->size() == normals->size());
1886  pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport);
1887  }
1888  printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1889  assembledCloud->clear();
1890  assembledCloudI->clear();
1891 
1892  // adjust with point of views
1893  printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1894  if(!rawAssembledCloud->empty()) {
1895  if(!cloudToExport->empty()) {
1896  util3d::adjustNormalsToViewPoints(
1897  rawViewpoints,
1898  rawAssembledCloud,
1899  rawViewpointIndices,
1900  cloudToExport,
1901  groundNormalsUp);
1902  }
1903  else if(!cloudIToExport->empty()) {
1904  util3d::adjustNormalsToViewPoints(
1905  rawViewpoints,
1906  rawAssembledCloud,
1907  rawViewpointIndices,
1908  cloudIToExport,
1909  groundNormalsUp);
1910  }
1911  }
1912  else if(!cloudToExport->empty()) {
1913  util3d::adjustNormalsToViewPoints(
1914  rawViewpoints,
1915  rawViewpointIndices,
1916  cloudToExport,
1917  groundNormalsUp);
1918  }
1919  else if(!cloudIToExport->empty()) {
1920  util3d::adjustNormalsToViewPoints(
1921  rawViewpoints,
1922  rawViewpointIndices,
1923  cloudIToExport,
1924  groundNormalsUp);
1925  }
1926  printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1927 
1928  if(randomSamples>0)
1929  {
1930  printf("Random samples filtering of the assembled cloud... (samples=%d, %d points)\n", randomSamples, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1931  if(!cloudToExport->empty())
1932  {
1933  cloudToExport = util3d::randomSampling(cloudToExport, randomSamples);
1934  }
1935  else if(!cloudIToExport->empty())
1936  {
1937  cloudIToExport = util3d::randomSampling(cloudIToExport, randomSamples);
1938  }
1939  printf("Random samples filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1940  }
1941 
1942  std::vector<int> pointToCamId;
1943  std::vector<float> pointToCamIntensity;
1944  if(camProjection && !robotPoses.empty())
1945  {
1946  printf("Camera projection... (cameras=%d)\n", (int)cameraModels.size());
1947  std::map<int, std::vector<rtabmap::CameraModel> > cameraModelsProj;
1948  if(cameraProjDecimation>1)
1949  {
1950  for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModels.begin();
1951  iter!=cameraModels.end();
1952  ++iter)
1953  {
1954  std::vector<rtabmap::CameraModel> models;
1955  for(size_t i=0; i<iter->second.size(); ++i)
1956  {
1957  models.push_back(iter->second[i].scaled(1.0/double(cameraProjDecimation)));
1958  }
1959  cameraModelsProj.insert(std::make_pair(iter->first, models));
1960  }
1961  }
1962  else
1963  {
1964  cameraModelsProj = cameraModels;
1965  }
1966 
1967  if(exportImages)
1968  {
1969  printf("Camera projection... projecting cloud to individual cameras (--images option)\n");
1970  // projectCloudToCamera requires PCLPointCloud2
1971  pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
1972  if(!cloudToExport->empty())
1973  {
1974  pcl::toPCLPointCloud2(*cloudToExport, *cloud2);
1975  }
1976  else if(!cloudIToExport->empty())
1977  {
1978  pcl::toPCLPointCloud2(*cloudIToExport, *cloud2);
1979  }
1980 
1981  std::string dir = outputDirectory+"/"+baseName+"_depth_from_scan";
1982  if(!UDirectory::exists(dir)) {
1983  UDirectory::makeDir(dir);
1984  }
1985 
1986  for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModelsProj.begin();
1987  iter!=cameraModelsProj.end();
1988  ++iter)
1989  {
1990  cv::Mat depth(iter->second.front().imageHeight(), iter->second.front().imageWidth()*iter->second.size(), CV_32FC1);
1991  for(size_t i=0; i<iter->second.size(); ++i)
1992  {
1993  cv::Mat subDepth = util3d::projectCloudToCamera(
1994  iter->second.at(i).imageSize(),
1995  iter->second.at(i).K(),
1996  cloud2,
1997  robotPoses.at(iter->first) * iter->second.at(i).localTransform());
1998  subDepth.copyTo(depth(cv::Range::all(), cv::Range(i*iter->second.front().imageWidth(), (i+1)*iter->second.front().imageWidth())));
1999  }
2000 
2001  depth = rtabmap::util2d::cvtDepthFromFloat(depth);
2002  std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
2003  cv::imwrite(outputPath, depth);
2004  }
2005  }
2006 
2007  cv::Mat projMask;
2008  if(!cameraProjMask.empty())
2009  {
2010  projMask = cv::imread(cameraProjMask, cv::IMREAD_GRAYSCALE);
2011  if(cameraProjDecimation>1)
2012  {
2013  cv::Mat out = projMask;
2014  cv::resize(projMask, out, cv::Size(), 1.0f/float(cameraProjDecimation), 1.0f/float(cameraProjDecimation), cv::INTER_NEAREST);
2015  projMask = out;
2016  }
2017  }
2018 
2019  printf("Camera projection... projecting cloud to all cameras\n");
2020  pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
2021  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
2022  if(!cloudToExport->empty())
2023  {
2024  pointToPixel = util3d::projectCloudToCameras(
2025  *cloudToExport,
2026  robotPoses,
2027  cameraModelsProj,
2028  textureRange,
2029  textureAngle,
2030  textureRoiRatios,
2031  projMask,
2032  distanceToCamPolicy,
2033  &progressState);
2034  }
2035  else if(!cloudIToExport->empty())
2036  {
2037  pointToPixel = util3d::projectCloudToCameras(
2038  *cloudIToExport,
2039  robotPoses,
2040  cameraModelsProj,
2041  textureRange,
2042  textureAngle,
2043  textureRoiRatios,
2044  projMask,
2045  distanceToCamPolicy,
2046  &progressState);
2047  pointToCamIntensity.resize(pointToPixel.size());
2048  }
2049 
2050  printf("Camera projection... coloring the cloud\n");
2051  // color the cloud
2052  UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size());
2053  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
2054  assembledCloudValidPoints->resize(pointToCamId.size());
2055 
2056  int imagesDone = 1;
2057  int coloredPoints = 0;
2058  for(std::map<int, rtabmap::Transform>::iterator iter=robotPoses.lower_bound(1); iter!=robotPoses.end(); ++iter)
2059  {
2060  int nodeID = iter->first;
2061  cv::Mat image;
2062  SensorData data;
2063  dbDriver->getNodeData(nodeID, data, true, false, false, false);
2064  if(!data.imageCompressed().empty())
2065  {
2066  data.uncompressDataConst(&image, 0);
2067  }
2068  if(!image.empty())
2069  {
2070  if(cameraProjDecimation>1)
2071  {
2072  image = util2d::decimate(image, cameraProjDecimation);
2073  }
2074  }
2075  UASSERT(cameraModelsProj.find(nodeID) != cameraModelsProj.end());
2076  int modelsSize = cameraModelsProj.at(nodeID).size();
2077  for(size_t i=0; i<pointToPixel.size(); ++i)
2078  {
2079  int cameraIndex = pointToPixel[i].first.second;
2080  if(nodeID == pointToPixel[i].first.first && cameraIndex>=0)
2081  {
2082  pcl::PointXYZRGBNormal pt;
2083  float intensity = 0;
2084  if(!cloudToExport->empty())
2085  {
2086  pt = cloudToExport->at(i);
2087  }
2088  else if(!cloudIToExport->empty())
2089  {
2090  pt.x = cloudIToExport->at(i).x;
2091  pt.y = cloudIToExport->at(i).y;
2092  pt.z = cloudIToExport->at(i).z;
2093  pt.normal_x = cloudIToExport->at(i).normal_x;
2094  pt.normal_y = cloudIToExport->at(i).normal_y;
2095  pt.normal_z = cloudIToExport->at(i).normal_z;
2096  intensity = cloudIToExport->at(i).intensity;
2097  }
2098 
2099  if(!image.empty())
2100  {
2101  int subImageWidth = image.cols / modelsSize;
2102  cv::Mat subImage = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
2103 
2104  int x = pointToPixel[i].second.x * (float)subImage.cols;
2105  int y = pointToPixel[i].second.y * (float)subImage.rows;
2106  UASSERT(x>=0 && x<subImage.cols);
2107  UASSERT(y>=0 && y<subImage.rows);
2108 
2109  if(subImage.type()==CV_8UC3)
2110  {
2111  cv::Vec3b bgr = subImage.at<cv::Vec3b>(y, x);
2112  pt.b = bgr[0];
2113  pt.g = bgr[1];
2114  pt.r = bgr[2];
2115  }
2116  else
2117  {
2118  UASSERT(subImage.type()==CV_8UC1);
2119  pt.r = pt.g = pt.b = subImage.at<unsigned char>(pointToPixel[i].second.y * subImage.rows, pointToPixel[i].second.x * subImage.cols);
2120  }
2121  ++coloredPoints;
2122  }
2123 
2124  int exportedId = nodeID;
2125  pointToCamId[i] = exportedId;
2126  if(!pointToCamIntensity.empty())
2127  {
2128  pointToCamIntensity[i] = intensity;
2129  }
2130  assembledCloudValidPoints->at(i) = pt;
2131  }
2132  }
2133  if(!image.empty())
2134  {
2135  printf("Processed %d/%d images\n", imagesDone++, (int)robotPoses.size());
2136  }
2137  else
2138  {
2139  printf("Node %d doesn't have image! (%d/%d)\n", iter->first, imagesDone++, (int)robotPoses.size());
2140  }
2141  }
2142  printf("Colored %d/%d points.\n", coloredPoints, (int)assembledCloudValidPoints->size());
2143 
2144  pcl::IndicesPtr validIndices(new std::vector<int>(pointToPixel.size()));
2145  size_t oi = 0;
2146  for(size_t i=0; i<pointToPixel.size(); ++i)
2147  {
2148  if(pointToPixel[i].first.first <=0)
2149  {
2150  if(camProjectionKeepAll)
2151  {
2152  pcl::PointXYZRGBNormal pt;
2153  float intensity = 0;
2154  if(!cloudToExport->empty())
2155  {
2156  pt = cloudToExport->at(i);
2157  }
2158  else if(!cloudIToExport->empty())
2159  {
2160  pt.x = cloudIToExport->at(i).x;
2161  pt.y = cloudIToExport->at(i).y;
2162  pt.z = cloudIToExport->at(i).z;
2163  pt.normal_x = cloudIToExport->at(i).normal_x;
2164  pt.normal_y = cloudIToExport->at(i).normal_y;
2165  pt.normal_z = cloudIToExport->at(i).normal_z;
2166  intensity = cloudIToExport->at(i).intensity;
2167  }
2168 
2169  pointToCamId[i] = 0; // invalid
2170  pt.b = 0;
2171  pt.g = 0;
2172  pt.r = 255;
2173  if(!pointToCamIntensity.empty())
2174  {
2175  pointToCamIntensity[i] = intensity;
2176  }
2177  assembledCloudValidPoints->at(i) = pt; // red
2178  validIndices->at(oi++) = i;
2179  }
2180  }
2181  else
2182  {
2183  validIndices->at(oi++) = i;
2184  }
2185  }
2186  if(oi != validIndices->size())
2187  {
2188  validIndices->resize(oi);
2189  assembledCloudValidPoints = util3d::extractIndices(assembledCloudValidPoints, validIndices, false, false);
2190  std::vector<int> pointToCamIdTmp(validIndices->size());
2191  std::vector<float> pointToCamIntensityTmp(pointToCamIntensity.empty()?0:validIndices->size());
2192  for(size_t i=0; i<validIndices->size(); ++i)
2193  {
2194  size_t index = validIndices->at(i);
2195  UASSERT(index < pointToCamId.size());
2196  pointToCamIdTmp[i] = pointToCamId[index];
2197  if(!pointToCamIntensity.empty())
2198  {
2199  UASSERT(index < pointToCamIntensity.size());
2200  pointToCamIntensityTmp[i] = pointToCamIntensity[index];
2201  }
2202  }
2203  pointToCamId = pointToCamIdTmp;
2204  pointToCamIntensity = pointToCamIntensityTmp;
2205  pointToCamIdTmp.clear();
2206  pointToCamIntensityTmp.clear();
2207  }
2208  cloudToExport = assembledCloudValidPoints;
2209  cloudIToExport->clear();
2210 
2211  printf("Camera projection... done! (%fs)\n", timer.ticks());
2212  }
2213 
2214  if(exportCloud)
2215  {
2216  if(saveInDb)
2217  {
2218  if(exportMesh)
2219  {
2220  printf("Option --save_in_db is set and both --cloud and --mesh are set, we will only save the mesh in the database.\n");
2221  }
2222  else
2223  {
2224  printf("Saving cloud in db... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
2225  if(!cloudToExport->empty())
2226  driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudToExport, Transform(), false).data());
2227  else if(!cloudIToExport->empty())
2228  driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudIToExport, Transform(), false).data());
2229  printf("Saving cloud in db... done!\n");
2230  }
2231  }
2232  else
2233  {
2234  std::string ext = las?"las":"ply";
2235  std::string outputPath=outputDirectory+"/"+baseName+"_cloud."+ext;
2236  printf("Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
2237 #ifdef RTABMAP_PDAL
2238  if(las || !pointToCamId.empty() || !pointToCamIntensity.empty())
2239  {
2240  if(!cloudToExport->empty())
2241  {
2242  if(!pointToCamIntensity.empty())
2243  {
2244  savePDALFile(outputPath, *cloudToExport, pointToCamId, binary, pointToCamIntensity);
2245  }
2246  else
2247  {
2248  savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
2249  }
2250  }
2251  else if(!cloudIToExport->empty())
2252  savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary);
2253  }
2254  else
2255 #endif
2256  {
2257  if(!pointToCamId.empty())
2258  {
2259  if(!pointToCamIntensity.empty())
2260  {
2261  printf("Option --cam_projection is enabled but rtabmap is not built "
2262  "with PDAL support, so camera IDs and lidar intensities won't be exported in the output cloud.\n");
2263  }
2264  else
2265  {
2266  printf("Option --cam_projection is enabled but rtabmap is not built "
2267  "with PDAL support, so camera IDs won't be exported in the output cloud.\n");
2268  }
2269  }
2270  if(!cloudToExport->empty())
2271  pcl::io::savePLYFile(outputPath, *cloudToExport, binary);
2272  else if(!cloudIToExport->empty())
2273  pcl::io::savePLYFile(outputPath, *cloudIToExport, binary);
2274  }
2275  printf("Saving %s... done!\n", outputPath.c_str());
2276  }
2277  }
2278 
2279  // Meshing...
2280  if(exportMesh)
2281  {
2282  if(!cloudIToExport->empty())
2283  {
2284  pcl::copyPointCloud(*cloudIToExport, *cloudToExport);
2285  cloudIToExport->clear();
2286  }
2287 
2288  Eigen::Vector4f min,max;
2289  pcl::getMinMax3D(*cloudToExport, min, max);
2290  float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
2291  int optimizedDepth = 12;
2292  for(int i=6; i<12; ++i)
2293  {
2294  if(mapLength/float(1<<i) < poissonSize)
2295  {
2296  optimizedDepth = i;
2297  break;
2298  }
2299  }
2300  if(poissonDepth>0)
2301  {
2302  optimizedDepth = poissonDepth;
2303  }
2304 
2305  // Mesh reconstruction
2306  printf("Mesh reconstruction... depth=%d\n", optimizedDepth);
2307  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
2308  pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
2309  poisson.setDepth(optimizedDepth);
2310  poisson.setInputCloud(cloudToExport);
2311  poisson.reconstruct(*mesh);
2312  printf("Mesh reconstruction... done (%fs, %d polygons).\n", timer.ticks(), (int)mesh->polygons.size());
2313 
2314  if(mesh->polygons.size())
2315  {
2316  printf("Mesh color transfer (max polygons=%d, color radius=%f, clean=%s)...\n",
2317  maxPolygons,
2318  colorRadius,
2319  doClean?"true":"false");
2320  rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
2321  mesh,
2322  0.0f,
2323  maxPolygons,
2324  cloudToExport,
2325  colorRadius,
2326  !(texture && textureVertexColorPolicy == 0),
2327  doClean,
2328  minCluster);
2329  printf("Mesh color transfer... done (%fs).\n", timer.ticks());
2330 
2331  if(!texture)
2332  {
2333  if(saveInDb)
2334  {
2335  printf("Saving mesh in db...\n");
2336  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
2337  polygons.push_back(util3d::convertPolygonsFromPCL(mesh->polygons));
2338  driver->saveOptimizedMesh(
2339  util3d::laserScanFromPointCloud(mesh->cloud, false).data(),
2340  polygons);
2341  printf("Saving mesh in db... done!\n");
2342  }
2343  else
2344  {
2345  std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
2346  printf("Saving %s...\n", outputPath.c_str());
2347  if(binary)
2348  pcl::io::savePLYFileBinary(outputPath, *mesh);
2349  else
2350  pcl::io::savePLYFile(outputPath, *mesh);
2351  printf("Saving %s... done!\n", outputPath.c_str());
2352  }
2353  }
2354  else
2355  {
2356  // Camera filtering for texturing
2357  std::map<int, rtabmap::Transform> robotPosesFiltered;
2358  if(laplacianThr>0)
2359  {
2360  printf("Filtering %ld images from texturing...\n", robotPoses.size());
2361  for(std::map<int, rtabmap::Transform>::iterator iter=robotPoses.lower_bound(1); iter!=robotPoses.end(); ++iter)
2362  {
2363  SensorData data;
2364  dbDriver->getNodeData(iter->first, data, true, false, false, false);
2365  cv::Mat img;
2366  data.uncompressDataConst(&img, 0);
2367  if(!img.empty())
2368  {
2369  cv::Mat imgLaplacian;
2370  cv::Laplacian(img, imgLaplacian, CV_16S);
2371  cv::Mat m, s;
2372  cv::meanStdDev(imgLaplacian, m, s);
2373  double stddev_pxl = s.at<double>(0);
2374  double var = stddev_pxl*stddev_pxl;
2375  if(var < (double)laplacianThr)
2376  {
2377  printf("Camera's image %d is detected as blurry (var=%f < thr=%d), camera is ignored for texturing.\n", iter->first, var, laplacianThr);
2378  }
2379  else
2380  {
2381  robotPosesFiltered.insert(*iter);
2382  }
2383  }
2384  }
2385  printf("Filtered %ld/%ld images from texturing", robotPosesFiltered.size(), robotPoses.size());
2386  }
2387  else
2388  {
2389  robotPosesFiltered = robotPoses;
2390  }
2391 
2392  printf("Texturing %d polygons... robotPoses=%d, cameraModels=%d, cameraDepths=%d\n", (int)mesh->polygons.size(), (int)robotPosesFiltered.size(), (int)cameraModels.size(), (int)cameraDepths.size());
2393  std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
2394  pcl::TextureMeshPtr textureMesh = rtabmap::util3d::createTextureMesh(
2395  mesh,
2396  robotPosesFiltered,
2397  cameraModels,
2398  cameraDepths,
2399  textureRange,
2400  textureDepthError,
2401  textureAngle,
2402  multiband?0:50, // Min polygons in camera view to be textured by this camera
2403  textureRoiRatios,
2404  &progressState,
2405  &vertexToPixels,
2406  distanceToCamPolicy);
2407  printf("Texturing... done (%fs).\n", timer.ticks());
2408 
2409  // Remove occluded polygons (polygons with no texture)
2410  if(doClean && textureMesh->tex_coordinates.size())
2411  {
2412  printf("Cleanup mesh...\n");
2413  rtabmap::util3d::cleanTextureMesh(*textureMesh, 100); // Min polygons in a cluster to keep them
2414  printf("Cleanup mesh... done (%fs).\n", timer.ticks());
2415  }
2416 
2417  if(textureMesh->tex_materials.size())
2418  {
2419  if(multiband)
2420  {
2421  printf("Merging %d texture(s) to single one (multiband enabled)...\n", (int)textureMesh->tex_materials.size());
2422  }
2423  else
2424  {
2425  printf("Merging %d texture(s)... (%d max textures)\n", (int)textureMesh->tex_materials.size(), textureCount);
2426  }
2427  std::map<int, std::map<int, cv::Vec4d> > gains;
2428  std::map<int, std::map<int, cv::Mat> > blendingGains;
2429  std::pair<float, float> contrastValues(0,0);
2430  cv::Mat textures = rtabmap::util3d::mergeTextures(
2431  *textureMesh,
2432  std::map<int, cv::Mat>(),
2433  std::map<int, std::vector<rtabmap::CameraModel> >(),
2434  0,
2435  dbDriver.get(),
2436  textureSize,
2437  multiband?1:textureCount, // to get contrast values based on all images in multiband mode
2438  vertexToPixels,
2439  gainValue>0.0f, gainValue, doGainCompensationRGB,
2440  doBlending, 0,
2441  lowBrightnessGain, highBrightnessGain, // low-high brightness/contrast balance
2442  false, // exposure fusion
2443  0, // state
2444  255, // blank value (0=white)
2445  textureVertexColorPolicy == 1,
2446  &gains,
2447  &blendingGains,
2448  &contrastValues);
2449  printf("Merging to %d texture(s)... done (%fs).\n", (int)textureMesh->tex_materials.size(), timer.ticks());
2450 
2451  if(saveInDb)
2452  {
2453  printf("Saving texture mesh in db...\n");
2454  driver->saveOptimizedMesh(
2455  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
2456  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
2457  textureMesh->tex_coordinates,
2458  textures);
2459  printf("Saving texture mesh in db... done!\n");
2460  }
2461  else
2462  {
2463  // Save multiband first
2464  if(multiband)
2465  {
2466  timer.restart();
2467  std::string outputPath=outputDirectory+"/"+baseName+"_mesh_multiband.obj";
2468  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",
2469  textureSize,
2470  multibandDownScale,
2471  multibandUnwrap==1?"ABF":multibandUnwrap==2?"LSCM":"Basic",
2472  multibandFillHoles?"true":"false",
2473  multibandPadding,
2474  multibandBestScoreThr,
2475  multibandAngleHardthr,
2476  multibandForceVisible?"false":"true",
2477  outputPath.c_str());
2478  if(util3d::multiBandTexturing(outputPath,
2479  textureMesh->cloud,
2480  textureMesh->tex_polygons[0],
2481  robotPosesFiltered,
2482  vertexToPixels,
2483  std::map<int, cv::Mat >(),
2484  std::map<int, std::vector<CameraModel> >(),
2485  0,
2486  dbDriver.get(),
2487  textureSize,
2488  multibandDownScale,
2489  multibandNbContrib,
2490  "jpg",
2491  gains,
2492  blendingGains,
2493  contrastValues,
2494  doGainCompensationRGB,
2495  multibandUnwrap,
2496  multibandFillHoles,
2497  multibandPadding,
2498  multibandBestScoreThr,
2499  multibandAngleHardthr,
2500  multibandForceVisible))
2501  {
2502  printf("MultiBand texturing...done (%fs).\n", timer.ticks());
2503  }
2504  else
2505  {
2506  printf("MultiBand texturing...failed! (%fs)\n", timer.ticks());
2507  }
2508  }
2509  else
2510  {
2511  // TextureMesh OBJ
2512  bool success = false;
2513  UASSERT(!textures.empty());
2514  for(size_t i=0; i<textureMesh->tex_materials.size(); ++i)
2515  {
2516  // Texture file name format is texture[#], replace "texture" by base filename
2517  std::list<std::string> values = uSplitNumChar(textureMesh->tex_materials[i].tex_file);
2518  textureMesh->tex_materials[i].tex_file.clear();
2519  for(const auto & v: values)
2520  {
2521  if(v.compare("texture") == 0){
2522  textureMesh->tex_materials[i].tex_file.append(baseName+"_mesh");
2523  }
2524  else {
2525  textureMesh->tex_materials[i].tex_file.append(v);
2526  }
2527  }
2528  textureMesh->tex_materials[i].tex_file += ".jpg";
2529  printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
2530  UASSERT(textures.cols % textures.rows == 0);
2531  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))));
2532  if(!success)
2533  {
2534  UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str());
2535  }
2536  else
2537  {
2538  printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
2539  }
2540  }
2541  if(success)
2542  {
2543  std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj";
2544  printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
2545  #if PCL_VERSION_COMPARE(>=, 1, 13, 0)
2546  textureMesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
2547  auto nr_meshes = static_cast<unsigned>(textureMesh->tex_polygons.size());
2548  unsigned f_idx = 0;
2549  for (unsigned m = 0; m < nr_meshes; m++) {
2550  std::vector<pcl::Vertices> ci = textureMesh->tex_polygons[m];
2551  for(std::size_t i = 0; i < ci.size(); i++) {
2552  for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
2553  ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
2554  }
2555  }
2556  textureMesh->tex_coord_indices.push_back(ci);
2557  f_idx += static_cast<unsigned>(textureMesh->tex_polygons[m].size());
2558  }
2559  #endif
2560  success = util3d::saveOBJFile(outputPath, *textureMesh) == 0;
2561 
2562  if(success)
2563  {
2564  printf("Saved obj to %s!\n", outputPath.c_str());
2565  }
2566  else
2567  {
2568  UERROR("Failed saving obj to %s!", outputPath.c_str());
2569  }
2570  }
2571  }
2572  }
2573  }
2574  }
2575  }
2576  }
2577  }
2578  else
2579  {
2580  printf("Export failed! The cloud is empty.\n");
2581  }
2582 
2583  if(driver)
2584  {
2585  driver->closeConnection();
2586  delete driver;
2587  driver = 0;
2588  }
2589 
2590  return 0;
2591 }
rtabmap::SensorData
Definition: SensorData.h:51
int
int
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1237
UFile::getName
std::string getName()
Definition: UFile.h:135
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
util3d_surface.h
rtabmap::savePDALFile
int savePDALFile(const std::string &filePath, const pcl::PointCloud< pcl::PointXYZ > &cloud, const std::vector< int > &cameraIds=std::vector< int >(), bool binary=false)
Definition: PDALWriter.cpp:76
ULogger::kError
@ kError
Definition: ULogger.h:252
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
timer
s
RealScalar s
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:135
rtabmap::util3d::cleanTextureMesh
void RTABMAP_CORE_EXPORT cleanTextureMesh(pcl::TextureMesh &textureMesh, int minClusterSize)
Definition: util3d_surface.cpp:873
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
LASWriter.h
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:605
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::util3d::passThrough
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
Definition: util3d_filtering.cpp:878
rtabmap::util3d::createTextureMesh
pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(const pcl::PolygonMesh::Ptr &mesh, const std::map< int, Transform > &poses, const std::map< int, CameraModel > &cameraModels, const std::map< int, cv::Mat > &cameraDepths, float maxDistance=0.0f, float maxDepthError=0.0f, float maxAngle=0.0f, int minClusterSize=50, const std::vector< float > &roiRatios=std::vector< float >(), const ProgressState *state=0, std::vector< std::map< int, pcl::PointXY > > *vertexToPixels=0, bool distanceToCamPolicy=false)
Definition: util3d_surface.cpp:675
rtabmap::graph::exportPoses
bool RTABMAP_CORE_EXPORT exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:761
rtabmap::util3d::projectCloudToCameras
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > RTABMAP_CORE_EXPORT projectCloudToCameras(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance=0.0f, float maxAngle=0.0f, const std::vector< float > &roiRatios=std::vector< float >(), const cv::Mat &projMask=cv::Mat(), bool distanceToCamPolicy=false, const ProgressState *state=0)
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::GPS
Definition: GPS.h:35
rtabmap_netvlad.img
img
Definition: rtabmap_netvlad.py:78
rtabmap::GeodeticCoords
Definition: GeodeticCoords.h:52
showUsage
void showUsage()
Definition: tools/Export/main.cpp:58
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_CORE_EXPORT commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:74
timer::restart
void restart()
y
Matrix3f y
rtabmap::OptimizerG2O
Definition: OptimizerG2O.h:37
rtabmap::util3d::convertPolygonsFromPCL
std::vector< std::vector< RTABMAP_PCL_INDEX > > RTABMAP_CORE_EXPORT convertPolygonsFromPCL(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:1227
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:475
iterator
util3d.h
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
UTimer.h
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UMath.h
Basic mathematics functions.
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:506
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1245
indices
indices
rtabmap::util3d::loadScan
LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string &path)
Definition: util3d.cpp:3583
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:670
rtabmap::util3d::adjustNormalsToViewPoints
void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(const std::map< int, Transform > &poses, const std::vector< int > &cameraIndices, pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, float groundNormalsUp=0.0f)
Definition: util3d_surface.cpp:3730
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1288
data
int data[]
util3d_transforms.h
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
j
std::ptrdiff_t j
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::util3d::projectCloudToCamera
cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Definition: util3d.cpp:2713
util3d_filtering.h
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
rtabmap::util3d::proportionalRadiusFiltering
pcl::IndicesPtr RTABMAP_CORE_EXPORT proportionalRadiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::vector< int > &viewpointIndices, const std::map< int, Transform > &viewpoints, float factor=0.01f, float neighborScale=2.0f)
Definition: util3d_filtering.cpp:1338
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2342
g2o
Definition: edge_se3_xyzprior.h:34
all
static const Eigen::internal::all_t all
Rtabmap.h
first
constexpr int first(int i)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
exportPoses
bool exportPoses
Definition: tools/Reprocess/main.cpp:141
rtabmap::util3d::extractIndices
pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
Definition: util3d_filtering.cpp:2447
rtabmap::graph::exportGPS
bool RTABMAP_CORE_EXPORT exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:510
ConsoleProgessState::callback
virtual bool callback(const std::string &msg) const
Definition: tools/Export/main.cpp:183
UASSERT
#define UASSERT(condition)
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
rtabmap::ProgressState
Definition: ProgressState.h:35
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
x
x
m
Matrix3f m
p
Point3_ p(2)
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
DBDriver.h
rtabmap::util3d::saveOBJFile
int RTABMAP_CORE_EXPORT saveOBJFile(const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision=5)
Definition: util3d_surface.cpp:3955
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2369
rtabmap::graph::computeMinMax
void RTABMAP_CORE_EXPORT computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2453
out
std::ofstream out("Result.txt")
rtabmap::util3d::multiBandTexturing
RTABMAP_DEPRECATED bool RTABMAP_CORE_EXPORT multiBandTexturing(const std::string &outputOBJPath, const pcl::PCLPointCloud2 &cloud, const std::vector< pcl::Vertices > &polygons, const std::map< int, Transform > &cameraPoses, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels, const std::map< int, cv::Mat > &images, const std::map< int, std::vector< CameraModel > > &cameraModels, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=8192, const std::string &textureFormat="jpg", const std::map< int, std::map< int, cv::Vec4d > > &gains=std::map< int, std::map< int, cv::Vec4d > >(), const std::map< int, std::map< int, cv::Mat > > &blendingGains=std::map< int, std::map< int, cv::Mat > >(), const std::pair< float, float > &contrastValues=std::pair< float, float >(0, 0), bool gainRGB=true)
Definition: util3d_surface.cpp:2301
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:578
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::DBDriver
Definition: DBDriver.h:62
PDALWriter.h
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
nodes
KeyVector nodes
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap::util3d::randomSampling
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT randomSampling(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int samples)
Definition: util3d_filtering.cpp:830
cameraPoses
std::array< Pose3, 3 > cameraPoses
rtabmap::Transform
Definition: Transform.h:41
util2d.h
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:892
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
Graph.h
values
leaf::MyValues values
iter
iterator iter(handle obj)
rtabmap::util3d::mergeTextures
cv::Mat RTABMAP_CORE_EXPORT mergeTextures(pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, CameraModel > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0, unsigned char blankValue=255, bool clearVertexColorUnderTexture=true, std::map< int, std::map< int, cv::Vec4d > > *gains=0, std::map< int, std::map< int, cv::Mat > > *blendingGains=0, std::pair< float, float > *contrastValues=0)
Definition: util3d_surface.cpp:1438
c_str
const char * c_str(Args &&...args)
rtabmap::GeodeticCoords::toENU_WGS84
cv::Point3d toENU_WGS84(const GeodeticCoords &origin) const
Definition: GeodeticCoords.cpp:108
UStl.h
Wrappers of STL for convenient functions.
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
v
Array< int, Dynamic, 1 > v
UTimer
Definition: UTimer.h:46
float
float
prior
const auto prior
main
int main(int argc, char *argv[])
Definition: tools/Export/main.cpp:191
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GPS.h:66
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2633
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_CORE_EXPORT computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
uIsDigit
bool uIsDigit(const char c)
Definition: UStl.h:620
OptimizerG2O.h
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1231
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_CORE_EXPORT radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1377
ConsoleProgessState
Definition: tools/Export/main.cpp:181
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
i
int i
binary
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
rtabmap::util3d::radiusFiltering
pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
Definition: util3d_filtering.cpp:1235
rtabmap::DBDriver::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons=std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: DBDriver.cpp:1260
msg
msg
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:56