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 using namespace rtabmap;
53 
54 void showUsage()
55 {
56  printf("\nUsage:\n"
57  "rtabmap-export [options] database.db\n"
58  "Options:\n"
59  " --output \"\" Output name (default: name of the database is used).\n"
60  " --output_dir \"\" Output directory (default: same directory than the database).\n"
61  " --ascii Export PLY in ascii format.\n"
62  " --las Export cloud in LAS instead of PLY (PDAL dependency required).\n"
63  " --mesh Create a mesh.\n"
64  " --texture Create a mesh with texture.\n"
65  " --texture_size # Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n"
66  " --texture_count # Maximum textures generated (default 1). Ignored by --multiband option (adjust --multiband_contrib instead).\n"
67  " --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
68  " --texture_angle # Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
69  " --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used, default=0).\n"
70  " --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%% of the image bottom not used).\n"
71  " --texture_d2c Distance to camera policy.\n"
72  " --texture_blur # Motion blur threshold (default 0: disabled). Below this threshold, the image is considered blurred. 0 means disabled. 50 can be good default.\n"
73  " --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
74  " --cam_projection_keep_all Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
75  " --cam_projection_decimation Decimate images before projecting the points.\n"
76  " --cam_projection_mask \"\" File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.\n"
77  " --poses Export optimized poses of the robot frame (e.g., base_link).\n"
78  " --poses_camera Export optimized poses of the camera frame (e.g., optical frame).\n"
79  " --poses_scan Export optimized poses of the scan frame.\n"
80  " --poses_format # Format used for exported poses (default is 11):\n"
81  " 0=Raw 3x4 transformation matrix (r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz)\n"
82  " 1=RGBD-SLAM (in motion capture coordinate frame)\n"
83  " 2=KITTI (same as raw but in optical frame)\n"
84  " 3=TORO\n"
85  " 4=g2o\n"
86  " 10=RGBD-SLAM in ROS coordinate frame (stamp x y z qx qy qz qw)\n"
87  " 11=RGBD-SLAM in ROS coordinate frame + ID (stamp x y z qx qy qz qw id)\n"
88  " --images Export images with stamp as file name.\n"
89  " --images_id Export images with node id as file name.\n"
90  " --ba Do global bundle adjustment before assembling the clouds.\n"
91  " --gain # Gain compensation value (default 1, set 0 to disable).\n"
92  " --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n"
93  " --no_blending Disable blending when texturing.\n"
94  " --no_clean Disable cleaning colorless polygons.\n"
95  " --min_cluster # When meshing, filter clusters of polygons with size less than this threshold (default 200, -1 means keep only biggest contiguous surface).\n"
96  " --low_gain # Low brightness gain 0-100 (default 0).\n"
97  " --high_gain # High brightness gain 0-100 (default 10).\n"
98  " --multiband Enable multiband texturing (AliceVision dependency required).\n"
99  " --multiband_downscale # Downscaling reduce the texture quality but speed up the computation time (default 2).\n"
100  " --multiband_contrib \"# # # # \" Number of contributions per frequency band for the multi-band blending, should be 4 values! (default \"1 5 10 0\").\n"
101  " --multiband_unwrap # Method to unwrap input mesh: 0=basic (default, >600k faces, fast), 1=ABF (<=300k faces, generate 1 atlas), 2=LSCM (<=600k faces, optimize space).\n"
102  " --multiband_fillholes Fill Texture holes with plausible values.\n"
103  " --multiband_padding # Texture edge padding size in pixel (0-100) (default 5).\n"
104  " --multiband_scorethr # 0 to disable filtering based on threshold to relative best score (0.0-1.0). (default 0.1).\n"
105  " --multiband_anglethr # 0 to disable angle hard threshold filtering (0.0, 180.0) (default 90.0).\n"
106  " --multiband_forcevisible Triangle visibility is based on the union of vertices visibility.\n"
107  " --poisson_depth # Set Poisson depth for mesh reconstruction.\n"
108  " --poisson_size # Set target polygon size when computing Poisson's depth for mesh reconstruction (default 0.03 m).\n"
109  " --max_polygons # Maximum polygons when creating a mesh (default 300000, set 0 for no limit).\n"
110  " --min_range # Minimum range of the created clouds (default 0 m).\n"
111  " --max_range # Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
112  " --decimation # Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
113  " --voxel # Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
114  " --ground_normals_up # Flip ground normals up if close to -z axis (default 0, 0=disabled, value should be >0 and <1, typical 0.9).\n"
115  " --noise_radius # Noise filtering search radius (default 0, 0=disabled).\n"
116  " --noise_k # Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
117  " --prop_radius_factor # Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
118  " --prop_radius_scale # Proportional radius filter neighbor scale (default 2).\n"
119  " --random_samples # Number of output samples using a random filter (default 0, 0=disabled).\n"
120  " --color_radius # Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
121  " --scan Use laser scan for the point cloud.\n"
122  " --save_in_db Save resulting assembled point cloud or mesh in the database.\n"
123  " --xmin # Minimum range on X axis to keep nodes to export.\n"
124  " --xmax # Maximum range on X axis to keep nodes to export.\n"
125  " --ymin # Minimum range on Y axis to keep nodes to export.\n"
126  " --ymax # Maximum range on Y axis to keep nodes to export.\n"
127  " --zmin # Minimum range on Z axis to keep nodes to export.\n"
128  " --zmax # Maximum range on Z axis to keep nodes to export.\n"
129  " --filter_ceiling # Filter points over a custom height (default 0 m, 0=disabled).\n"
130  " --filter_floor # Filter points below a custom height (default 0 m, 0=disabled).\n"
131 
132  "\n%s", Parameters::showUsage());
133  ;
134  exit(1);
135 }
136 
137 class ConsoleProgessState : public ProgressState
138 {
139  virtual bool callback(const std::string & msg) const
140  {
141  if(!msg.empty())
142  printf("%s\n", msg.c_str());
143  return true;
144  }
145 };
146 
147 int main(int argc, char * argv[])
148 {
149  ULogger::setType(ULogger::kTypeConsole);
150  ULogger::setLevel(ULogger::kError);
151 
152  if(argc < 2)
153  {
154  showUsage();
155  }
156 
157  bool binary = true;
158  bool las = false;
159  bool mesh = false;
160  bool texture = false;
161  bool ba = false;
162  bool doGainCompensationRGB = true;
163  float gainValue = 1;
164  bool doBlending = true;
165  bool doClean = true;
166  int minCluster = 200;
167  int poissonDepth = 0;
168  float poissonSize = 0.03;
169  int maxPolygons = 300000;
170  int decimation = -1;
171  float minRange = 0.0f;
172  float maxRange = -1.0f;
173  float voxelSize = -1.0f;
174  float groundNormalsUp = 0.0f;
175  float noiseRadius = 0.0f;
176  int noiseMinNeighbors = 5;
177  float proportionalRadiusFactor = 0.0f;
178  float proportionalRadiusScale = 2.0f;
179  int randomSamples = 0;
180  int textureSize = 8192;
181  int textureCount = 1;
182  float textureRange = 0;
183  float textureAngle = 0;
184  float textureDepthError = 0;
185  std::vector<float> textureRoiRatios;
186  bool distanceToCamPolicy = false;
187  int laplacianThr = 0;
188  bool multiband = false;
189  int multibandDownScale = 2;
190  std::string multibandNbContrib = "1 5 10 0";
191  int multibandUnwrap = 0;
192  bool multibandFillHoles = false;
193  int multibandPadding = 5;
194  double multibandBestScoreThr = 0.1;
195  double multibandAngleHardthr = 90;
196  bool multibandForceVisible = false;
197  float colorRadius = -1.0f;
198  bool cloudFromScan = false;
199  bool saveInDb = false;
200  int lowBrightnessGain = 0;
201  int highBrightnessGain = 10;
202  bool camProjection = false;
203  bool camProjectionKeepAll = false;
204  int cameraProjDecimation = 1;
205  std::string cameraProjMask;
206  bool exportPoses = false;
207  bool exportPosesCamera = false;
208  bool exportPosesScan = false;
209  int exportPosesFormat = 11;
210  bool exportImages = false;
211  bool exportImagesId = false;
212  std::string outputName;
213  std::string outputDir;
214  cv::Vec3f min, max;
215  float filter_ceiling = 0.0f;
216  float filter_floor = 0.0f;
217  for(int i=1; i<argc; ++i)
218  {
219  if(std::strcmp(argv[i], "--help") == 0)
220  {
221  showUsage();
222  }
223  else if(std::strcmp(argv[i], "--output") == 0)
224  {
225  ++i;
226  if(i<argc-1)
227  {
228  outputName = argv[i];
229  }
230  else
231  {
232  showUsage();
233  }
234  }
235  else if(std::strcmp(argv[i], "--output_dir") == 0)
236  {
237  ++i;
238  if(i<argc-1)
239  {
240  outputDir = argv[i];
241  }
242  else
243  {
244  showUsage();
245  }
246  }
247  else if(std::strcmp(argv[i], "--bin") == 0)
248  {
249  printf("No need to set --bin anymore, ply are now automatically exported in binary by default. Set --ascii to export as text.\n");
250  }
251  else if(std::strcmp(argv[i], "--ascii") == 0)
252  {
253  binary = false;
254  }
255  else if(std::strcmp(argv[i], "--las") == 0)
256  {
257 #ifdef RTABMAP_PDAL
258  las = true;
259 #elif defined(RTABMAP_LIBLAS)
260  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");
261 #else
262  printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n");
263 #endif
264 
265  }
266  else if(std::strcmp(argv[i], "--mesh") == 0)
267  {
268  mesh = true;
269  }
270  else if(std::strcmp(argv[i], "--texture") == 0)
271  {
272  texture = true;
273  }
274  else if(std::strcmp(argv[i], "--texture_size") == 0)
275  {
276  ++i;
277  if(i<argc-1)
278  {
279  textureSize = uStr2Int(argv[i]);
280  UASSERT(textureSize%256==0);
281  }
282  else
283  {
284  showUsage();
285  }
286  }
287  else if(std::strcmp(argv[i], "--texture_count") == 0)
288  {
289  ++i;
290  if(i<argc-1)
291  {
292  textureCount = uStr2Int(argv[i]);
293  }
294  else
295  {
296  showUsage();
297  }
298  }
299  else if(std::strcmp(argv[i], "--texture_range") == 0)
300  {
301  ++i;
302  if(i<argc-1)
303  {
304  textureRange = uStr2Float(argv[i]);
305  }
306  else
307  {
308  showUsage();
309  }
310  }
311  else if(std::strcmp(argv[i], "--texture_angle") == 0)
312  {
313  ++i;
314  if(i<argc-1)
315  {
316  textureAngle = uStr2Float(argv[i])*M_PI/180.0f;
317  }
318  else
319  {
320  showUsage();
321  }
322  }
323  else if(std::strcmp(argv[i], "--texture_depth_error") == 0)
324  {
325  ++i;
326  if(i<argc-1)
327  {
328  textureDepthError = uStr2Float(argv[i]);
329  }
330  else
331  {
332  showUsage();
333  }
334  }
335  else if(std::strcmp(argv[i], "--texture_roi_ratios") == 0)
336  {
337  ++i;
338  if(i<argc-1)
339  {
340  std::list<std::string> strValues = uSplit(argv[i], ' ');
341  if(strValues.size() != 4)
342  {
343  printf("The number of values must be 4 (roi=\"%s\")\n", argv[i]);
344  showUsage();
345  }
346  else
347  {
348  std::vector<float> tmpValues(4);
349  unsigned int i=0;
350  for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
351  {
352  tmpValues[i] = uStr2Float(*jter);
353  ++i;
354  }
355 
356  if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
357  tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
358  tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
359  tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
360  {
361  textureRoiRatios = tmpValues;
362  }
363  else
364  {
365  printf("The roi ratios are not valid (roi=\"%s\")\n", argv[i]);
366  showUsage();
367  }
368  }
369  }
370  else
371  {
372  showUsage();
373  }
374  }
375  else if(std::strcmp(argv[i], "--texture_d2c") == 0)
376  {
377  distanceToCamPolicy = true;
378  }
379  else if(std::strcmp(argv[i], "--texture_blur") == 0)
380  {
381  ++i;
382  if(i<argc-1)
383  {
384  laplacianThr = uStr2Int(argv[i]);
385  }
386  else
387  {
388  showUsage();
389  }
390  }
391  else if(std::strcmp(argv[i], "--cam_projection") == 0)
392  {
393  camProjection = true;
394  }
395  else if(std::strcmp(argv[i], "--cam_projection_keep_all") == 0)
396  {
397  camProjectionKeepAll = true;
398  }
399  else if(std::strcmp(argv[i], "--cam_projection_decimation") == 0)
400  {
401  ++i;
402  if(i<argc-1)
403  {
404  cameraProjDecimation = uStr2Int(argv[i]);
405  if(cameraProjDecimation<1)
406  {
407  printf("--cam_projection_decimation cannot be <1! value=\"%s\"\n", argv[i]);
408  showUsage();
409  }
410  }
411  else
412  {
413  showUsage();
414  }
415  }
416  else if(std::strcmp(argv[i], "--cam_projection_mask") == 0)
417  {
418  ++i;
419  if(i<argc-1)
420  {
421  cameraProjMask = argv[i];
422  if(!UFile::exists(cameraProjMask))
423  {
424  printf("--cam_projection_mask is set with a file not existing or don't have permissions to open it. Path=\"%s\"\n", argv[i]);
425  showUsage();
426  }
427  }
428  else
429  {
430  showUsage();
431  }
432  }
433  else if(std::strcmp(argv[i], "--poses") == 0)
434  {
435  exportPoses = true;
436  }
437  else if(std::strcmp(argv[i], "--poses_camera") == 0)
438  {
439  exportPosesCamera = true;
440  }
441  else if(std::strcmp(argv[i], "--poses_scan") == 0)
442  {
443  exportPosesScan = true;
444  }
445  else if(std::strcmp(argv[i], "--poses_format") == 0)
446  {
447  ++i;
448  if(i<argc-1)
449  {
450  exportPosesFormat = uStr2Int(argv[i]);
451  }
452  else
453  {
454  showUsage();
455  }
456  }
457  else if(std::strcmp(argv[i], "--images") == 0)
458  {
459  exportImages = true;
460  }
461  else if(std::strcmp(argv[i], "--images_id") == 0)
462  {
463  exportImages = true;
464  exportImagesId = true;
465  }
466  else if(std::strcmp(argv[i], "--ba") == 0)
467  {
468  ba = true;
469  }
470  else if(std::strcmp(argv[i], "--gain_gray") == 0)
471  {
472  doGainCompensationRGB = false;
473  }
474  else if(std::strcmp(argv[i], "--gain") == 0)
475  {
476  ++i;
477  if(i<argc-1)
478  {
479  gainValue = uStr2Float(argv[i]);
480  UASSERT(gainValue>=0.0f);
481  }
482  else
483  {
484  showUsage();
485  }
486  }
487  else if(std::strcmp(argv[i], "--no_blending") == 0)
488  {
489  doBlending = false;
490  }
491  else if(std::strcmp(argv[i], "--no_clean") == 0)
492  {
493  doClean = false;
494  }
495  else if(std::strcmp(argv[i], "--min_cluster") == 0)
496  {
497  ++i;
498  if(i<argc-1)
499  {
500  minCluster = uStr2Int(argv[i]);
501  }
502  else
503  {
504  showUsage();
505  }
506  }
507  else if(std::strcmp(argv[i], "--multiband") == 0)
508  {
509 #ifdef RTABMAP_ALICE_VISION
510  multiband = true;
511 #else
512  printf("\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
513 #endif
514  }
515  else if(std::strcmp(argv[i], "--multiband_fillholes") == 0)
516  {
517  multibandFillHoles = true;
518  }
519  else if(std::strcmp(argv[i], "--multiband_downscale") == 0)
520  {
521  ++i;
522  if(i<argc-1)
523  {
524  multibandDownScale = uStr2Int(argv[i]);
525  }
526  else
527  {
528  showUsage();
529  }
530  }
531  else if(std::strcmp(argv[i], "--multiband_contrib") == 0)
532  {
533  ++i;
534  if(i<argc-1)
535  {
536  if(uSplit(argv[i], ' ').size() != 4)
537  {
538  printf("--multiband_contrib has wrong format! value=\"%s\"\n", argv[i]);
539  showUsage();
540  }
541  multibandNbContrib = argv[i];
542  }
543  else
544  {
545  showUsage();
546  }
547  }
548  else if(std::strcmp(argv[i], "--multiband_unwrap") == 0)
549  {
550  ++i;
551  if(i<argc-1)
552  {
553  multibandUnwrap = uStr2Int(argv[i]);
554  }
555  else
556  {
557  showUsage();
558  }
559  }
560  else if(std::strcmp(argv[i], "--multiband_padding") == 0)
561  {
562  ++i;
563  if(i<argc-1)
564  {
565  multibandPadding = uStr2Int(argv[i]);
566  }
567  else
568  {
569  showUsage();
570  }
571  }
572  else if(std::strcmp(argv[i], "--multiband_forcevisible") == 0)
573  {
574  multibandForceVisible = true;
575  }
576  else if(std::strcmp(argv[i], "--multiband_scorethr") == 0)
577  {
578  ++i;
579  if(i<argc-1)
580  {
581  multibandBestScoreThr = uStr2Float(argv[i]);
582  }
583  else
584  {
585  showUsage();
586  }
587  }
588  else if(std::strcmp(argv[i], "--multiband_anglethr") == 0)
589  {
590  ++i;
591  if(i<argc-1)
592  {
593  multibandAngleHardthr = uStr2Float(argv[i]);
594  }
595  else
596  {
597  showUsage();
598  }
599  }
600  else if(std::strcmp(argv[i], "--poisson_depth") == 0)
601  {
602  ++i;
603  if(i<argc-1)
604  {
605  poissonDepth = uStr2Int(argv[i]);
606  }
607  else
608  {
609  showUsage();
610  }
611  }
612  else if(std::strcmp(argv[i], "--poisson_size") == 0)
613  {
614  ++i;
615  if(i<argc-1)
616  {
617  poissonSize = uStr2Float(argv[i]);
618  }
619  else
620  {
621  showUsage();
622  }
623  }
624  else if(std::strcmp(argv[i], "--max_polygons") == 0)
625  {
626  ++i;
627  if(i<argc-1)
628  {
629  maxPolygons = uStr2Int(argv[i]);
630  }
631  else
632  {
633  showUsage();
634  }
635  }
636  else if(std::strcmp(argv[i], "--min_range") == 0)
637  {
638  ++i;
639  if(i<argc-1)
640  {
641  minRange = uStr2Float(argv[i]);
642  }
643  else
644  {
645  showUsage();
646  }
647  }
648  else if(std::strcmp(argv[i], "--max_range") == 0)
649  {
650  ++i;
651  if(i<argc-1)
652  {
653  maxRange = uStr2Float(argv[i]);
654  }
655  else
656  {
657  showUsage();
658  }
659  }
660  else if(std::strcmp(argv[i], "--decimation") == 0)
661  {
662  ++i;
663  if(i<argc-1)
664  {
665  decimation = uStr2Int(argv[i]);
666  }
667  else
668  {
669  showUsage();
670  }
671  }
672  else if(std::strcmp(argv[i], "--voxel") == 0)
673  {
674  ++i;
675  if(i<argc-1)
676  {
677  voxelSize = uStr2Float(argv[i]);
678  }
679  else
680  {
681  showUsage();
682  }
683  }
684  else if(std::strcmp(argv[i], "--ground_normals_up") == 0)
685  {
686  ++i;
687  if(i<argc-1)
688  {
689  groundNormalsUp = uStr2Float(argv[i]);
690  }
691  else
692  {
693  showUsage();
694  }
695  }
696  else if(std::strcmp(argv[i], "--noise_radius") == 0)
697  {
698  ++i;
699  if(i<argc-1)
700  {
701  noiseRadius = uStr2Float(argv[i]);
702  }
703  else
704  {
705  showUsage();
706  }
707  }
708  else if(std::strcmp(argv[i], "--noise_k") == 0)
709  {
710  ++i;
711  if(i<argc-1)
712  {
713  noiseMinNeighbors = uStr2Int(argv[i]);
714  }
715  else
716  {
717  showUsage();
718  }
719  }
720  else if(std::strcmp(argv[i], "--prop_radius_factor") == 0)
721  {
722  ++i;
723  if(i<argc-1)
724  {
725  proportionalRadiusFactor = uStr2Float(argv[i]);
726  }
727  else
728  {
729  showUsage();
730  }
731  }
732  else if(std::strcmp(argv[i], "--prop_radius_scale") == 0)
733  {
734  ++i;
735  if(i<argc-1)
736  {
737  proportionalRadiusScale = uStr2Float(argv[i]);
738  UASSERT_MSG(proportionalRadiusScale>=1.0f, "--prop_radius_scale should be >= 1.0");
739  }
740  else
741  {
742  showUsage();
743  }
744  }
745  else if(std::strcmp(argv[i], "--random_samples") == 0)
746  {
747  ++i;
748  if(i<argc-1)
749  {
750  randomSamples = uStr2Int(argv[i]);
751  }
752  else
753  {
754  showUsage();
755  }
756  }
757  else if(std::strcmp(argv[i], "--color_radius") == 0)
758  {
759  ++i;
760  if(i<argc-1)
761  {
762  colorRadius = uStr2Float(argv[i]);
763  }
764  else
765  {
766  showUsage();
767  }
768  }
769  else if(std::strcmp(argv[i], "--scan") == 0)
770  {
771  cloudFromScan = true;
772  }
773  else if(std::strcmp(argv[i], "--save_in_db") == 0)
774  {
775  saveInDb = true;
776  }
777  else if(std::strcmp(argv[i], "--low_gain") == 0)
778  {
779  ++i;
780  if(i<argc-1)
781  {
782  lowBrightnessGain = uStr2Int(argv[i]);
783  }
784  else
785  {
786  showUsage();
787  }
788  }
789  else if(std::strcmp(argv[i], "--high_gain") == 0)
790  {
791  ++i;
792  if(i<argc-1)
793  {
794  highBrightnessGain = uStr2Int(argv[i]);
795  }
796  else
797  {
798  showUsage();
799  }
800  }
801  else if(std::strcmp(argv[i], "--xmin") == 0)
802  {
803  ++i;
804  if(i<argc-1)
805  {
806  min[0] = uStr2Float(argv[i]);
807  }
808  else
809  {
810  showUsage();
811  }
812  }
813  else if(std::strcmp(argv[i], "--xmax") == 0)
814  {
815  ++i;
816  if(i<argc-1)
817  {
818  max[0] = uStr2Float(argv[i]);
819  }
820  else
821  {
822  showUsage();
823  }
824  }
825  else if(std::strcmp(argv[i], "--ymin") == 0)
826  {
827  ++i;
828  if(i<argc-1)
829  {
830  min[1] = uStr2Float(argv[i]);
831  }
832  else
833  {
834  showUsage();
835  }
836  }
837  else if(std::strcmp(argv[i], "--ymax") == 0)
838  {
839  ++i;
840  if(i<argc-1)
841  {
842  max[1] = uStr2Float(argv[i]);
843  }
844  else
845  {
846  showUsage();
847  }
848  }
849  else if(std::strcmp(argv[i], "--zmin") == 0)
850  {
851  ++i;
852  if(i<argc-1)
853  {
854  min[2] = uStr2Float(argv[i]);
855  }
856  else
857  {
858  showUsage();
859  }
860  }
861  else if(std::strcmp(argv[i], "--zmax") == 0)
862  {
863  ++i;
864  if(i<argc-1)
865  {
866  max[2] = uStr2Float(argv[i]);
867  }
868  else
869  {
870  showUsage();
871  }
872  }
873  else if(std::strcmp(argv[i], "--filter_ceiling") == 0)
874  {
875  ++i;
876  if(i<argc-1)
877  {
878  filter_ceiling = uStr2Float(argv[i]);
879  if(filter_floor!=0.0f && filter_ceiling != 0.0f && filter_ceiling<filter_floor)
880  {
881  printf("Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
882  showUsage();
883  }
884  }
885  else
886  {
887  showUsage();
888  }
889  }
890  else if(std::strcmp(argv[i], "--filter_floor") == 0)
891  {
892  ++i;
893  if(i<argc-1)
894  {
895  filter_floor = uStr2Float(argv[i]);
896  if(filter_floor!=0.0f && filter_ceiling != 0.0f && filter_ceiling<filter_floor)
897  {
898  printf("Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
899  showUsage();
900  }
901  }
902  else
903  {
904  showUsage();
905  }
906  }
907 
908  }
909 
910  if(decimation < 1)
911  {
912  decimation = cloudFromScan?1:4;
913  }
914  if(maxRange < 0)
915  {
916  maxRange = cloudFromScan?0:4;
917  }
918  if(voxelSize < 0.0f)
919  {
920  voxelSize = cloudFromScan?0:0.01f;
921  }
922  if(colorRadius < 0.0f)
923  {
924  colorRadius = cloudFromScan?0:0.05f;
925  }
926 
927  if(saveInDb)
928  {
929  if(multiband)
930  {
931  printf("Option --multiband is not supported with --save_in_db option, disabling multiband...\n");
932  multiband = false;
933  }
934  if(textureCount>1)
935  {
936  printf("Option --texture_count > 1 is not supported with --save_in_db option, setting texture_count to 1...\n");
937  textureCount = 1;
938  }
939  }
940 
941  ParametersMap params = Parameters::parseArguments(argc, argv, false);
942 
943  std::string dbPath = argv[argc-1];
944 
945  if(!UFile::exists(dbPath))
946  {
947  UERROR("File \"%s\" doesn't exist!", dbPath.c_str());
948  return -1;
949  }
950 
951  // Get parameters
952  ParametersMap parameters;
953  DBDriver * driver = DBDriver::create();
954  if(driver->openConnection(dbPath))
955  {
956  parameters = driver->getLastParameters();
957  driver->closeConnection(false);
958  }
959  else
960  {
961  UERROR("Cannot open database %s!", dbPath.c_str());
962  return -1;
963  }
964  delete driver;
965  driver = 0;
966 
967  for(ParametersMap::iterator iter=params.begin(); iter!=params.end(); ++iter)
968  {
969  printf("Added custom parameter %s=%s\n",iter->first.c_str(), iter->second.c_str());
970  }
971 
972  UTimer timer;
973 
974  printf("Loading database \"%s\"...\n", dbPath.c_str());
975  // Get the global optimized map
976  Rtabmap rtabmap;
977  uInsert(parameters, params);
978  rtabmap.init(parameters, dbPath);
979  printf("Loading database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks());
980 
981  std::map<int, Signature> nodes;
982  std::map<int, Transform> optimizedPoses;
983  std::multimap<int, Link> links;
984  printf("Optimizing the map...\n");
985  rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
986  printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size());
987 
988  if(optimizedPoses.empty())
989  {
990  printf("The optimized graph is empty!? Aborting...\n");
991  return -1;
992  }
993 
994  if(min[0] != max[0] || min[1] != max[1] || min[2] != max[2])
995  {
996  cv::Vec3f minP,maxP;
997  graph::computeMinMax(optimizedPoses, minP, maxP);
998  printf("Filtering poses (range: x=%f<->%f, y=%f<->%f, z=%f<->%f, map size=%f x %f x %f)...\n",
999  min[0],max[0],min[1],max[1],min[2],max[2],
1000  maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
1001  std::map<int, Transform> posesFiltered;
1002  for(std::map<int, Transform>::const_iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
1003  {
1004  bool ignore = false;
1005  if(min[0] != max[0] && (iter->second.x() < min[0] || iter->second.x() > max[0]))
1006  {
1007  ignore = true;
1008  }
1009  if(min[1] != max[1] && (iter->second.y() < min[1] || iter->second.y() > max[1]))
1010  {
1011  ignore = true;
1012  }
1013  if(min[2] != max[2] && (iter->second.z() < min[2] || iter->second.z() > max[2]))
1014  {
1015  ignore = true;
1016  }
1017  if(!ignore)
1018  {
1019  posesFiltered.insert(*iter);
1020  }
1021  }
1022  graph::computeMinMax(posesFiltered, minP, maxP);
1023  printf("Filtering poses... done! %d/%d remaining (new map size=%f x %f x %f).\n", (int)posesFiltered.size(), (int)optimizedPoses.size(), maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
1024  optimizedPoses = posesFiltered;
1025  if(optimizedPoses.empty())
1026  {
1027  return -1;
1028  }
1029  }
1030 
1031  std::string outputDirectory = outputDir.empty()?UDirectory::getDir(dbPath):outputDir;
1032  if(!UDirectory::exists(outputDirectory))
1033  {
1034  UDirectory::makeDir(outputDirectory);
1035  }
1036  std::string baseName = outputName.empty()?uSplit(UFile::getName(dbPath), '.').front():outputName;
1037 
1038  if(ba)
1039  {
1040  printf("Global bundle adjustment...\n");
1041  OptimizerG2O g2o(parameters);
1042  optimizedPoses = ((Optimizer*)&g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
1043  printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
1044  }
1045 
1046  // Construct the cloud
1047  printf("Create and assemble the clouds...\n");
1048  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1049  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
1050  std::map<int, rtabmap::Transform> robotPoses;
1051  std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
1052  std::map<int, rtabmap::Transform> scanPoses;
1053  std::map<int, double> cameraStamps;
1054  std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
1055  std::map<int, cv::Mat> cameraDepths;
1056  int imagesExported = 0;
1057  std::vector<int> rawViewpointIndices;
1058  std::map<int, Transform> rawViewpoints;
1059  for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
1060  {
1061  Signature node = nodes.find(iter->first)->second;
1062 
1063  // uncompress data
1064  std::vector<CameraModel> models = node.sensorData().cameraModels();
1065  std::vector<StereoCameraModel> stereoModels = node.sensorData().stereoCameraModels();
1066  cv::Mat rgb;
1067  cv::Mat depth;
1068 
1069  pcl::IndicesPtr indices(new std::vector<int>);
1070  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1071  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
1072  if(node.getWeight() != -1)
1073  {
1074  if(cloudFromScan)
1075  {
1076  cv::Mat tmpDepth;
1077  LaserScan scan;
1078  node.sensorData().uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
1079  if(scan.empty())
1080  {
1081  printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first);
1082  }
1083  if(decimation>1 || minRange>0.0f || maxRange)
1084  {
1085  scan = util3d::commonFiltering(scan, decimation, minRange, maxRange);
1086  }
1087  if(scan.hasRGB())
1088  {
1089  cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
1090  if(noiseRadius>0.0f && noiseMinNeighbors>0)
1091  {
1092  indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors);
1093  }
1094  }
1095  else
1096  {
1097  cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
1098  if(noiseRadius>0.0f && noiseMinNeighbors>0)
1099  {
1100  indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors);
1101  }
1102  }
1103  }
1104  else
1105  {
1106  node.sensorData().uncompressData(&rgb, &depth);
1107  if(depth.empty())
1108  {
1109  printf("Node %d doesn't have depth or stereo data, empty cloud is "
1110  "created (if you want to create point cloud from scan, use --scan option).\n", iter->first);
1111  }
1112  cloud = util3d::cloudRGBFromSensorData(
1113  node.sensorData(),
1114  decimation, // image decimation before creating the clouds
1115  maxRange, // maximum depth of the cloud
1116  minRange,
1117  indices.get());
1118  if(noiseRadius>0.0f && noiseMinNeighbors>0)
1119  {
1120  indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors);
1121  }
1122  }
1123  }
1124 
1125  if(exportImages && !rgb.empty())
1126  {
1127  std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1 && !depth.empty())?"left":"rgb";
1128  std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix;
1129  if(!UDirectory::exists(dir)) {
1130  UDirectory::makeDir(dir);
1131  }
1132  std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",node.getStamp()))+".jpg";
1133  cv::imwrite(outputPath, rgb);
1134  ++imagesExported;
1135  if(!depth.empty())
1136  {
1137  std::string ext;
1138  cv::Mat depthExported = depth;
1139  if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1)
1140  {
1141  ext = ".jpg";
1142  dir = outputDirectory+"/"+baseName+"_right";
1143  }
1144  else
1145  {
1146  ext = ".png";
1147  dir = outputDirectory+"/"+baseName+"_depth";
1148  if(depth.type() == CV_32FC1)
1149  {
1150  depthExported = rtabmap::util2d::cvtDepthFromFloat(depth);
1151  }
1152  }
1153  if(!UDirectory::exists(dir)) {
1154  UDirectory::makeDir(dir);
1155  }
1156 
1157  outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",node.getStamp()))+ext;
1158  cv::imwrite(outputPath, depthExported);
1159  }
1160 
1161  // save calibration per image (calibration can change over time, e.g. camera has auto focus)
1162  for(size_t i=0; i<models.size(); ++i)
1163  {
1164  CameraModel model = models[i];
1165  std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f",node.getStamp()));
1166  if(models.size() > 1) {
1167  modelName += "_" + uNumber2Str((int)i);
1168  }
1169  model.setName(modelName);
1170  std::string dir = outputDirectory+"/"+baseName+"_calib";
1171  if(!UDirectory::exists(dir)) {
1172  UDirectory::makeDir(dir);
1173  }
1174  model.save(dir);
1175  }
1176  for(size_t i=0; i<stereoModels.size(); ++i)
1177  {
1178  StereoCameraModel model = stereoModels[i];
1179  std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f",node.getStamp()));
1180  if(stereoModels.size() > 1) {
1181  modelName += "_" + uNumber2Str((int)i);
1182  }
1183  model.setName(modelName, "left", "right");
1184  std::string dir = outputDirectory+"/"+baseName+"_calib";
1185  if(!UDirectory::exists(dir)) {
1186  UDirectory::makeDir(dir);
1187  }
1188  model.save(dir);
1189  }
1190  }
1191 
1192  if(voxelSize>0.0f)
1193  {
1194  if(cloud.get() && !cloud->empty())
1195  cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
1196  else if(cloudI.get() && !cloudI->empty())
1197  cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
1198  }
1199  if(cloud.get() && !cloud->empty())
1200  cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
1201  else if(cloudI.get() && !cloudI->empty())
1202  cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
1203 
1204  if(filter_ceiling != 0.0 || filter_floor != 0.0f)
1205  {
1206  if(cloud.get() && !cloud->empty())
1207  {
1208  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());
1209  }
1210  if(cloudI.get() && !cloudI->empty())
1211  {
1212  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());
1213  }
1214  }
1215 
1216  if(cloudFromScan)
1217  {
1218  Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
1219  rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
1220  }
1221  else if(!node.sensorData().cameraModels().empty() && !node.sensorData().cameraModels()[0].localTransform().isNull())
1222  {
1223  Transform cameraViewpoint = iter->second * node.sensorData().cameraModels()[0].localTransform(); // take the first camera
1224  rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
1225  }
1226  else if(!node.sensorData().stereoCameraModels().empty() && !node.sensorData().stereoCameraModels()[0].localTransform().isNull())
1227  {
1228  Transform cameraViewpoint = iter->second * node.sensorData().stereoCameraModels()[0].localTransform();
1229  rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
1230  }
1231  else
1232  {
1233  rawViewpoints.insert(*iter);
1234  }
1235 
1236  if(cloud.get() && !cloud->empty())
1237  {
1238  if(assembledCloud->empty())
1239  {
1240  *assembledCloud = *cloud;
1241  }
1242  else
1243  {
1244  *assembledCloud += *cloud;
1245  }
1246  rawViewpointIndices.resize(assembledCloud->size(), iter->first);
1247  }
1248  else if(cloudI.get() && !cloudI->empty())
1249  {
1250  if(assembledCloudI->empty())
1251  {
1252  *assembledCloudI = *cloudI;
1253  }
1254  else
1255  {
1256  *assembledCloudI += *cloudI;
1257  }
1258  rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
1259  }
1260 
1261  if(models.empty())
1262  {
1263  for(size_t i=0; i<node.sensorData().stereoCameraModels().size(); ++i)
1264  {
1265  models.push_back(node.sensorData().stereoCameraModels()[i].left());
1266  }
1267  }
1268 
1269  robotPoses.insert(std::make_pair(iter->first, iter->second));
1270  cameraStamps.insert(std::make_pair(iter->first, node.getStamp()));
1271  if(models.empty() && node.getWeight() == -1 && !cameraModels.empty())
1272  {
1273  // For intermediate nodes, use latest models
1274  models = cameraModels.rbegin()->second;
1275  }
1276  if(!models.empty())
1277  {
1278  if(!node.sensorData().imageCompressed().empty())
1279  {
1280  cameraModels.insert(std::make_pair(iter->first, models));
1281  }
1282  if(exportPosesCamera)
1283  {
1284  if(cameraPoses.empty())
1285  {
1286  cameraPoses.resize(models.size());
1287  }
1288  UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
1289  for(size_t i=0; i<models.size(); ++i)
1290  {
1291  cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
1292  }
1293  }
1294  }
1295  if(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
1296  {
1297  cameraDepths.insert(std::make_pair(iter->first, depth));
1298  }
1299  if(exportPosesScan && !node.sensorData().laserScanCompressed().empty())
1300  {
1301  scanPoses.insert(std::make_pair(iter->first, iter->second*node.sensorData().laserScanCompressed().localTransform()));
1302  }
1303  }
1304  printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1305 
1306  if(imagesExported>0)
1307  printf("%d images exported!\n", imagesExported);
1308 
1309  ConsoleProgessState progressState;
1310 
1311  if(!assembledCloud->empty() || !assembledCloudI->empty())
1312  {
1313  if(saveInDb)
1314  {
1315  driver = DBDriver::create();
1316  UASSERT(driver->openConnection(dbPath, false));
1317  Transform lastlocalizationPose;
1318  driver->loadOptimizedPoses(&lastlocalizationPose);
1319  //optimized poses have changed, reset 2d map
1320  driver->save2DMap(cv::Mat(), 0, 0, 0);
1321  driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
1322  }
1323  else
1324  {
1325  std::string posesExt = (exportPosesFormat==3?"toro":exportPosesFormat==4?"g2o":"txt");
1326  if(exportPoses)
1327  {
1328  std::string outputPath=outputDirectory+"/"+baseName+"_poses." + posesExt;
1329  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps);
1330  printf("Poses exported to \"%s\".\n", outputPath.c_str());
1331  }
1332  if(exportPosesCamera)
1333  {
1334  for(size_t i=0; i<cameraPoses.size(); ++i)
1335  {
1336  std::string outputPath;
1337  if(cameraPoses.size()==1)
1338  outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
1339  else
1340  outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
1341  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
1342  printf("Camera poses exported to \"%s\".\n", outputPath.c_str());
1343  }
1344  }
1345  if(exportPosesScan)
1346  {
1347  std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
1348  rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), cameraStamps);
1349  printf("Scan poses exported to \"%s\".\n", outputPath.c_str());
1350  }
1351  }
1352 
1353  if(proportionalRadiusFactor>0.0f && proportionalRadiusScale>=1.0f)
1354  {
1355  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());
1356  pcl::IndicesPtr indices;
1357  if(!assembledCloud->empty())
1358  {
1359  indices = util3d::proportionalRadiusFiltering(assembledCloud, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
1360  pcl::PointCloud<pcl::PointXYZRGB> tmp;
1361  pcl::copyPointCloud(*assembledCloud, *indices, tmp);
1362  *assembledCloud = tmp;
1363  }
1364  else if(!assembledCloudI->empty())
1365  {
1366  indices = util3d::proportionalRadiusFiltering(assembledCloudI, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
1367  pcl::PointCloud<pcl::PointXYZI> tmp;
1368  pcl::copyPointCloud(*assembledCloudI, *indices, tmp);
1369  *assembledCloudI = tmp;
1370  }
1371  if(indices.get())
1372  {
1373  std::vector<int> rawCameraIndicesTmp(indices->size());
1374  for (std::size_t i = 0; i < indices->size(); ++i)
1375  rawCameraIndicesTmp[i] = rawViewpointIndices[indices->at(i)];
1376  rawViewpointIndices = rawCameraIndicesTmp;
1377  }
1378  printf("Proportional radius filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1379  }
1380 
1381  pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
1382  if(!assembledCloud->empty())
1383  pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation
1384  else if(!assembledCloudI->empty())
1385  pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation
1386 
1387  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals = rawAssembledCloud;
1388 
1389  if(voxelSize>0.0f)
1390  {
1391  printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1392  if(!assembledCloud->empty())
1393  {
1394  assembledCloud = util3d::voxelize(assembledCloud, voxelSize);
1395  cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
1396  pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
1397  }
1398  else if(!assembledCloudI->empty())
1399  {
1400  assembledCloudI = util3d::voxelize(assembledCloudI, voxelSize);
1401  cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
1402  pcl::copyPointCloud(*assembledCloudI, *cloudWithoutNormals);
1403  }
1404  printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1405  }
1406 
1407  printf("Computing normals of the assembled cloud... (k=20, %d points)\n", !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
1408  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, 20, 0);
1409 
1410  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
1411  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport(new pcl::PointCloud<pcl::PointXYZINormal>);
1412  if(!assembledCloud->empty())
1413  {
1414  UASSERT(assembledCloud->size() == normals->size());
1415  pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport);
1416  printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloud->size());
1417  assembledCloud->clear();
1418 
1419  // adjust with point of views
1420  printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudToExport->size());
1421  util3d::adjustNormalsToViewPoints(
1422  rawViewpoints,
1423  rawAssembledCloud,
1424  rawViewpointIndices,
1425  cloudToExport,
1426  groundNormalsUp);
1427  printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudToExport->size());
1428  }
1429  else if(!assembledCloudI->empty())
1430  {
1431  UASSERT(assembledCloudI->size() == normals->size());
1432  pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport);
1433  printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloudI->size());
1434  assembledCloudI->clear();
1435 
1436  // adjust with point of views
1437  printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudIToExport->size());
1438  util3d::adjustNormalsToViewPoints(
1439  rawViewpoints,
1440  rawAssembledCloud,
1441  rawViewpointIndices,
1442  cloudIToExport,
1443  groundNormalsUp);
1444  printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudIToExport->size());
1445  }
1446  cloudWithoutNormals->clear();
1447  rawAssembledCloud->clear();
1448 
1449  if(randomSamples>0)
1450  {
1451  printf("Random samples filtering of the assembled cloud... (samples=%d, %d points)\n", randomSamples, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1452  if(!cloudToExport->empty())
1453  {
1454  cloudToExport = util3d::randomSampling(cloudToExport, randomSamples);
1455  }
1456  else if(!cloudIToExport->empty())
1457  {
1458  cloudIToExport = util3d::randomSampling(cloudIToExport, randomSamples);
1459  }
1460  printf("Random samples filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1461  }
1462 
1463  std::vector<int> pointToCamId;
1464  std::vector<float> pointToCamIntensity;
1465  if(camProjection && !robotPoses.empty())
1466  {
1467  printf("Camera projection...\n");
1468  std::map<int, std::vector<rtabmap::CameraModel> > cameraModelsProj;
1469  if(cameraProjDecimation>1)
1470  {
1471  for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModels.begin();
1472  iter!=cameraModels.end();
1473  ++iter)
1474  {
1475  std::vector<rtabmap::CameraModel> models;
1476  for(size_t i=0; i<iter->second.size(); ++i)
1477  {
1478  models.push_back(iter->second[i].scaled(1.0/double(cameraProjDecimation)));
1479  }
1480  cameraModelsProj.insert(std::make_pair(iter->first, models));
1481  }
1482  }
1483  else
1484  {
1485  cameraModelsProj = cameraModels;
1486  }
1487 
1488  if(exportImages)
1489  {
1490  printf("Camera projection... projecting cloud to individual cameras (--images option)\n");
1491  // projectCloudToCamera requires PCLPointCloud2
1492  pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
1493  if(!cloudToExport->empty())
1494  {
1495  pcl::toPCLPointCloud2(*cloudToExport, *cloud2);
1496  }
1497  else if(!cloudIToExport->empty())
1498  {
1499  pcl::toPCLPointCloud2(*cloudIToExport, *cloud2);
1500  }
1501 
1502  std::string dir = outputDirectory+"/"+baseName+"_depth_from_scan";
1503  if(!UDirectory::exists(dir)) {
1504  UDirectory::makeDir(dir);
1505  }
1506 
1507  for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModelsProj.begin();
1508  iter!=cameraModelsProj.end();
1509  ++iter)
1510  {
1511  cv::Mat depth(iter->second.front().imageHeight(), iter->second.front().imageWidth()*iter->second.size(), CV_32FC1);
1512  for(size_t i=0; i<iter->second.size(); ++i)
1513  {
1514  cv::Mat subDepth = util3d::projectCloudToCamera(
1515  iter->second.at(i).imageSize(),
1516  iter->second.at(i).K(),
1517  cloud2,
1518  robotPoses.at(iter->first) * iter->second.at(i).localTransform());
1519  subDepth.copyTo(depth(cv::Range::all(), cv::Range(i*iter->second.front().imageWidth(), (i+1)*iter->second.front().imageWidth())));
1520  }
1521 
1522  depth = rtabmap::util2d::cvtDepthFromFloat(depth);
1523  std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
1524  cv::imwrite(outputPath, depth);
1525  }
1526  }
1527 
1528  cv::Mat projMask;
1529  if(!cameraProjMask.empty())
1530  {
1531  projMask = cv::imread(cameraProjMask, cv::IMREAD_GRAYSCALE);
1532  if(cameraProjDecimation>1)
1533  {
1534  cv::Mat out = projMask;
1535  cv::resize(projMask, out, cv::Size(), 1.0f/float(cameraProjDecimation), 1.0f/float(cameraProjDecimation), cv::INTER_NEAREST);
1536  projMask = out;
1537  }
1538  }
1539 
1540  printf("Camera projection... projecting cloud to all cameras\n");
1541  pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
1542  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
1543  if(!cloudToExport->empty())
1544  {
1545  pointToPixel = util3d::projectCloudToCameras(
1546  *cloudToExport,
1547  robotPoses,
1548  cameraModelsProj,
1549  textureRange,
1550  textureAngle,
1551  textureRoiRatios,
1552  projMask,
1553  distanceToCamPolicy,
1554  &progressState);
1555  }
1556  else if(!cloudIToExport->empty())
1557  {
1558  pointToPixel = util3d::projectCloudToCameras(
1559  *cloudIToExport,
1560  robotPoses,
1561  cameraModelsProj,
1562  textureRange,
1563  textureAngle,
1564  textureRoiRatios,
1565  projMask,
1566  distanceToCamPolicy,
1567  &progressState);
1568  pointToCamIntensity.resize(pointToPixel.size());
1569  }
1570 
1571  printf("Camera projection... coloring the cloud\n");
1572  // color the cloud
1573  UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size());
1574  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
1575  assembledCloudValidPoints->resize(pointToCamId.size());
1576 
1577  int imagesDone = 1;
1578  for(std::map<int, rtabmap::Transform>::iterator iter=robotPoses.begin(); iter!=robotPoses.end(); ++iter)
1579  {
1580  int nodeID = iter->first;
1581  cv::Mat image;
1582  if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty())
1583  {
1584  nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0);
1585  }
1586  if(!image.empty())
1587  {
1588  if(cameraProjDecimation>1)
1589  {
1590  image = util2d::decimate(image, cameraProjDecimation);
1591  }
1592  UASSERT(cameraModelsProj.find(nodeID) != cameraModelsProj.end());
1593  int modelsSize = cameraModelsProj.at(nodeID).size();
1594  for(size_t i=0; i<pointToPixel.size(); ++i)
1595  {
1596  int cameraIndex = pointToPixel[i].first.second;
1597  if(nodeID == pointToPixel[i].first.first && cameraIndex>=0)
1598  {
1599  pcl::PointXYZRGBNormal pt;
1600  float intensity = 0;
1601  if(!cloudToExport->empty())
1602  {
1603  pt = cloudToExport->at(i);
1604  }
1605  else if(!cloudIToExport->empty())
1606  {
1607  pt.x = cloudIToExport->at(i).x;
1608  pt.y = cloudIToExport->at(i).y;
1609  pt.z = cloudIToExport->at(i).z;
1610  pt.normal_x = cloudIToExport->at(i).normal_x;
1611  pt.normal_y = cloudIToExport->at(i).normal_y;
1612  pt.normal_z = cloudIToExport->at(i).normal_z;
1613  intensity = cloudIToExport->at(i).intensity;
1614  }
1615 
1616  int subImageWidth = image.cols / modelsSize;
1617  cv::Mat subImage = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
1618 
1619  int x = pointToPixel[i].second.x * (float)subImage.cols;
1620  int y = pointToPixel[i].second.y * (float)subImage.rows;
1621  UASSERT(x>=0 && x<subImage.cols);
1622  UASSERT(y>=0 && y<subImage.rows);
1623 
1624  if(subImage.type()==CV_8UC3)
1625  {
1626  cv::Vec3b bgr = subImage.at<cv::Vec3b>(y, x);
1627  pt.b = bgr[0];
1628  pt.g = bgr[1];
1629  pt.r = bgr[2];
1630  }
1631  else
1632  {
1633  UASSERT(subImage.type()==CV_8UC1);
1634  pt.r = pt.g = pt.b = subImage.at<unsigned char>(pointToPixel[i].second.y * subImage.rows, pointToPixel[i].second.x * subImage.cols);
1635  }
1636 
1637  int exportedId = nodeID;
1638  pointToCamId[i] = exportedId;
1639  if(!pointToCamIntensity.empty())
1640  {
1641  pointToCamIntensity[i] = intensity;
1642  }
1643  assembledCloudValidPoints->at(i) = pt;
1644  }
1645  }
1646  }
1647  UINFO("Processed %d/%d images", imagesDone++, (int)robotPoses.size());
1648  }
1649 
1650  pcl::IndicesPtr validIndices(new std::vector<int>(pointToPixel.size()));
1651  size_t oi = 0;
1652  for(size_t i=0; i<pointToPixel.size(); ++i)
1653  {
1654  if(pointToPixel[i].first.first <=0)
1655  {
1656  if(camProjectionKeepAll)
1657  {
1658  pcl::PointXYZRGBNormal pt;
1659  float intensity = 0;
1660  if(!cloudToExport->empty())
1661  {
1662  pt = cloudToExport->at(i);
1663  }
1664  else if(!cloudIToExport->empty())
1665  {
1666  pt.x = cloudIToExport->at(i).x;
1667  pt.y = cloudIToExport->at(i).y;
1668  pt.z = cloudIToExport->at(i).z;
1669  pt.normal_x = cloudIToExport->at(i).normal_x;
1670  pt.normal_y = cloudIToExport->at(i).normal_y;
1671  pt.normal_z = cloudIToExport->at(i).normal_z;
1672  intensity = cloudIToExport->at(i).intensity;
1673  }
1674 
1675  pointToCamId[i] = 0; // invalid
1676  pt.b = 0;
1677  pt.g = 0;
1678  pt.r = 255;
1679  if(!pointToCamIntensity.empty())
1680  {
1681  pointToCamIntensity[i] = intensity;
1682  }
1683  assembledCloudValidPoints->at(i) = pt; // red
1684  validIndices->at(oi++) = i;
1685  }
1686  }
1687  else
1688  {
1689  validIndices->at(oi++) = i;
1690  }
1691  }
1692 
1693  if(oi != validIndices->size())
1694  {
1695  validIndices->resize(oi);
1696  assembledCloudValidPoints = util3d::extractIndices(assembledCloudValidPoints, validIndices, false, false);
1697  std::vector<int> pointToCamIdTmp(validIndices->size());
1698  std::vector<float> pointToCamIntensityTmp(validIndices->size());
1699  for(size_t i=0; i<validIndices->size(); ++i)
1700  {
1701  pointToCamIdTmp[i] = pointToCamId[validIndices->at(i)];
1702  pointToCamIntensityTmp[i] = pointToCamIntensity[validIndices->at(i)];
1703  }
1704  pointToCamId = pointToCamIdTmp;
1705  pointToCamIntensity = pointToCamIntensityTmp;
1706  pointToCamIdTmp.clear();
1707  pointToCamIntensityTmp.clear();
1708  }
1709 
1710  cloudToExport = assembledCloudValidPoints;
1711  cloudIToExport->clear();
1712 
1713  printf("Camera projection... done! (%fs)\n", timer.ticks());
1714  }
1715 
1716  if(!(mesh || texture))
1717  {
1718  if(saveInDb)
1719  {
1720  printf("Saving in db... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1721  if(!cloudToExport->empty())
1722  driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudToExport, Transform(), false).data());
1723  else if(!cloudIToExport->empty())
1724  driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudIToExport, Transform(), false).data());
1725  printf("Saving in db... done!\n");
1726  }
1727  else
1728  {
1729  std::string ext = las?"las":"ply";
1730  std::string outputPath=outputDirectory+"/"+baseName+"_cloud."+ext;
1731  printf("Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
1732 #ifdef RTABMAP_PDAL
1733  if(las || !pointToCamId.empty() || !pointToCamIntensity.empty())
1734  {
1735  if(!cloudToExport->empty())
1736  {
1737  if(!pointToCamIntensity.empty())
1738  {
1739  savePDALFile(outputPath, *cloudToExport, pointToCamId, binary, pointToCamIntensity);
1740  }
1741  else
1742  {
1743  savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
1744  }
1745  }
1746  else if(!cloudIToExport->empty())
1747  savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary);
1748  }
1749  else
1750 #endif
1751  {
1752  if(!pointToCamId.empty())
1753  {
1754  if(!pointToCamIntensity.empty())
1755  {
1756  printf("Option --cam_projection is enabled but rtabmap is not built "
1757  "with PDAL support, so camera IDs and lidar intensities won't be exported in the output cloud.\n");
1758  }
1759  else
1760  {
1761  printf("Option --cam_projection is enabled but rtabmap is not built "
1762  "with PDAL support, so camera IDs won't be exported in the output cloud.\n");
1763  }
1764  }
1765  if(!cloudToExport->empty())
1766  pcl::io::savePLYFile(outputPath, *cloudToExport, binary);
1767  else if(!cloudIToExport->empty())
1768  pcl::io::savePLYFile(outputPath, *cloudIToExport, binary);
1769  }
1770  printf("Saving %s... done!\n", outputPath.c_str());
1771  }
1772  }
1773 
1774  // Meshing...
1775  if(mesh || texture)
1776  {
1777  if(!cloudIToExport->empty())
1778  {
1779  pcl::copyPointCloud(*cloudIToExport, *cloudToExport);
1780  cloudIToExport->clear();
1781  }
1782 
1783  Eigen::Vector4f min,max;
1784  pcl::getMinMax3D(*cloudToExport, min, max);
1785  float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
1786  int optimizedDepth = 12;
1787  for(int i=6; i<12; ++i)
1788  {
1789  if(mapLength/float(1<<i) < poissonSize)
1790  {
1791  optimizedDepth = i;
1792  break;
1793  }
1794  }
1795  if(poissonDepth>0)
1796  {
1797  optimizedDepth = poissonDepth;
1798  }
1799 
1800  // Mesh reconstruction
1801  printf("Mesh reconstruction... depth=%d\n", optimizedDepth);
1802  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
1803  pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
1804  poisson.setDepth(optimizedDepth);
1805  poisson.setInputCloud(cloudToExport);
1806  poisson.reconstruct(*mesh);
1807  printf("Mesh reconstruction... done (%fs, %d polygons).\n", timer.ticks(), (int)mesh->polygons.size());
1808 
1809  if(mesh->polygons.size())
1810  {
1811  printf("Mesh color transfer (max polygons=%d, color radius=%f, clean=%s)...\n",
1812  maxPolygons,
1813  colorRadius,
1814  doClean?"true":"false");
1815  rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
1816  mesh,
1817  0.0f,
1818  maxPolygons,
1819  cloudToExport,
1820  colorRadius,
1821  !texture,
1822  doClean,
1823  minCluster);
1824  printf("Mesh color transfer... done (%fs).\n", timer.ticks());
1825 
1826  if(!texture)
1827  {
1828  if(saveInDb)
1829  {
1830  printf("Saving mesh in db...\n");
1831  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
1832  polygons.push_back(util3d::convertPolygonsFromPCL(mesh->polygons));
1833  driver->saveOptimizedMesh(
1834  util3d::laserScanFromPointCloud(mesh->cloud, false).data(),
1835  polygons);
1836  printf("Saving mesh in db... done!\n");
1837  }
1838  else
1839  {
1840  std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
1841  printf("Saving %s...\n", outputPath.c_str());
1842  if(binary)
1843  pcl::io::savePLYFileBinary(outputPath, *mesh);
1844  else
1845  pcl::io::savePLYFile(outputPath, *mesh);
1846  printf("Saving %s... done!\n", outputPath.c_str());
1847  }
1848  }
1849  else
1850  {
1851  // Camera filtering for texturing
1852  std::map<int, rtabmap::Transform> robotPosesFiltered;
1853  if(laplacianThr>0)
1854  {
1855  printf("Filtering %ld images from texturing...\n", robotPoses.size());
1856  for(std::map<int, rtabmap::Transform>::iterator iter=robotPoses.begin(); iter!=robotPoses.end(); ++iter)
1857  {
1858  UASSERT(nodes.find(iter->first) != nodes.end());
1859  cv::Mat img;
1860  nodes.find(iter->first)->second.sensorData().uncompressDataConst(&img, 0);
1861  if(!img.empty())
1862  {
1863  cv::Mat imgLaplacian;
1864  cv::Laplacian(img, imgLaplacian, CV_16S);
1865  cv::Mat m, s;
1866  cv::meanStdDev(imgLaplacian, m, s);
1867  double stddev_pxl = s.at<double>(0);
1868  double var = stddev_pxl*stddev_pxl;
1869  if(var < (double)laplacianThr)
1870  {
1871  printf("Camera's image %d is detected as blurry (var=%f < thr=%d), camera is ignored for texturing.\n", iter->first, var, laplacianThr);
1872  }
1873  else
1874  {
1875  robotPosesFiltered.insert(*iter);
1876  }
1877  }
1878  }
1879  printf("Filtered %ld/%ld images from texturing", robotPosesFiltered.size(), robotPoses.size());
1880  }
1881  else
1882  {
1883  robotPosesFiltered = robotPoses;
1884  }
1885 
1886  printf("Texturing %d polygons... robotPoses=%d, cameraModels=%d, cameraDepths=%d\n", (int)mesh->polygons.size(), (int)robotPosesFiltered.size(), (int)cameraModels.size(), (int)cameraDepths.size());
1887  std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
1888  pcl::TextureMeshPtr textureMesh = rtabmap::util3d::createTextureMesh(
1889  mesh,
1890  robotPosesFiltered,
1891  cameraModels,
1892  cameraDepths,
1893  textureRange,
1894  textureDepthError,
1895  textureAngle,
1896  multiband?0:50, // Min polygons in camera view to be textured by this camera
1897  textureRoiRatios,
1898  &progressState,
1899  &vertexToPixels,
1900  distanceToCamPolicy);
1901  printf("Texturing... done (%fs).\n", timer.ticks());
1902 
1903  // Remove occluded polygons (polygons with no texture)
1904  if(doClean && textureMesh->tex_coordinates.size())
1905  {
1906  printf("Cleanup mesh...\n");
1907  rtabmap::util3d::cleanTextureMesh(*textureMesh, 100); // Min polygons in a cluster to keep them
1908  printf("Cleanup mesh... done (%fs).\n", timer.ticks());
1909  }
1910 
1911  if(textureMesh->tex_materials.size())
1912  {
1913  if(multiband)
1914  {
1915  printf("Merging %d texture(s) to single one (multiband enabled)...\n", (int)textureMesh->tex_materials.size());
1916  }
1917  else
1918  {
1919  printf("Merging %d texture(s)... (%d max textures)\n", (int)textureMesh->tex_materials.size(), textureCount);
1920  }
1921  std::map<int, std::map<int, cv::Vec4d> > gains;
1922  std::map<int, std::map<int, cv::Mat> > blendingGains;
1923  std::pair<float, float> contrastValues(0,0);
1924  cv::Mat textures = rtabmap::util3d::mergeTextures(
1925  *textureMesh,
1926  std::map<int, cv::Mat>(),
1927  std::map<int, std::vector<rtabmap::CameraModel> >(),
1928  rtabmap.getMemory(),
1929  0,
1930  textureSize,
1931  multiband?1:textureCount, // to get contrast values based on all images in multiband mode
1932  vertexToPixels,
1933  gainValue>0.0f, gainValue, doGainCompensationRGB,
1934  doBlending, 0,
1935  lowBrightnessGain, highBrightnessGain, // low-high brightness/contrast balance
1936  false, // exposure fusion
1937  0, // state
1938  0, // blank value (0=black)
1939  &gains,
1940  &blendingGains,
1941  &contrastValues);
1942  printf("Merging to %d texture(s)... done (%fs).\n", (int)textureMesh->tex_materials.size(), timer.ticks());
1943 
1944  if(saveInDb)
1945  {
1946  printf("Saving texture mesh in db...\n");
1947  driver->saveOptimizedMesh(
1948  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
1949  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
1950  textureMesh->tex_coordinates,
1951  textures);
1952  printf("Saving texture mesh in db... done!\n");
1953  }
1954  else
1955  {
1956  // Save multiband first
1957  if(multiband)
1958  {
1959  timer.restart();
1960  std::string outputPath=outputDirectory+"/"+baseName+"_mesh_multiband.obj";
1961  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",
1962  textureSize,
1963  multibandDownScale,
1964  multibandUnwrap==1?"ABF":multibandUnwrap==2?"LSCM":"Basic",
1965  multibandFillHoles?"true":"false",
1966  multibandPadding,
1967  multibandBestScoreThr,
1968  multibandAngleHardthr,
1969  multibandForceVisible?"false":"true",
1970  outputPath.c_str());
1971  if(util3d::multiBandTexturing(outputPath,
1972  textureMesh->cloud,
1973  textureMesh->tex_polygons[0],
1974  robotPosesFiltered,
1975  vertexToPixels,
1976  std::map<int, cv::Mat >(),
1977  std::map<int, std::vector<CameraModel> >(),
1978  rtabmap.getMemory(),
1979  0,
1980  textureSize,
1981  multibandDownScale,
1982  multibandNbContrib,
1983  "jpg",
1984  gains,
1985  blendingGains,
1986  contrastValues,
1987  doGainCompensationRGB,
1988  multibandUnwrap,
1989  multibandFillHoles,
1990  multibandPadding,
1991  multibandBestScoreThr,
1992  multibandAngleHardthr,
1993  multibandForceVisible))
1994  {
1995  printf("MultiBand texturing...done (%fs).\n", timer.ticks());
1996  }
1997  else
1998  {
1999  printf("MultiBand texturing...failed! (%fs)\n", timer.ticks());
2000  }
2001  }
2002 
2003  // TextureMesh OBJ
2004  bool success = false;
2005  UASSERT(!textures.empty());
2006  for(size_t i=0; i<textureMesh->tex_materials.size(); ++i)
2007  {
2008  textureMesh->tex_materials[i].tex_file += ".jpg";
2009  printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
2010  UASSERT(textures.cols % textures.rows == 0);
2011  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))));
2012  if(!success)
2013  {
2014  UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str());
2015  }
2016  else
2017  {
2018  printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
2019  }
2020  }
2021  if(success)
2022  {
2023  std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj";
2024  printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
2025 #if PCL_VERSION_COMPARE(>=, 1, 13, 0)
2026  textureMesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
2027  auto nr_meshes = static_cast<unsigned>(textureMesh->tex_polygons.size());
2028  unsigned f_idx = 0;
2029  for (unsigned m = 0; m < nr_meshes; m++) {
2030  std::vector<pcl::Vertices> ci = textureMesh->tex_polygons[m];
2031  for(std::size_t i = 0; i < ci.size(); i++) {
2032  for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
2033  ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
2034  }
2035  }
2036  textureMesh->tex_coord_indices.push_back(ci);
2037  f_idx += static_cast<unsigned>(textureMesh->tex_polygons[m].size());
2038  }
2039 #endif
2040  success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
2041 
2042  if(success)
2043  {
2044  printf("Saved obj to %s!\n", outputPath.c_str());
2045  }
2046  else
2047  {
2048  UERROR("Failed saving obj to %s!", outputPath.c_str());
2049  }
2050  }
2051  }
2052  }
2053  }
2054  }
2055  }
2056  }
2057  else
2058  {
2059  printf("Export failed! The cloud is empty.\n");
2060  }
2061 
2062  if(driver)
2063  {
2064  driver->closeConnection();
2065  delete driver;
2066  driver = 0;
2067  }
2068 
2069  return 0;
2070 }
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:1197
UFile::getName
std::string getName()
Definition: UFile.h:135
UINFO
#define UINFO(...)
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::SensorData::uncompressData
void uncompressData()
Definition: SensorData.cpp:528
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
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:602
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:181
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_netvlad.img
img
Definition: rtabmap_netvlad.py:78
showUsage
void showUsage()
Definition: tools/Export/main.cpp:54
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:179
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
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::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
UMath.h
Basic mathematics functions.
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1205
indices
indices
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
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
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
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
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::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
ConsoleProgessState::callback
virtual bool callback(const std::string &msg) const
Definition: tools/Export/main.cpp:139
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
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
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::Signature::getStamp
double getStamp() const
Definition: Signature.h:79
rtabmap::graph::computeMinMax
void RTABMAP_CORE_EXPORT computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2440
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::util3d::adjustNormalsToViewPoints
void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(const std::map< int, Transform > &poses, const pcl::PointCloud< pcl::PointXYZ >::Ptr &rawCloud, const std::vector< int > &rawCameraIndices, pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, float groundNormalsUp=0.0f)
Definition: util3d_surface.cpp:3679
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
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
Graph.h
iter
iterator iter(handle obj)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
UTimer
Definition: UTimer.h:46
float
float
main
int main(int argc, char *argv[])
Definition: tools/Export/main.cpp:147
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
rtabmap::Rtabmap
Definition: Rtabmap.h:54
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_CORE_EXPORT computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
OptimizerG2O.h
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1191
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
ConsoleProgessState
Definition: tools/Export/main.cpp:137
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::Signature
Definition: Signature.h:48
rtabmap::SensorData::depthOrRightCompressed
const cv::Mat & depthOrRightCompressed() const
Definition: SensorData.h:180
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:1220
msg
msg
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:47