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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29