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