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/optimizer/OptimizerG2O.h>
35 #include <rtabmap/utilite/UMath.h>
36 #include <rtabmap/utilite/UTimer.h>
37 #include <rtabmap/utilite/UFile.h>
38 #include <rtabmap/utilite/UStl.h>
39 #include <pcl/filters/filter.h>
40 #include <pcl/io/ply_io.h>
41 #include <pcl/io/obj_io.h>
42 #include <pcl/common/common.h>
43 #include <pcl/surface/poisson.h>
44 #include <stdio.h>
45 
46 using namespace rtabmap;
47 
48 void showUsage()
49 {
50  printf("\nUsage:\n"
51  "rtabmap-exportCloud [options] database.db\n"
52  "Options:\n"
53  " --mesh Create a mesh.\n"
54  " --texture Create a mesh with texture.\n"
55  " --texture_size # Texture size (default 4096).\n"
56  " --texture_count # Maximum textures generated (default 1).\n"
57  " --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
58  " --texture_d2c Distance to camera policy.\n"
59  " --ba Do global bundle adjustment before assembling the clouds.\n"
60  " --gain # Gain compensation value (default 1, set 0 to disable).\n"
61  " --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n"
62  " --no_blending Disable blending when texturing.\n"
63  " --no_clean Disable cleaning colorless polygons.\n"
64  " --low_gain # Low brightness gain 0-100 (default 0).\n"
65  " --high_gain # High brightness gain 0-100 (default 10).\n"
66  " --multiband Enable multiband texturing (AliceVision dependency required).\n"
67  " --poisson_depth # Set Poisson depth for mesh reconstruction.\n"
68  " --max_polygons # Maximum polygons when creating a mesh (default 500000, set 0 for no limit).\n"
69  " --max_range # Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
70  " --decimation # Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
71  " --voxel # Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
72  " --color_radius # Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
73  " --scan Use laser scan for the point cloud.\n"
74  " --save_in_db Save resulting assembled point cloud or mesh in the database.\n"
75  "\n%s", Parameters::showUsage());
76  ;
77  exit(1);
78 }
79 
80 int main(int argc, char * argv[])
81 {
82  ULogger::setType(ULogger::kTypeConsole);
83  ULogger::setLevel(ULogger::kError);
84 
85  if(argc < 2)
86  {
87  showUsage();
88  }
89 
90  bool mesh = false;
91  bool texture = false;
92  bool ba = false;
93  bool doGainCompensationRGB = true;
94  float gainValue = 1;
95  bool doBlending = true;
96  bool doClean = true;
97  int poissonDepth = 0;
98  int maxPolygons = 500000;
99  int decimation = -1;
100  float maxRange = -1.0f;
101  float voxelSize = -1.0f;
102  int textureSize = 4096;
103  int textureCount = 1;
104  int textureRange = 0;
105  bool distanceToCamPolicy = false;
106  bool multiband = false;
107  float colorRadius = -1.0f;
108  bool cloudFromScan = false;
109  bool saveInDb = false;
110  int lowBrightnessGain = 0;
111  int highBrightnessGain = 10;
112  for(int i=1; i<argc; ++i)
113  {
114  if(std::strcmp(argv[i], "--help") == 0)
115  {
116  showUsage();
117  }
118  else if(std::strcmp(argv[i], "--mesh") == 0)
119  {
120  mesh = true;
121  }
122  else if(std::strcmp(argv[i], "--texture") == 0)
123  {
124  texture = true;
125  }
126  else if(std::strcmp(argv[i], "--texture_size") == 0)
127  {
128  ++i;
129  if(i<argc-1)
130  {
131  textureSize = uStr2Int(argv[i]);
132  UASSERT(textureSize%256==0);
133  }
134  else
135  {
136  showUsage();
137  }
138  }
139  else if(std::strcmp(argv[i], "--texture_count") == 0)
140  {
141  ++i;
142  if(i<argc-1)
143  {
144  textureCount = uStr2Int(argv[i]);
145  }
146  else
147  {
148  showUsage();
149  }
150  }
151  else if(std::strcmp(argv[i], "--texture_range") == 0)
152  {
153  ++i;
154  if(i<argc-1)
155  {
156  textureRange = uStr2Int(argv[i]);
157  }
158  else
159  {
160  showUsage();
161  }
162  }
163  else if(std::strcmp(argv[i], "--texture_d2c") == 0)
164  {
165  distanceToCamPolicy = true;
166  }
167  else if(std::strcmp(argv[i], "--ba") == 0)
168  {
169  ba = true;
170  }
171  else if(std::strcmp(argv[i], "--gain_gray") == 0)
172  {
173  doGainCompensationRGB = false;
174  }
175  else if(std::strcmp(argv[i], "--gain") == 0)
176  {
177  ++i;
178  if(i<argc-1)
179  {
180  gainValue = uStr2Float(argv[i]);
181  UASSERT(gainValue>0.0f);
182  }
183  else
184  {
185  showUsage();
186  }
187  }
188  else if(std::strcmp(argv[i], "--no_blending") == 0)
189  {
190  doBlending = false;
191  }
192  else if(std::strcmp(argv[i], "--no_clean") == 0)
193  {
194  doClean = false;
195  }
196  else if(std::strcmp(argv[i], "--multiband") == 0)
197  {
198 #ifdef RTABMAP_ALICE_VISION
199  multiband = true;
200 #else
201  printf("\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
202 #endif
203  }
204  else if(std::strcmp(argv[i], "--poisson_depth") == 0)
205  {
206  ++i;
207  if(i<argc-1)
208  {
209  poissonDepth = uStr2Int(argv[i]);
210  }
211  else
212  {
213  showUsage();
214  }
215  }
216  else if(std::strcmp(argv[i], "--max_polygons") == 0)
217  {
218  ++i;
219  if(i<argc-1)
220  {
221  maxPolygons = uStr2Int(argv[i]);
222  }
223  else
224  {
225  showUsage();
226  }
227  }
228  else if(std::strcmp(argv[i], "--max_range") == 0)
229  {
230  ++i;
231  if(i<argc-1)
232  {
233  maxRange = uStr2Float(argv[i]);
234  }
235  else
236  {
237  showUsage();
238  }
239  }
240  else if(std::strcmp(argv[i], "--decimation") == 0)
241  {
242  ++i;
243  if(i<argc-1)
244  {
245  decimation = uStr2Int(argv[i]);
246  }
247  else
248  {
249  showUsage();
250  }
251  }
252  else if(std::strcmp(argv[i], "--voxel") == 0)
253  {
254  ++i;
255  if(i<argc-1)
256  {
257  voxelSize = uStr2Float(argv[i]);
258  }
259  else
260  {
261  showUsage();
262  }
263  }
264  else if(std::strcmp(argv[i], "--color_radius") == 0)
265  {
266  ++i;
267  if(i<argc-1)
268  {
269  colorRadius = uStr2Float(argv[i]);
270  }
271  else
272  {
273  showUsage();
274  }
275  }
276  else if(std::strcmp(argv[i], "--scan") == 0)
277  {
278  cloudFromScan = true;
279  }
280  else if(std::strcmp(argv[i], "--save_in_db") == 0)
281  {
282  saveInDb = true;
283  }
284  else if(std::strcmp(argv[i], "--low_gain") == 0)
285  {
286  ++i;
287  if(i<argc-1)
288  {
289  lowBrightnessGain = uStr2Int(argv[i]);
290  }
291  else
292  {
293  showUsage();
294  }
295  }
296  else if(std::strcmp(argv[i], "--high_gain") == 0)
297  {
298  ++i;
299  if(i<argc-1)
300  {
301  highBrightnessGain = uStr2Int(argv[i]);
302  }
303  else
304  {
305  showUsage();
306  }
307  }
308  }
309 
310  if(decimation < 1)
311  {
312  decimation = cloudFromScan?1:4;
313  }
314  if(maxRange < 0)
315  {
316  maxRange = cloudFromScan?0:4;
317  }
318  if(voxelSize < 0.0f)
319  {
320  voxelSize = cloudFromScan?0:0.01f;
321  }
322  if(colorRadius < 0.0f)
323  {
324  colorRadius = cloudFromScan?0:0.05f;
325  }
326 
327  if(saveInDb)
328  {
329  if(multiband)
330  {
331  printf("Option --multiband is not supported with --save_in_db option, disabling multiband...\n");
332  multiband = false;
333  }
334  if(textureCount>1)
335  {
336  printf("Option --texture_count > 1 is not supported with --save_in_db option, setting texture_count to 1...\n");
337  textureCount = 1;
338  }
339  }
340 
341  ParametersMap params = Parameters::parseArguments(argc, argv, false);
342 
343  std::string dbPath = argv[argc-1];
344 
345  // Get parameters
346  ParametersMap parameters;
347  DBDriver * driver = DBDriver::create();
348  if(driver->openConnection(dbPath))
349  {
350  parameters = driver->getLastParameters();
351  driver->closeConnection(false);
352  }
353  else
354  {
355  UERROR("Cannot open database %s!", dbPath.c_str());
356  return -1;
357  }
358  delete driver;
359  driver = 0;
360 
361  for(ParametersMap::iterator iter=params.begin(); iter!=params.end(); ++iter)
362  {
363  printf("Added custom parameter %s=%s\n",iter->first.c_str(), iter->second.c_str());
364  }
365 
366  UTimer timer;
367 
368  printf("Loading database \"%s\"...\n", dbPath.c_str());
369  // Get the global optimized map
370  Rtabmap rtabmap;
371  uInsert(parameters, params);
372  rtabmap.init(parameters, dbPath);
373  printf("Loading database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks());
374 
375  std::map<int, Signature> nodes;
376  std::map<int, Transform> optimizedPoses;
377  std::multimap<int, Link> links;
378  printf("Optimizing the map...\n");
379  rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
380  printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size());
381 
382  if(optimizedPoses.empty())
383  {
384  printf("The optimized graph is empty!? Aborting...\n");
385  return -1;
386  }
387 
388  std::string outputDirectory = UDirectory::getDir(dbPath);
389  std::string baseName = uSplit(UFile::getName(dbPath), '.').front();
390 
391  if(ba)
392  {
393  printf("Global bundle adjustment...\n");
394  OptimizerG2O g2o(parameters);
395  std::map<int, cv::Point3f> points3DMap;
396  std::map<int, std::map<int, FeatureBA> > wordReferences;
397  g2o.computeBACorrespondences(optimizedPoses, links, nodes, points3DMap, wordReferences, true);
398  std::map<int, rtabmap::CameraModel> cameraSingleModels;
399  for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
400  {
401  Signature node = nodes.find(iter->first)->second;
402  UASSERT(node.sensorData().cameraModels().size()==1);
403  cameraSingleModels.insert(std::make_pair(iter->first, node.sensorData().cameraModels().front()));
404  }
405  optimizedPoses = g2o.optimizeBA(optimizedPoses.begin()->first, optimizedPoses, links, cameraSingleModels, points3DMap, wordReferences);
406  printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
407  }
408 
409  // Construct the cloud
410  printf("Create and assemble the clouds...\n");
411  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
412  std::map<int, rtabmap::Transform> cameraPoses;
413  std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
414  std::map<int, cv::Mat> cameraDepths;
415 
416  for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
417  {
418  Signature node = nodes.find(iter->first)->second;
419 
420  // uncompress data
421  std::vector<CameraModel> models = node.sensorData().cameraModels();
422  cv::Mat depth;
423 
424  pcl::IndicesPtr indices(new std::vector<int>);
425  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
426  if(cloudFromScan)
427  {
428  cv::Mat tmpDepth;
429  LaserScan scan;
430  node.sensorData().uncompressData(0, texture&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
431  if(decimation>1 || maxRange)
432  {
433  scan = util3d::commonFiltering(scan, decimation, 0, maxRange);
434  }
435  cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
436  }
437  else
438  {
439  cv::Mat tmpRGB;
440  node.sensorData().uncompressData(&tmpRGB, &depth);
441  cloud = util3d::cloudRGBFromSensorData(
442  node.sensorData(),
443  decimation, // image decimation before creating the clouds
444  maxRange, // maximum depth of the cloud
445  0.0f,
446  indices.get());
447  }
448  if(voxelSize>0.0f)
449  {
450  cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
451  }
452  cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
453 
454  Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
455  if(cloudFromScan)
456  {
457  Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
458  viewpoint = Eigen::Vector3f(iter->second.x(), iter->second.y(), iter->second.z());
459  }
460  pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
461 
462  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
463  pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
464 
465  if(mergedClouds->size() == 0)
466  {
467  *mergedClouds = *cloudWithNormals;
468  }
469  else
470  {
471  *mergedClouds += *cloudWithNormals;
472  }
473 
474  cameraPoses.insert(std::make_pair(iter->first, iter->second));
475  if(!models.empty())
476  {
477  cameraModels.insert(std::make_pair(iter->first, models));
478  }
479  if(!depth.empty())
480  {
481  cameraDepths.insert(std::make_pair(iter->first, depth));
482  }
483  }
484  printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), (int)mergedClouds->size());
485 
486  if(mergedClouds->size())
487  {
488  if(saveInDb)
489  {
490  driver = DBDriver::create();
491  UASSERT(driver->openConnection(dbPath, false));
492  Transform lastlocalizationPose;
493  driver->loadOptimizedPoses(&lastlocalizationPose);
494  //optimized poses have changed, reset 2d map
495  driver->save2DMap(cv::Mat(), 0, 0, 0);
496  driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
497  }
498 
499  if(!(mesh || texture))
500  {
501  if(voxelSize>0.0f)
502  {
503  printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", voxelSize, (int)mergedClouds->size());
504  mergedClouds = util3d::voxelize(mergedClouds, voxelSize);
505  }
506 
507  if(saveInDb)
508  {
509  printf("Saving in db... (%d points)\n", (int)mergedClouds->size());
510  driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*mergedClouds, Transform(), false));
511  printf("Saving in db... done!\n");
512  }
513  else
514  {
515  std::string outputPath=outputDirectory+"/"+baseName+"_cloud.ply";
516  printf("Saving %s... (%d points)\n", outputPath.c_str(), (int)mergedClouds->size());
517  pcl::io::savePLYFile(outputPath, *mergedClouds);
518  printf("Saving %s... done!\n", outputPath.c_str());
519  }
520  }
521  else
522  {
523  Eigen::Vector4f min,max;
524  pcl::getMinMax3D(*mergedClouds, min, max);
525  float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
526  int optimizedDepth = 12;
527  for(int i=6; i<12; ++i)
528  {
529  if(mapLength/float(1<<i) < 0.03f)
530  {
531  optimizedDepth = i;
532  break;
533  }
534  }
535  if(poissonDepth>0)
536  {
537  optimizedDepth = poissonDepth;
538  }
539 
540  // Mesh reconstruction
541  printf("Mesh reconstruction... depth=%d\n", optimizedDepth);
542  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
543  pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
544  poisson.setDepth(optimizedDepth);
545  poisson.setInputCloud(mergedClouds);
546  poisson.reconstruct(*mesh);
547  printf("Mesh reconstruction... done (%fs, %d polygons).\n", timer.ticks(), (int)mesh->polygons.size());
548 
549  if(mesh->polygons.size())
550  {
551  rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
552  mesh,
553  0.0f,
554  maxPolygons,
555  mergedClouds,
556  colorRadius,
557  !texture,
558  doClean,
559  200);
560 
561  if(!texture)
562  {
563  if(saveInDb)
564  {
565  printf("Saving mesh in db...\n");
566  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
567  polygons.push_back(util3d::convertPolygonsFromPCL(mesh->polygons));
568  driver->saveOptimizedMesh(
569  util3d::laserScanFromPointCloud(mesh->cloud, false).data(),
570  polygons);
571  printf("Saving mesh in db... done!\n");
572  }
573  else
574  {
575  std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
576  printf("Saving %s...\n", outputPath.c_str());
577  pcl::io::savePLYFile(outputPath, *mesh);
578  printf("Saving %s... done!\n", outputPath.c_str());
579  }
580  }
581  else
582  {
583  printf("Texturing %d polygons... cameraPoses=%d, cameraDepths=%d\n", (int)mesh->polygons.size(), (int)cameraPoses.size(), (int)cameraDepths.size());
584  std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
585  pcl::TextureMeshPtr textureMesh = rtabmap::util3d::createTextureMesh(
586  mesh,
587  cameraPoses,
588  cameraModels,
589  cameraDepths,
590  textureRange,
591  0.0f,
592  0.0f,
593  multiband?0:50, // Min polygons in camera view to be textured by this camera
594  std::vector<float>(),
595  0,
596  &vertexToPixels,
597  distanceToCamPolicy);
598  printf("Texturing... done (%fs).\n", timer.ticks());
599 
600  // Remove occluded polygons (polygons with no texture)
601  if(doClean && textureMesh->tex_coordinates.size())
602  {
603  printf("Cleanup mesh...\n");
604  rtabmap::util3d::cleanTextureMesh(*textureMesh, 100); // Min polygons in a cluster to keep them
605  printf("Cleanup mesh... done (%fs).\n", timer.ticks());
606  }
607 
608  if(textureMesh->tex_materials.size())
609  {
610  if(multiband)
611  {
612  printf("Merging %d texture(s) to single one (multiband enabled)...\n", (int)textureMesh->tex_materials.size());
613  }
614  else
615  {
616  printf("Merging %d texture(s)... (%d max textures)\n", (int)textureMesh->tex_materials.size(), textureCount);
617  }
618  std::map<int, std::map<int, cv::Vec4d> > gains;
619  std::map<int, std::map<int, cv::Mat> > blendingGains;
620  std::pair<float, float> contrastValues(0,0);
621  cv::Mat textures = rtabmap::util3d::mergeTextures(
622  *textureMesh,
623  std::map<int, cv::Mat>(),
624  std::map<int, std::vector<rtabmap::CameraModel> >(),
625  rtabmap.getMemory(),
626  0,
627  textureSize,
628  multiband?1:textureCount, // to get contrast values based on all images in multiband mode
629  vertexToPixels,
630  gainValue>0.0f, gainValue, doGainCompensationRGB,
631  doBlending, 0,
632  lowBrightnessGain, highBrightnessGain, // low-high brightness/contrast balance
633  false, // exposure fusion
634  0, // state
635  0, // blank value (0=black)
636  &gains,
637  &blendingGains,
638  &contrastValues);
639  printf("Merging to %d texture(s)... done (%fs).\n", (int)textureMesh->tex_materials.size(), timer.ticks());
640 
641  if(saveInDb)
642  {
643  printf("Saving texture mesh in db...\n");
644  driver->saveOptimizedMesh(
645  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
646  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
647  textureMesh->tex_coordinates,
648  textures);
649  printf("Saving texture mesh in db... done!\n");
650  }
651  else
652  {
653  // TextureMesh OBJ
654  bool success = false;
655  UASSERT(!textures.empty());
656  for(size_t i=0; i<textureMesh->tex_materials.size(); ++i)
657  {
658  textureMesh->tex_materials[i].tex_file += ".jpg";
659  printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
660  UASSERT(textures.cols % textures.rows == 0);
661  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))));
662  if(!success)
663  {
664  UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str());
665  }
666  else
667  {
668  printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
669  }
670  }
671  if(success)
672  {
673 
674  std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj";
675  printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
676  success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
677 
678  if(success)
679  {
680  printf("Saved obj to %s!\n", outputPath.c_str());
681  }
682  else
683  {
684  UERROR("Failed saving obj to %s!", outputPath.c_str());
685  }
686  }
687  }
688 
689  if(multiband)
690  {
691  timer.restart();
692  std::string outputPath=outputDirectory+"/"+baseName+"_mesh_multiband.obj";
693  printf("MultiBand texturing... \"%s\"\n", outputPath.c_str());
694  if(util3d::multiBandTexturing(outputPath,
695  textureMesh->cloud,
696  textureMesh->tex_polygons[0],
697  cameraPoses,
698  vertexToPixels,
699  std::map<int, cv::Mat >(),
700  std::map<int, std::vector<CameraModel> >(),
701  rtabmap.getMemory(),
702  0,
703  textureSize,
704  "jpg",
705  gains,
706  blendingGains,
707  contrastValues,
708  doGainCompensationRGB))
709  {
710  printf("MultiBand texturing...done (%fs).\n", timer.ticks());
711  }
712  else
713  {
714  printf("MultiBand texturing...failed! (%fs)\n", timer.ticks());
715  }
716  }
717  }
718  }
719  }
720  }
721  }
722  else
723  {
724  printf("Export failed! The cloud is empty.\n");
725  }
726 
727  if(driver)
728  {
729  driver->closeConnection();
730  delete driver;
731  driver = 0;
732  }
733 
734  return 0;
735 }
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:118
UTimer
Definition: UTimer.h:46
UFile::getName
std::string getName()
Definition: UFile.h:135
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:88
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1181
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::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2666
f
f
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:548
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
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
rtabmap::OptimizerG2O::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: OptimizerG2O.cpp:1246
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, 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:2214
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:575
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UMath.h
Basic mathematics functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
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.
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:2417
main
int main(int argc, char *argv[])
Definition: tools/Export/main.cpp:80
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
rtabmap::SensorData::depthOrRightCompressed
const cv::Mat & depthOrRightCompressed() const
Definition: SensorData.h:161
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
rtabmap::DBDriver::closeConnection
void closeConnection(bool save=true, const std::string &outputUrl="")
Definition: DBDriver.cpp:64
util3d_filtering.h
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:1031
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:98
util3d_transforms.h
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:1434
rtabmap::util3d::convertPolygonsFromPCL
std::vector< std::vector< RTABMAP_PCL_INDEX > > RTABMAP_EXP convertPolygonsFromPCL(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:1223
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:671
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
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:869
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::Optimizer::computeBACorrespondences
void computeBACorrespondences(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, Signature > &signatures, std::map< int, cv::Point3f > &points3DMap, std::map< int, std::map< int, FeatureBA > > &wordReferences, bool rematchFeatures=false)
Definition: Optimizer.cpp:554
showUsage
void showUsage()
Definition: tools/Export/main.cpp:48
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:519
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1195
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
util3d_surface.h
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1187
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())
Definition: util3d.cpp:1266

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59