util3d_surface.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/util3d_surface.h"
29 #include "rtabmap/core/util3d_filtering.h"
30 #include "rtabmap/core/util3d_transforms.h"
31 #include "rtabmap/core/util3d.h"
32 #include "rtabmap/core/util2d.h"
33 #include "rtabmap/core/Memory.h"
34 #include "rtabmap/core/DBDriver.h"
35 #include "rtabmap/core/Compression.h"
36 #include "rtabmap/utilite/ULogger.h"
37 #include "rtabmap/utilite/UDirectory.h"
38 #include "rtabmap/utilite/UFile.h"
39 #include "rtabmap/utilite/UConversion.h"
40 #include "rtabmap/utilite/UMath.h"
41 #include "rtabmap/utilite/UTimer.h"
42 #include <opencv2/core/core_c.h>
43 #include <opencv2/imgproc/types_c.h>
44 #include <pcl/search/kdtree.h>
45 #include <pcl/surface/gp3.h>
46 #include <pcl/features/normal_3d_omp.h>
47 #include <pcl/surface/mls.h>
48 #include <pcl18/surface/texture_mapping.h>
49 #include <pcl/features/integral_image_normal.h>
50 
51 #ifdef RTABMAP_ALICE_VISION
52 #include <aliceVision/sfmData/SfMData.hpp>
53 #include <aliceVision/sfmDataIO/sfmDataIO.hpp>
54 #include <aliceVision/mesh/Mesh.hpp>
55 #include <aliceVision/mesh/Texturing.hpp>
56 #include <aliceVision/camera/Pinhole.hpp>
57 #include <boost/algorithm/string.hpp>
58 using namespace aliceVision;
59 #endif
60 
61 #ifndef DISABLE_VTK
62 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
63 #endif
64 
65 #if PCL_VERSION_COMPARE(>, 1, 11, 1)
66 #include <pcl/types.h>
67 #endif
68 #if PCL_VERSION_COMPARE(<, 1, 8, 0)
69 #include "pcl18/surface/organized_fast_mesh.h"
70 #else
71 #include <pcl/surface/organized_fast_mesh.h>
72 #include <pcl/surface/impl/marching_cubes.hpp>
73 #include <pcl/surface/impl/organized_fast_mesh.hpp>
74 #include <pcl/impl/instantiate.hpp>
75 #include <pcl/point_types.h>
76 
77 // Instantiations of specific point types
78 PCL_INSTANTIATE(OrganizedFastMesh, (pcl::PointXYZRGBNormal))
79 
80 #include <pcl/features/impl/normal_3d_omp.hpp>
81 #if PCL_VERSION_COMPARE(<=, 1, 8, 0)
82 #ifdef PCL_ONLY_CORE_POINT_TYPES
83 PCL_INSTANTIATE_PRODUCT(NormalEstimationOMP, ((pcl::PointXYZRGB))((pcl::Normal)))
84 #endif
85 #endif
86 #endif
87 
88 namespace rtabmap
89 {
90 
91 namespace util3d
92 {
93 
94 void createPolygonIndexes(
95  const std::vector<pcl::Vertices> & polygons,
96  int cloudSize,
97  std::vector<std::set<int> > & neighbors,
98  std::vector<std::set<int> > & vertexToPolygons)
99 {
100  vertexToPolygons = std::vector<std::set<int> >(cloudSize);
101  neighbors = std::vector<std::set<int> >(polygons.size());
102 
103  for(unsigned int i=0; i<polygons.size(); ++i)
104  {
105  std::set<int> vertices(polygons[i].vertices.begin(), polygons[i].vertices.end());
106 
107  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
108  {
109  int v = polygons[i].vertices.at(j);
110  for(std::set<int>::iterator iter=vertexToPolygons[v].begin(); iter!=vertexToPolygons[v].end(); ++iter)
111  {
112  int numSharedVertices = 0;
113  for(unsigned int k=0; k<polygons.at(*iter).vertices.size() && numSharedVertices<2; ++k)
114  {
115  if(vertices.find(polygons.at(*iter).vertices.at(k)) != vertices.end())
116  {
117  ++numSharedVertices;
118  }
119  }
120  if(numSharedVertices >= 2)
121  {
122  neighbors[*iter].insert(i);
123  neighbors[i].insert(*iter);
124  }
125  }
126  vertexToPolygons[v].insert(i);
127  }
128  }
129 }
130 
131 std::list<std::list<int> > clusterPolygons(
132  const std::vector<std::set<int> > & neighborPolygons,
133  int minClusterSize)
134 {
135  std::set<int> polygonsChecked;
136 
137  std::list<std::list<int> > clusters;
138 
139  for(unsigned int i=0; i<neighborPolygons.size(); ++i)
140  {
141  if(polygonsChecked.find(i) == polygonsChecked.end())
142  {
143  std::list<int> currentCluster;
144  currentCluster.push_back(i);
145  polygonsChecked.insert(i);
146 
147  for(std::list<int>::iterator iter=currentCluster.begin(); iter!=currentCluster.end(); ++iter)
148  {
149  // get neighbor polygons
150  std::set<int> neighbors = neighborPolygons[*iter];
151  for(std::set<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
152  {
153  if(polygonsChecked.insert(*jter).second)
154  {
155  currentCluster.push_back(*jter);
156  }
157  }
158  }
159  if((int)currentCluster.size() > minClusterSize)
160  {
161  clusters.push_back(currentCluster);
162  }
163  }
164  }
165  return clusters;
166 }
167 
168 std::vector<pcl::Vertices> organizedFastMesh(
169  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
170  double angleTolerance,
171  bool quad,
172  int trianglePixelSize,
173  const Eigen::Vector3f & viewpoint)
174 {
175  UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
176  UASSERT(cloud->is_dense == false);
177  UASSERT(cloud->width > 1 && cloud->height > 1);
178 
179  pcl::OrganizedFastMesh<pcl::PointXYZ> ofm;
180  ofm.setTrianglePixelSize (trianglePixelSize);
181  ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZ>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_RIGHT_CUT);
182  ofm.setInputCloud (cloud);
183  ofm.setAngleTolerance(angleTolerance);
184  ofm.setViewpoint(viewpoint);
185 
186  std::vector<pcl::Vertices> vertices;
187  ofm.reconstruct (vertices);
188 
189  if(quad)
190  {
191  //flip all polygons (right handed)
192  std::vector<pcl::Vertices> output(vertices.size());
193  for(unsigned int i=0; i<vertices.size(); ++i)
194  {
195  output[i].vertices.resize(4);
196  output[i].vertices[0] = vertices[i].vertices[0];
197  output[i].vertices[3] = vertices[i].vertices[1];
198  output[i].vertices[2] = vertices[i].vertices[2];
199  output[i].vertices[1] = vertices[i].vertices[3];
200  }
201  return output;
202  }
203 
204  return vertices;
205 }
206 std::vector<pcl::Vertices> organizedFastMesh(
207  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
208  double angleTolerance,
209  bool quad,
210  int trianglePixelSize,
211  const Eigen::Vector3f & viewpoint)
212 {
213  UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
214  UASSERT(cloud->is_dense == false);
215  UASSERT(cloud->width > 1 && cloud->height > 1);
216 
217  pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
218  ofm.setTrianglePixelSize (trianglePixelSize);
219  ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
220  ofm.setInputCloud (cloud);
221  ofm.setAngleTolerance(angleTolerance);
222  ofm.setViewpoint(viewpoint);
223 
224  std::vector<pcl::Vertices> vertices;
225  ofm.reconstruct (vertices);
226 
227  if(quad)
228  {
229  //flip all polygons (right handed)
230  std::vector<pcl::Vertices> output(vertices.size());
231  for(unsigned int i=0; i<vertices.size(); ++i)
232  {
233  output[i].vertices.resize(4);
234  output[i].vertices[0] = vertices[i].vertices[0];
235  output[i].vertices[3] = vertices[i].vertices[1];
236  output[i].vertices[2] = vertices[i].vertices[2];
237  output[i].vertices[1] = vertices[i].vertices[3];
238  }
239  return output;
240  }
241 
242  return vertices;
243 }
244 std::vector<pcl::Vertices> organizedFastMesh(
245  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
246  double angleTolerance,
247  bool quad,
248  int trianglePixelSize,
249  const Eigen::Vector3f & viewpoint)
250 {
251  UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
252  UASSERT(cloud->is_dense == false);
253  UASSERT(cloud->width > 1 && cloud->height > 1);
254 
255  pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal> ofm;
256  ofm.setTrianglePixelSize (trianglePixelSize);
257  ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
258  ofm.setInputCloud (cloud);
259  ofm.setAngleTolerance(angleTolerance);
260  ofm.setViewpoint(viewpoint);
261 
262  std::vector<pcl::Vertices> vertices;
263  ofm.reconstruct (vertices);
264 
265  if(quad)
266  {
267  //flip all polygons (right handed)
268  std::vector<pcl::Vertices> output(vertices.size());
269  for(unsigned int i=0; i<vertices.size(); ++i)
270  {
271  output[i].vertices.resize(4);
272  output[i].vertices[0] = vertices[i].vertices[0];
273  output[i].vertices[3] = vertices[i].vertices[1];
274  output[i].vertices[2] = vertices[i].vertices[2];
275  output[i].vertices[1] = vertices[i].vertices[3];
276  }
277  return output;
278  }
279 
280  return vertices;
281 }
282 
283 void appendMesh(
284  pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
285  std::vector<pcl::Vertices> & polygonsA,
286  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
287  const std::vector<pcl::Vertices> & polygonsB)
288 {
289  UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
290  UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
291 
292  int sizeA = (int)cloudA.size();
293  cloudA += cloudB;
294 
295  int sizePolygonsA = (int)polygonsA.size();
296  polygonsA.resize(sizePolygonsA+polygonsB.size());
297 
298  for(unsigned int i=0; i<polygonsB.size(); ++i)
299  {
300  pcl::Vertices vertices = polygonsB[i];
301  for(unsigned int j=0; j<vertices.vertices.size(); ++j)
302  {
303  vertices.vertices[j] += sizeA;
304  }
305  polygonsA[i+sizePolygonsA] = vertices;
306  }
307 }
308 
309 void appendMesh(
310  pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
311  std::vector<pcl::Vertices> & polygonsA,
312  const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
313  const std::vector<pcl::Vertices> & polygonsB)
314 {
315  UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
316  UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
317 
318  int sizeA = (int)cloudA.size();
319  cloudA += cloudB;
320 
321  int sizePolygonsA = (int)polygonsA.size();
322  polygonsA.resize(sizePolygonsA+polygonsB.size());
323 
324  for(unsigned int i=0; i<polygonsB.size(); ++i)
325  {
326  pcl::Vertices vertices = polygonsB[i];
327  for(unsigned int j=0; j<vertices.vertices.size(); ++j)
328  {
329  vertices.vertices[j] += sizeA;
330  }
331  polygonsA[i+sizePolygonsA] = vertices;
332  }
333 }
334 
335 std::vector<int> filterNotUsedVerticesFromMesh(
336  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
337  const std::vector<pcl::Vertices> & polygons,
338  pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
339  std::vector<pcl::Vertices> & outputPolygons)
340 {
341  UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
342  std::map<int, int> addedVertices; //<oldIndex, newIndex>
343  std::vector<int> output; //<oldIndex>
344  output.resize(cloud.size());
345  outputCloud.resize(cloud.size());
346  outputCloud.is_dense = true;
347  outputPolygons.resize(polygons.size());
348  int oi = 0;
349  for(unsigned int i=0; i<polygons.size(); ++i)
350  {
351  pcl::Vertices & v = outputPolygons[i];
352  v.vertices.resize(polygons[i].vertices.size());
353  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
354  {
355  std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
356  if(iter == addedVertices.end())
357  {
358  outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
359  addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
360  output[oi] = polygons[i].vertices[j];
361  v.vertices[j] = oi++;
362  }
363  else
364  {
365  v.vertices[j] = iter->second;
366  }
367  }
368  }
369  outputCloud.resize(oi);
370  output.resize(oi);
371 
372  return output;
373 }
374 
375 std::vector<int> filterNotUsedVerticesFromMesh(
376  const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
377  const std::vector<pcl::Vertices> & polygons,
378  pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
379  std::vector<pcl::Vertices> & outputPolygons)
380 {
381  UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
382  std::map<int, int> addedVertices; //<oldIndex, newIndex>
383  std::vector<int> output; //<oldIndex>
384  output.resize(cloud.size());
385  outputCloud.resize(cloud.size());
386  outputCloud.is_dense = true;
387  outputPolygons.resize(polygons.size());
388  int oi = 0;
389  for(unsigned int i=0; i<polygons.size(); ++i)
390  {
391  pcl::Vertices & v = outputPolygons[i];
392  v.vertices.resize(polygons[i].vertices.size());
393  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
394  {
395  std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
396  if(iter == addedVertices.end())
397  {
398  outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
399  addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
400  output[oi] = polygons[i].vertices[j];
401  v.vertices[j] = oi++;
402  }
403  else
404  {
405  v.vertices[j] = iter->second;
406  }
407  }
408  }
409  outputCloud.resize(oi);
410  output.resize(oi);
411 
412  return output;
413 }
414 
415 std::vector<int> filterNaNPointsFromMesh(
416  const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
417  const std::vector<pcl::Vertices> & polygons,
418  pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
419  std::vector<pcl::Vertices> & outputPolygons)
420 {
421  UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
422  std::map<int, int> addedVertices; //<oldIndex, newIndex>
423  std::vector<int> output; //<oldIndex>
424  output.resize(cloud.size());
425  outputCloud.resize(cloud.size());
426  outputCloud.is_dense = true;
427  std::vector<int> organizedToDense(cloud.size(), -1);
428 
429  int oi = 0;
430  for(unsigned int i=0; i<cloud.size(); ++i)
431  {
432  if(pcl::isFinite(cloud.at(i)))
433  {
434  outputCloud.at(oi) = cloud.at(i);
435  output[oi] = i;
436  organizedToDense[i] = oi;
437  ++oi;
438  }
439  }
440  outputCloud.resize(oi);
441  output.resize(oi);
442 
443  // remap polygons to dense cloud
444  outputPolygons = polygons;
445  for(unsigned int i=0; i<outputPolygons.size(); ++i)
446  {
447  pcl::Vertices & v = outputPolygons[i];
448  for(unsigned int j=0; j<v.vertices.size(); ++j)
449  {
450  UASSERT(organizedToDense[v.vertices[j]] >= 0);
451  v.vertices[j] = organizedToDense[v.vertices[j]];
452  }
453  }
454 
455  return output;
456 }
457 
458 std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
459  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
460  const std::vector<pcl::Vertices> & polygons,
461  float radius,
462  float angle, // FIXME angle not used
463  bool keepLatestInRadius)
464 {
465  UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
466  (int)cloud->size(), (int)polygons.size(), radius, angle, keepLatestInRadius?1:0);
467  std::vector<pcl::Vertices> outputPolygons;
468  pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
469  kdtree->setInputCloud(cloud);
470 
471  std::map<int, int> verticesDone;
472  outputPolygons = polygons;
473  for(unsigned int i=0; i<outputPolygons.size(); ++i)
474  {
475  pcl::Vertices & polygon = outputPolygons[i];
476  for(unsigned int j=0; j<polygon.vertices.size(); ++j)
477  {
478  std::map<int, int>::iterator iter = verticesDone.find(polygon.vertices[j]);
479  if(iter != verticesDone.end())
480  {
481  polygon.vertices[j] = iter->second;
482  }
483  else
484  {
485  std::vector<int> kIndices;
486  std::vector<float> kDistances;
487  kdtree->radiusSearch(polygon.vertices[j], radius, kIndices, kDistances);
488  if(kIndices.size())
489  {
490  int reference = -1;
491  for(unsigned int z=0; z<kIndices.size(); ++z)
492  {
493  if(reference == -1)
494  {
495  reference = kIndices[z];
496  }
497  else if(keepLatestInRadius)
498  {
499  if(kIndices[z] < reference)
500  {
501  reference = kIndices[z];
502  }
503  }
504  else
505  {
506  if(kIndices[z] > reference)
507  {
508  reference = kIndices[z];
509  }
510  }
511  }
512  if(reference >= 0)
513  {
514  for(unsigned int z=0; z<kIndices.size(); ++z)
515  {
516  verticesDone.insert(std::make_pair(kIndices[j], reference));
517  }
518  polygon.vertices[j] = reference;
519  }
520  }
521  else
522  {
523  verticesDone.insert(std::make_pair(polygon.vertices[j], polygon.vertices[j]));
524  }
525  }
526  }
527  }
528  return outputPolygons;
529 }
530 
531 std::vector<pcl::Vertices> filterInvalidPolygons(const std::vector<pcl::Vertices> & polygons)
532 {
533  std::vector<pcl::Vertices> output(polygons.size());
534  int oi=0;
535  for(unsigned int i=0; i<polygons.size(); ++i)
536  {
537  bool valid = true;
538  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
539  {
540  if(polygons[i].vertices[j] == polygons[i].vertices[(j+1)%polygons[i].vertices.size()])
541  {
542  valid = false;
543  break;
544  }
545  }
546  if(valid)
547  {
548  output[oi++] = polygons[i];
549  }
550  }
551  output.resize(oi);
552  return output;
553 }
554 
555 pcl::PolygonMesh::Ptr createMesh(
556  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
557  float gp3SearchRadius,
558  float gp3Mu,
559  int gp3MaximumNearestNeighbors,
560  float gp3MaximumSurfaceAngle,
561  float gp3MinimumAngle,
562  float gp3MaximumAngle,
563  bool gp3NormalConsistency)
564 {
565  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
566 
567  // Create search tree*
568  pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
569  tree2->setInputCloud (cloudWithNormalsNoNaN);
570 
571  // Initialize objects
572  pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
573  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
574 
575  // Set the maximum distance between connected points (maximum edge length)
576  gp3.setSearchRadius (gp3SearchRadius);
577 
578  // Set typical values for the parameters
579  gp3.setMu (gp3Mu);
580  gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
581  gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle); // 45 degrees
582  gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
583  gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
584  gp3.setNormalConsistency(gp3NormalConsistency);
585  gp3.setConsistentVertexOrdering(gp3NormalConsistency);
586 
587  // Get result
588  gp3.setInputCloud (cloudWithNormalsNoNaN);
589  gp3.setSearchMethod (tree2);
590  gp3.reconstruct (*mesh);
591 
592  //UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size());
593  //mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons);
594 
595  return mesh;
596 }
597 
598 pcl::texture_mapping::CameraVector createTextureCameras(
599  const std::map<int, Transform> & poses,
600  const std::map<int, std::vector<CameraModel> > & cameraModels,
601  const std::map<int, cv::Mat> & cameraDepths,
602  const std::vector<float> & roiRatios)
603 {
604  UASSERT(roiRatios.empty() || roiRatios.size() == 4);
605  pcl::texture_mapping::CameraVector cameras;
606 
607  for(std::map<int, Transform>::const_iterator poseIter=poses.begin(); poseIter!=poses.end(); ++poseIter)
608  {
609  std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.find(poseIter->first);
610 
611  if(modelIter!=cameraModels.end())
612  {
613  std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);
614 
615  // for each sub camera
616  for(unsigned int i=0; i<modelIter->second.size(); ++i)
617  {
618  pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
619  // should be in camera frame
620  UASSERT(!modelIter->second[i].localTransform().isNull() && !poseIter->second.isNull());
621  Transform t = poseIter->second*modelIter->second[i].localTransform();
622 
623  cam.pose = t.toEigen3f();
624 
625  if(modelIter->second[i].imageHeight() <=0 || modelIter->second[i].imageWidth() <=0)
626  {
627  UERROR("Should have camera models with width/height set to create texture cameras!");
628  return pcl::texture_mapping::CameraVector();
629  }
630 
631  UASSERT(modelIter->second[i].fx()>0 && modelIter->second[i].imageHeight()>0 && modelIter->second[i].imageWidth()>0);
632  cam.focal_length_w=modelIter->second[i].fx();
633  cam.focal_length_h=modelIter->second[i].fy();
634  cam.center_w=modelIter->second[i].cx();
635  cam.center_h=modelIter->second[i].cy();
636  cam.height=modelIter->second[i].imageHeight();
637  cam.width=modelIter->second[i].imageWidth();
638  if(modelIter->second.size() == 1)
639  {
640  cam.texture_file = uFormat("%d", poseIter->first); // camera index
641  }
642  else
643  {
644  cam.texture_file = uFormat("%d_%d", poseIter->first, (int)i); // camera index, sub camera model index
645  }
646  if(!roiRatios.empty())
647  {
648  cam.roi.resize(4);
649  cam.roi[0] = cam.width * roiRatios[0]; // left -> x
650  cam.roi[1] = cam.height * roiRatios[2]; // top -> y
651  cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
652  cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
653  }
654 
655  if(depthIter != cameraDepths.end() && !depthIter->second.empty())
656  {
657  UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
658  UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
659  int subWidth = depthIter->second.cols/(modelIter->second.size());
660  cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
661  }
662 
663  UDEBUG("%f", cam.focal_length);
664  UDEBUG("%f", cam.height);
665  UDEBUG("%f", cam.width);
666  UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
667 
668  cameras.push_back(cam);
669  }
670  }
671  }
672  return cameras;
673 }
674 
675 pcl::TextureMesh::Ptr createTextureMesh(
676  const pcl::PolygonMesh::Ptr & mesh,
677  const std::map<int, Transform> & poses,
678  const std::map<int, CameraModel> & cameraModels,
679  const std::map<int, cv::Mat> & cameraDepths,
680  float maxDistance,
681  float maxDepthError,
682  float maxAngle,
683  int minClusterSize,
684  const std::vector<float> & roiRatios,
685  const ProgressState * state,
686  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
687  bool distanceToCamPolicy)
688 {
689  std::map<int, std::vector<CameraModel> > cameraSubModels;
690  for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
691  {
692  std::vector<CameraModel> models;
693  models.push_back(iter->second);
694  cameraSubModels.insert(std::make_pair(iter->first, models));
695  }
696 
697  return createTextureMesh(
698  mesh,
699  poses,
700  cameraSubModels,
701  cameraDepths,
702  maxDistance,
703  maxDepthError,
704  maxAngle,
705  minClusterSize,
706  roiRatios,
707  state,
708  vertexToPixels,
709  distanceToCamPolicy);
710 }
711 
712 pcl::TextureMesh::Ptr createTextureMesh(
713  const pcl::PolygonMesh::Ptr & mesh,
714  const std::map<int, Transform> & poses,
715  const std::map<int, std::vector<CameraModel> > & cameraModels,
716  const std::map<int, cv::Mat> & cameraDepths,
717  float maxDistance,
718  float maxDepthError,
719  float maxAngle,
720  int minClusterSize,
721  const std::vector<float> & roiRatios,
722  const ProgressState * state,
723  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
724  bool distanceToCamPolicy)
725 {
726  UASSERT(mesh->polygons.size());
727  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
728  textureMesh->cloud = mesh->cloud;
729  textureMesh->tex_polygons.push_back(mesh->polygons);
730 
731  // Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
732  // Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
733 
734  // Create the texturemesh object that will contain our UV-mapped mesh
735 
736  // create cameras
737  pcl::texture_mapping::CameraVector cameras = createTextureCameras(
738  poses,
739  cameraModels,
740  cameraDepths,
741  roiRatios);
742 
743  // Create materials for each texture (and one extra for occluded faces)
744  textureMesh->tex_materials.resize (cameras.size () + 1);
745  for(unsigned int i = 0 ; i <= cameras.size() ; ++i)
746  {
747  pcl::TexMaterial mesh_material;
748  mesh_material.tex_Ka.r = 0.2f;
749  mesh_material.tex_Ka.g = 0.2f;
750  mesh_material.tex_Ka.b = 0.2f;
751 
752  mesh_material.tex_Kd.r = 0.8f;
753  mesh_material.tex_Kd.g = 0.8f;
754  mesh_material.tex_Kd.b = 0.8f;
755 
756  mesh_material.tex_Ks.r = 1.0f;
757  mesh_material.tex_Ks.g = 1.0f;
758  mesh_material.tex_Ks.b = 1.0f;
759 
760  mesh_material.tex_d = 1.0f;
761  mesh_material.tex_Ns = 75.0f;
762  mesh_material.tex_illum = 2;
763 
764  std::stringstream tex_name;
765  tex_name << "material_" << i;
766  tex_name >> mesh_material.tex_name;
767 
768  if(i < cameras.size ())
769  {
770  mesh_material.tex_file = cameras[i].texture_file;
771  }
772  else
773  {
774  mesh_material.tex_file = "occluded";
775  }
776 
777  textureMesh->tex_materials[i] = mesh_material;
778  }
779 
780  // Texture by projection
781  pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
782  tm.setMaxDistance(maxDistance);
783  tm.setMaxAngle(maxAngle);
784  if(maxDepthError > 0.0f)
785  {
786  tm.setMaxDepthError(maxDepthError);
787  }
788  tm.setMinClusterSize(minClusterSize);
789  if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
790  {
791  // compute normals for the mesh if not already here
792  bool hasNormals = false;
793  bool hasColors = false;
794  for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
795  {
796  if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
797  {
798  hasNormals = true;
799  }
800  else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
801  {
802  hasColors = true;
803  }
804  }
805  if(!hasNormals)
806  {
807  // use polygons
808  if(hasColors)
809  {
810  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
811  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
812 
813  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
814  {
815  pcl::Vertices & v = mesh->polygons[i];
816  UASSERT(v.vertices.size()>2);
817  Eigen::Vector3f v0(
818  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
819  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
820  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
821  int last = v.vertices.size()-1;
822  Eigen::Vector3f v1(
823  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
824  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
825  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
826  Eigen::Vector3f normal = v0.cross(v1);
827  normal.normalize();
828  // flat normal (per face)
829  for(unsigned int j=0; j<v.vertices.size(); ++j)
830  {
831  cloud->at(v.vertices[j]).normal_x = normal[0];
832  cloud->at(v.vertices[j]).normal_y = normal[1];
833  cloud->at(v.vertices[j]).normal_z = normal[2];
834  }
835  }
836  pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
837  }
838  else
839  {
840  pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
841  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
842 
843  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
844  {
845  pcl::Vertices & v = mesh->polygons[i];
846  UASSERT(v.vertices.size()>2);
847  Eigen::Vector3f v0(
848  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
849  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
850  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
851  int last = v.vertices.size()-1;
852  Eigen::Vector3f v1(
853  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
854  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
855  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
856  Eigen::Vector3f normal = v0.cross(v1);
857  normal.normalize();
858  // flat normal (per face)
859  for(unsigned int j=0; j<v.vertices.size(); ++j)
860  {
861  cloud->at(v.vertices[j]).normal_x = normal[0];
862  cloud->at(v.vertices[j]).normal_y = normal[1];
863  cloud->at(v.vertices[j]).normal_z = normal[2];
864  }
865  }
866  pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
867  }
868  }
869  }
870  return textureMesh;
871 }
872 
873 void cleanTextureMesh(
874  pcl::TextureMesh & textureMesh,
875  int minClusterSize)
876 {
877  UDEBUG("minClusterSize=%d", minClusterSize);
878  // Remove occluded polygons (polygons with no texture)
879  if(textureMesh.tex_coordinates.size())
880  {
881  // assume last texture is the occluded texture
882  textureMesh.tex_coordinates.pop_back();
883  textureMesh.tex_polygons.pop_back();
884  textureMesh.tex_materials.pop_back();
885 
886  if(minClusterSize!=0)
887  {
888  // concatenate all polygons
889  unsigned int totalSize = 0;
890  for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
891  {
892  totalSize+=textureMesh.tex_polygons[t].size();
893  }
894  std::vector<pcl::Vertices> allPolygons(totalSize);
895  int oi=0;
896  for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
897  {
898  for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
899  {
900  allPolygons[oi++] = textureMesh.tex_polygons[t][i];
901  }
902  }
903 
904  // filter polygons
905  std::vector<std::set<int> > neighbors;
906  std::vector<std::set<int> > vertexToPolygons;
907  util3d::createPolygonIndexes(allPolygons,
908  (int)textureMesh.cloud.data.size()/textureMesh.cloud.point_step,
909  neighbors,
910  vertexToPolygons);
911 
912  std::list<std::list<int> > clusters = util3d::clusterPolygons(
913  neighbors,
914  minClusterSize<0?0:minClusterSize);
915 
916  std::set<int> validPolygons;
917  if(minClusterSize < 0)
918  {
919  // only keep the biggest cluster
920  std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
921  unsigned int biggestClusterSize = 0;
922  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
923  {
924  if(iter->size() > biggestClusterSize)
925  {
926  biggestClusterIndex = iter;
927  biggestClusterSize = iter->size();
928  }
929  }
930  if(biggestClusterIndex != clusters.end())
931  {
932  for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
933  {
934  validPolygons.insert(*jter);
935  }
936  }
937  }
938  else
939  {
940  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
941  {
942  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
943  {
944  validPolygons.insert(*jter);
945  }
946  }
947  }
948 
949  if(validPolygons.size() == 0)
950  {
951  UWARN("All %d polygons filtered after polygon cluster filtering. Cluster minimum size is %d.",totalSize, minClusterSize);
952  }
953 
954  // for each texture
955  unsigned int allPolygonsIndex = 0;
956  for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
957  {
958  std::vector<pcl::Vertices> filteredPolygons(textureMesh.tex_polygons[t].size());
959 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
960  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > filteredCoordinates(textureMesh.tex_coordinates[t].size());
961 #else
962  std::vector<Eigen::Vector2f> filteredCoordinates(textureMesh.tex_coordinates[t].size());
963 #endif
964 
965  if(textureMesh.tex_polygons[t].size())
966  {
967  UASSERT_MSG(allPolygonsIndex < allPolygons.size(), uFormat("%d vs %d", (int)allPolygonsIndex, (int)allPolygons.size()).c_str());
968 
969  // make index polygon to coordinate
970  std::vector<unsigned int> polygonToCoord(textureMesh.tex_polygons[t].size());
971  unsigned int totalCoord = 0;
972  for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
973  {
974  polygonToCoord[i] = totalCoord;
975  totalCoord+=textureMesh.tex_polygons[t][i].vertices.size();
976  }
977  UASSERT_MSG(totalCoord == textureMesh.tex_coordinates[t].size(), uFormat("%d vs %d", totalCoord, (int)textureMesh.tex_coordinates[t].size()).c_str());
978 
979  int oi=0;
980  int ci=0;
981  for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
982  {
983  if(validPolygons.find(allPolygonsIndex) != validPolygons.end())
984  {
985  filteredPolygons[oi] = textureMesh.tex_polygons[t].at(i);
986  for(unsigned int j=0; j<filteredPolygons[oi].vertices.size(); ++j)
987  {
988  UASSERT(polygonToCoord[i] < textureMesh.tex_coordinates[t].size());
989  filteredCoordinates[ci] = textureMesh.tex_coordinates[t][polygonToCoord[i]+j];
990  ++ci;
991  }
992  ++oi;
993  }
994  ++allPolygonsIndex;
995  }
996  filteredPolygons.resize(oi);
997  filteredCoordinates.resize(ci);
998  textureMesh.tex_polygons[t] = filteredPolygons;
999  textureMesh.tex_coordinates[t] = filteredCoordinates;
1000  }
1001  }
1002  }
1003  }
1004 }
1005 
1006 pcl::TextureMesh::Ptr concatenateTextureMeshes(const std::list<pcl::TextureMesh::Ptr> & meshes)
1007 {
1008  pcl::TextureMesh::Ptr output(new pcl::TextureMesh);
1009  std::map<std::string, int> addedMaterials; //<file, index>
1010  for(std::list<pcl::TextureMesh::Ptr>::const_iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
1011  {
1012  if((*iter)->cloud.point_step &&
1013  (*iter)->cloud.data.size()/(*iter)->cloud.point_step &&
1014  (*iter)->tex_polygons.size() &&
1015  (*iter)->tex_coordinates.size())
1016  {
1017  // append point cloud
1018  int polygonStep = output->cloud.height * output->cloud.width;
1019  pcl::PCLPointCloud2 tmp;
1020 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
1021  pcl::concatenate(output->cloud, iter->get()->cloud, tmp);
1022 #else
1023  pcl::concatenatePointCloud(output->cloud, iter->get()->cloud, tmp);
1024 #endif
1025  output->cloud = tmp;
1026 
1027  UASSERT((*iter)->tex_polygons.size() == (*iter)->tex_coordinates.size() &&
1028  (*iter)->tex_polygons.size() == (*iter)->tex_materials.size());
1029 
1030  int materialCount = (*iter)->tex_polygons.size();
1031  for(int i=0; i<materialCount; ++i)
1032  {
1033  std::map<std::string, int>::iterator jter = addedMaterials.find((*iter)->tex_materials[i].tex_file);
1034  int index;
1035  if(jter != addedMaterials.end())
1036  {
1037  index = jter->second;
1038  }
1039  else
1040  {
1041  addedMaterials.insert(std::make_pair((*iter)->tex_materials[i].tex_file, output->tex_materials.size()));
1042  index = output->tex_materials.size();
1043  output->tex_materials.push_back((*iter)->tex_materials[i]);
1044  output->tex_materials.back().tex_name = uFormat("material_%d", index);
1045  output->tex_polygons.resize(output->tex_polygons.size() + 1);
1046  output->tex_coordinates.resize(output->tex_coordinates.size() + 1);
1047  }
1048 
1049  // update and append polygon indices
1050  int oi = output->tex_polygons[index].size();
1051  output->tex_polygons[index].resize(output->tex_polygons[index].size() + (*iter)->tex_polygons[i].size());
1052  for(unsigned int j=0; j<(*iter)->tex_polygons[i].size(); ++j)
1053  {
1054  pcl::Vertices polygon = (*iter)->tex_polygons[i][j];
1055  for(unsigned int k=0; k<polygon.vertices.size(); ++k)
1056  {
1057  polygon.vertices[k] += polygonStep;
1058  }
1059  output->tex_polygons[index][oi+j] = polygon;
1060  }
1061 
1062  // append uv coordinates
1063  oi = output->tex_coordinates[index].size();
1064  output->tex_coordinates[index].resize(output->tex_coordinates[index].size() + (*iter)->tex_coordinates[i].size());
1065  for(unsigned int j=0; j<(*iter)->tex_coordinates[i].size(); ++j)
1066  {
1067  output->tex_coordinates[index][oi+j] = (*iter)->tex_coordinates[i][j];
1068  }
1069  }
1070  }
1071  }
1072  return output;
1073 }
1074 
1075 int gcd(int a, int b) {
1076  return b == 0 ? a : gcd(b, a % b);
1077 }
1078 
1079 void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept)
1080 {
1081  UASSERT(textureSize>0 && imageSize.width>0 && imageSize.height>0);
1082  if(maxTextures < 1)
1083  {
1084  maxTextures = 1;
1085  }
1086  int materials = 0;
1087  for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
1088  {
1089  if(mesh.tex_polygons.size())
1090  {
1091  ++materials;
1092  }
1093  }
1094  if(materials)
1095  {
1096  int w = imageSize.width; // 640
1097  int h = imageSize.height; // 480
1098  int g = gcd(w,h); // 160
1099  int a = w/g; // 4=640/160
1100  int b = h/g; // 3=480/160
1101  UDEBUG("w=%d h=%d g=%d a=%d b=%d", w, h, g, a, b);
1102  int colCount = 0;
1103  int rowCount = 0;
1104  float factor = 0.1f;
1105  float epsilon = 0.001f;
1106  scale = 1.0f;
1107  while((colCount*rowCount)*maxTextures < materials || (factor == 0.1f || scale > 1.0f))
1108  {
1109  // first run try scale = 1 (no scaling)
1110  if(factor!=0.1f)
1111  {
1112  scale = float(textureSize)/float(w*b*factor);
1113  }
1114  colCount = float(textureSize)/(scale*float(w));
1115  rowCount = float(textureSize)/(scale*float(h));
1116  factor+=epsilon; // search the maximum perfect fit
1117  }
1118  int outputTextures = (materials / (colCount*rowCount)) + (materials % (colCount*rowCount) > 0?1:0);
1119  UDEBUG("materials=%d col=%d row=%d output textures=%d factor=%f scale=%f", materials, colCount, rowCount, outputTextures, factor-epsilon, scale);
1120 
1121  UASSERT(mesh.tex_coordinates.size() == mesh.tex_materials.size() && mesh.tex_polygons.size() == mesh.tex_materials.size());
1122 
1123  // prepare size
1124  std::vector<int> totalPolygons(outputTextures, 0);
1125  std::vector<int> totalCoordinates(outputTextures, 0);
1126  int count = 0;
1127  for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
1128  {
1129  if(mesh.tex_polygons[i].size())
1130  {
1131  int indexMaterial = count / (colCount*rowCount);
1132  UASSERT(indexMaterial < outputTextures);
1133 
1134  totalPolygons[indexMaterial]+=mesh.tex_polygons[i].size();
1135  totalCoordinates[indexMaterial]+=mesh.tex_coordinates[i].size();
1136 
1137  ++count;
1138  }
1139  }
1140 
1141  pcl::TextureMesh outputMesh;
1142 
1143  int pi = 0;
1144  int ci = 0;
1145  int ti=0;
1146  float scaledHeight = float(int(scale*float(h)))/float(textureSize);
1147  float scaledWidth = float(int(scale*float(w)))/float(textureSize);
1148  float lowerBorderSize = 1.0f - scaledHeight*float(rowCount);
1149  UDEBUG("scaledWidth=%f scaledHeight=%f lowerBorderSize=%f", scaledWidth, scaledHeight, lowerBorderSize);
1150  if(materialsKept)
1151  {
1152  materialsKept->resize(mesh.tex_materials.size(), false);
1153  }
1154  for(unsigned int t=0; t<mesh.tex_materials.size(); ++t)
1155  {
1156  if(mesh.tex_polygons[t].size())
1157  {
1158  int indexMaterial = ti / (colCount*rowCount);
1159  UASSERT(indexMaterial < outputTextures);
1160  if((int)outputMesh.tex_polygons.size() <= indexMaterial)
1161  {
1162  std::vector<pcl::Vertices> newPolygons(totalPolygons[indexMaterial]);
1163 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1164  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > newCoordinates(totalCoordinates[indexMaterial]); // UV coordinates
1165 #else
1166  std::vector<Eigen::Vector2f> newCoordinates(totalCoordinates[indexMaterial]); // UV coordinates
1167 #endif
1168  outputMesh.tex_polygons.push_back(newPolygons);
1169  outputMesh.tex_coordinates.push_back(newCoordinates);
1170 
1171  pi=0;
1172  ci=0;
1173  }
1174 
1175  int row = (ti/colCount) % rowCount;
1176  int col = ti%colCount;
1177  float offsetU = scaledWidth * float(col);
1178  float offsetV = scaledHeight * float((rowCount - 1) - row) + lowerBorderSize;
1179  // Texture coords have lower-left origin
1180 
1181  for(unsigned int i=0; i<mesh.tex_polygons[t].size(); ++i)
1182  {
1183  UASSERT(pi < (int)outputMesh.tex_polygons[indexMaterial].size());
1184  outputMesh.tex_polygons[indexMaterial][pi++] = mesh.tex_polygons[t].at(i);
1185  }
1186 
1187  for(unsigned int i=0; i<mesh.tex_coordinates[t].size(); ++i)
1188  {
1189  const Eigen::Vector2f & v = mesh.tex_coordinates[t].at(i);
1190  if(v[0] >= 0 && v[1] >=0)
1191  {
1192  outputMesh.tex_coordinates[indexMaterial][ci][0] = v[0]*scaledWidth + offsetU;
1193  outputMesh.tex_coordinates[indexMaterial][ci][1] = v[1]*scaledHeight + offsetV;
1194  }
1195  else
1196  {
1197  outputMesh.tex_coordinates[indexMaterial][ci] = v;
1198  }
1199  ++ci;
1200  }
1201  ++ti;
1202  if(materialsKept)
1203  {
1204  materialsKept->at(t) = true;
1205  }
1206  }
1207  }
1208  pcl::TexMaterial m = mesh.tex_materials.front();
1209  mesh.tex_materials.clear();
1210  for(int i=0; i<outputTextures; ++i)
1211  {
1212  m.tex_file = "texture";
1213  m.tex_name = "material";
1214  if(outputTextures > 1)
1215  {
1216  m.tex_file += uNumber2Str(i);
1217  m.tex_name += uNumber2Str(i);
1218  }
1219 
1220  mesh.tex_materials.push_back(m);
1221  }
1222  mesh.tex_coordinates = outputMesh.tex_coordinates;
1223  mesh.tex_polygons = outputMesh.tex_polygons;
1224  }
1225 }
1226 
1227 std::vector<std::vector<RTABMAP_PCL_INDEX> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
1228 {
1229  std::vector<std::vector<RTABMAP_PCL_INDEX> > polygonsOut(polygons.size());
1230  for(unsigned int p=0; p<polygons.size(); ++p)
1231  {
1232  polygonsOut[p] = polygons[p].vertices;
1233  }
1234  return polygonsOut;
1235 }
1236 std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
1237 {
1238  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygonsOut(tex_polygons.size());
1239  for(unsigned int t=0; t<tex_polygons.size(); ++t)
1240  {
1241  polygonsOut[t].resize(tex_polygons[t].size());
1242  for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
1243  {
1244  polygonsOut[t][p] = tex_polygons[t][p].vertices;
1245  }
1246  }
1247  return polygonsOut;
1248 }
1249 std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons)
1250 {
1251  std::vector<pcl::Vertices> polygonsOut(polygons.size());
1252  for(unsigned int p=0; p<polygons.size(); ++p)
1253  {
1254  polygonsOut[p].vertices = polygons[p];
1255  }
1256  return polygonsOut;
1257 }
1258 std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & tex_polygons)
1259 {
1260  std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
1261  for(unsigned int t=0; t<tex_polygons.size(); ++t)
1262  {
1263  polygonsOut[t].resize(tex_polygons[t].size());
1264  for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
1265  {
1266  polygonsOut[t][p].vertices = tex_polygons[t][p];
1267  }
1268  }
1269  return polygonsOut;
1270 }
1271 
1272 pcl::TextureMesh::Ptr assembleTextureMesh(
1273  const cv::Mat & cloudMat,
1274  const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
1275 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1276  const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
1277 #else
1278  const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
1279 #endif
1280  cv::Mat & textures,
1281  bool mergeTextures)
1282 {
1283  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
1284 
1285  if(cloudMat.channels() <= 3)
1286  {
1287  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
1288  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1289  }
1290  else if(cloudMat.channels() == 4)
1291  {
1292  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
1293  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1294  }
1295  else if(cloudMat.channels() == 6)
1296  {
1297  pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
1298  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1299  }
1300  else if(cloudMat.channels() == 7)
1301  {
1302  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
1303  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1304  }
1305 
1306  if(textureMesh->cloud.data.size() && polygons.size())
1307  {
1308  textureMesh->tex_polygons.resize(polygons.size());
1309  for(unsigned int t=0; t<polygons.size(); ++t)
1310  {
1311  textureMesh->tex_polygons[t].resize(polygons[t].size());
1312  for(unsigned int p=0; p<polygons[t].size(); ++p)
1313  {
1314  textureMesh->tex_polygons[t][p].vertices = polygons[t][p];
1315  }
1316  }
1317 
1318  if(!texCoords.empty() && !textures.empty())
1319  {
1320  textureMesh->tex_coordinates = texCoords;
1321 
1322  textureMesh->tex_materials.resize (textureMesh->tex_coordinates.size());
1323  for(unsigned int i = 0 ; i < textureMesh->tex_coordinates.size() ; ++i)
1324  {
1325  pcl::TexMaterial mesh_material;
1326  mesh_material.tex_Ka.r = 0.2f;
1327  mesh_material.tex_Ka.g = 0.2f;
1328  mesh_material.tex_Ka.b = 0.2f;
1329 
1330  mesh_material.tex_Kd.r = 0.8f;
1331  mesh_material.tex_Kd.g = 0.8f;
1332  mesh_material.tex_Kd.b = 0.8f;
1333 
1334  mesh_material.tex_Ks.r = 1.0f;
1335  mesh_material.tex_Ks.g = 1.0f;
1336  mesh_material.tex_Ks.b = 1.0f;
1337 
1338  mesh_material.tex_d = 1.0f;
1339  mesh_material.tex_Ns = 75.0f;
1340  mesh_material.tex_illum = 2;
1341 
1342  std::stringstream tex_name;
1343  tex_name << "material_" << i;
1344  tex_name >> mesh_material.tex_name;
1345 
1346  mesh_material.tex_file = uFormat("%d", i);
1347 
1348  textureMesh->tex_materials[i] = mesh_material;
1349  }
1350 
1351  if(mergeTextures && textures.cols/textures.rows > 1)
1352  {
1353  UASSERT(textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)textureMesh->tex_coordinates.size());
1354  std::vector<bool> materialsKept;
1355  float scale = 0.0f;
1356  cv::Size imageSize(textures.rows, textures.rows);
1357  int imageType = textures.type();
1358  rtabmap::util3d::concatenateTextureMaterials(*textureMesh, imageSize, textures.rows, 1, scale, &materialsKept);
1359  if(scale && textureMesh->tex_materials.size() == 1)
1360  {
1361  int cols = float(textures.rows)/(scale*imageSize.width);
1362  int rows = float(textures.rows)/(scale*imageSize.height);
1363 
1364  cv::Mat mergedTextures = cv::Mat(textures.rows, textures.rows, imageType, cv::Scalar::all(255));
1365 
1366  // make a blank texture
1367  cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
1368  int oi=0;
1369  for(int i=0; i<(int)materialsKept.size(); ++i)
1370  {
1371  if(materialsKept.at(i))
1372  {
1373  int u = oi%cols * resizedImageSize.width;
1374  int v = ((oi/cols) % rows ) * resizedImageSize.height;
1375  UASSERT(u < textures.rows-resizedImageSize.width);
1376  UASSERT(v < textures.rows-resizedImageSize.height);
1377 
1378  cv::Mat resizedImage;
1379  cv::resize(textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
1380 
1381  UASSERT(resizedImage.type() == mergedTextures.type());
1382  resizedImage.copyTo(mergedTextures(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
1383 
1384  ++oi;
1385  }
1386  }
1387  textures = mergedTextures;
1388  }
1389  }
1390  }
1391  }
1392  return textureMesh;
1393 }
1394 
1395 pcl::PolygonMesh::Ptr assemblePolygonMesh(
1396  const cv::Mat & cloudMat,
1397  const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons)
1398 {
1399  pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
1400 
1401  if(cloudMat.channels() <= 3)
1402  {
1403  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
1404  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1405  }
1406  else if(cloudMat.channels() == 4)
1407  {
1408  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
1409  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1410  }
1411  else if(cloudMat.channels() == 6)
1412  {
1413  pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
1414  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1415  }
1416  else if(cloudMat.channels() == 7)
1417  {
1418  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
1419  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1420  }
1421 
1422  if(polygonMesh->cloud.data.size() && polygons.size())
1423  {
1424  polygonMesh->polygons.resize(polygons.size());
1425  for(unsigned int p=0; p<polygons.size(); ++p)
1426  {
1427  polygonMesh->polygons[p].vertices = polygons[p];
1428  }
1429  }
1430  return polygonMesh;
1431 }
1432 
1433 double sqr(uchar v)
1434 {
1435  return double(v)*double(v);
1436 }
1437 
1438 cv::Mat mergeTextures(
1439  pcl::TextureMesh & mesh,
1440  const std::map<int, cv::Mat> & images,
1441  const std::map<int, CameraModel> & calibrations,
1442  const Memory * memory,
1443  const DBDriver * dbDriver,
1444  int textureSize,
1445  int textureCount,
1446  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
1447  bool gainCompensation,
1448  float gainBeta,
1449  bool gainRGB,
1450  bool blending,
1451  int blendingDecimation,
1452  int brightnessContrastRatioLow,
1453  int brightnessContrastRatioHigh,
1454  bool exposureFusion,
1455  const ProgressState * state,
1456  unsigned char blankValue,
1457  std::map<int, std::map<int, cv::Vec4d> > * gains,
1458  std::map<int, std::map<int, cv::Mat> > * blendingGains,
1459  std::pair<float, float> * contrastValues)
1460 {
1461  std::map<int, std::vector<CameraModel> > calibVectors;
1462  for(std::map<int, CameraModel>::const_iterator iter=calibrations.begin(); iter!=calibrations.end(); ++iter)
1463  {
1464  std::vector<CameraModel> m;
1465  m.push_back(iter->second);
1466  calibVectors.insert(std::make_pair(iter->first, m));
1467  }
1468  return mergeTextures(mesh,
1469  images,
1470  calibVectors,
1471  memory,
1472  dbDriver,
1473  textureSize,
1474  textureCount,
1475  vertexToPixels,
1476  gainCompensation,
1477  gainBeta,
1478  gainRGB,
1479  blending,
1480  blendingDecimation,
1481  brightnessContrastRatioLow,
1482  brightnessContrastRatioHigh,
1483  exposureFusion,
1484  state,
1485  blankValue,
1486  gains,
1487  blendingGains,
1488  contrastValues);
1489 }
1490 cv::Mat mergeTextures(
1491  pcl::TextureMesh & mesh,
1492  const std::map<int, cv::Mat> & images,
1493  const std::map<int, std::vector<CameraModel> > & calibrations,
1494  const Memory * memory,
1495  const DBDriver * dbDriver,
1496  int textureSize,
1497  int textureCount,
1498  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
1499  bool gainCompensation,
1500  float gainBeta,
1501  bool gainRGB,
1502  bool blending,
1503  int blendingDecimation,
1504  int brightnessContrastRatioLow,
1505  int brightnessContrastRatioHigh,
1506  bool exposureFusion,
1507  const ProgressState * state,
1508  unsigned char blankValue,
1509  std::map<int, std::map<int, cv::Vec4d> > * gainsOut,
1510  std::map<int, std::map<int, cv::Mat> > * blendingGainsOut,
1511  std::pair<float, float> * contrastValuesOut)
1512 {
1513  //get texture size, if disabled use default 1024
1514  UASSERT(textureSize%256 == 0);
1515  UDEBUG("textureSize = %d", textureSize);
1516  cv::Mat globalTextures;
1517  if(!mesh.tex_materials.empty())
1518  {
1519  std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,0));
1520  cv::Size imageSize;
1521  const int imageType=CV_8UC3;
1522 
1523  UDEBUG("");
1524  for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
1525  {
1526  std::list<std::string> texFileSplit = uSplit(mesh.tex_materials[i].tex_file, '_');
1527  if(!mesh.tex_materials[i].tex_file.empty() &&
1528  mesh.tex_polygons[i].size() &&
1529  uIsInteger(texFileSplit.front(), false))
1530  {
1531  textures[i].first = uStr2Int(texFileSplit.front());
1532  if(texFileSplit.size() == 2 &&
1533  uIsInteger(texFileSplit.back(), false) )
1534  {
1535  textures[i].second = uStr2Int(texFileSplit.back());
1536  }
1537 
1538  int textureId = textures[i].first;
1539  if(imageSize.width == 0 || imageSize.height == 0)
1540  {
1541  if(images.find(textureId) != images.end() &&
1542  !images.find(textureId)->second.empty() &&
1543  calibrations.find(textureId) != calibrations.end())
1544  {
1545  const std::vector<CameraModel> & models = calibrations.find(textureId)->second;
1546  UASSERT(models.size()>=1);
1547  if( models[0].imageHeight()>0 &&
1548  models[0].imageWidth()>0)
1549  {
1550  imageSize = models[0].imageSize();
1551  }
1552  else if(images.find(textureId)!=images.end())
1553  {
1554  // backward compatibility for image size not set in CameraModel
1555  cv::Mat image = images.find(textureId)->second;
1556  if(image.rows == 1 && image.type() == CV_8UC1)
1557  {
1558  image = uncompressImage(image);
1559  }
1560  UASSERT(!image.empty());
1561  imageSize = image.size();
1562  if(models.size()>1)
1563  {
1564  imageSize.width/=models.size();
1565  }
1566  }
1567  }
1568  else if(memory)
1569  {
1570  SensorData data = memory->getNodeData(textureId, true, false, false, false);
1571  const std::vector<CameraModel> & models = data.cameraModels();
1572  const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
1573  if(models.size()>=1 &&
1574  models[0].imageHeight()>0 &&
1575  models[0].imageWidth()>0)
1576  {
1577  imageSize = models[0].imageSize();
1578  }
1579  else if(stereoModels.size()>=1 &&
1580  stereoModels[0].left().imageHeight() > 0 &&
1581  stereoModels[0].left().imageWidth() > 0)
1582  {
1583  imageSize = stereoModels[0].left().imageSize();
1584  }
1585  else // backward compatibility for image size not set in CameraModel
1586  {
1587  cv::Mat image;
1588  data.uncompressDataConst(&image, 0);
1589  UASSERT(!image.empty());
1590  imageSize = image.size();
1591  if(data.cameraModels().size()>1)
1592  {
1593  imageSize.width/=data.cameraModels().size();
1594  }
1595  }
1596  }
1597  else if(dbDriver)
1598  {
1599  std::vector<CameraModel> models;
1600  std::vector<StereoCameraModel> stereoModels;
1601  dbDriver->getCalibration(textureId, models, stereoModels);
1602  if(models.size()>=1 &&
1603  models[0].imageHeight()>0 &&
1604  models[0].imageWidth()>0)
1605  {
1606  imageSize = models[0].imageSize();
1607  }
1608  else if(stereoModels.size()>=1 &&
1609  stereoModels[0].left().imageHeight() > 0 &&
1610  stereoModels[0].left().imageWidth() > 0)
1611  {
1612  imageSize = stereoModels[0].left().imageSize();
1613  }
1614  else // backward compatibility for image size not set in CameraModel
1615  {
1616  SensorData data;
1617  dbDriver->getNodeData(textureId, data, true, false, false, false);
1618  cv::Mat image;
1619  data.uncompressDataConst(&image, 0);
1620  UASSERT(!image.empty());
1621  imageSize = image.size();
1622  if(data.cameraModels().size()>1)
1623  {
1624  imageSize.width/=data.cameraModels().size();
1625  }
1626  }
1627  }
1628  }
1629  }
1630  else if(mesh.tex_polygons[i].size() && mesh.tex_materials[i].tex_file.compare("occluded")!=0)
1631  {
1632  UWARN("Failed parsing texture file name: %s", mesh.tex_materials[i].tex_file.c_str());
1633  }
1634  }
1635  UDEBUG("textures=%d imageSize=%dx%d", (int)textures.size(), imageSize.height, imageSize.width);
1636  if(textures.size() && imageSize.height>0 && imageSize.width>0)
1637  {
1638  float scale = 0.0f;
1639  UDEBUG("");
1640  std::vector<bool> materialsKept;
1641  util3d::concatenateTextureMaterials(mesh, imageSize, textureSize, textureCount, scale, &materialsKept);
1642  if(scale && mesh.tex_materials.size())
1643  {
1644  int materials = (int)mesh.tex_materials.size();
1645  int cols = float(textureSize)/(scale*imageSize.width);
1646  int rows = float(textureSize)/(scale*imageSize.height);
1647 
1648  globalTextures = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(blankValue));
1649  cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));
1650 
1651  // used for multi camera texturing, to avoid reloading same texture for sub cameras
1652  cv::Mat previousImage;
1653  int previousTextureId = 0;
1654  std::vector<CameraModel> previousCameraModels;
1655 
1656  // make a blank texture
1657  cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(blankValue));
1658  cv::Mat emptyImageMask(int(imageSize.height*scale), int(imageSize.width*scale), CV_8UC1, cv::Scalar::all(255));
1659  int oi=0;
1660  std::vector<cv::Point2i> imageOrigin(textures.size());
1661  std::vector<int> newCamIndex(textures.size(), -1);
1662  for(int t=0; t<(int)textures.size(); ++t)
1663  {
1664  if(materialsKept.at(t))
1665  {
1666  int indexMaterial = oi / (cols*rows);
1667  UASSERT(indexMaterial < materials);
1668 
1669  newCamIndex[t] = oi;
1670  int u = oi%cols * emptyImage.cols;
1671  int v = ((oi/cols) % rows ) * emptyImage.rows;
1672  UASSERT_MSG(u < textureSize-emptyImage.cols, uFormat("u=%d textureSize=%d emptyImage.cols=%d", u, textureSize, emptyImage.cols).c_str());
1673  UASSERT_MSG(v < textureSize-emptyImage.rows, uFormat("v=%d textureSize=%d emptyImage.rows=%d", v, textureSize, emptyImage.rows).c_str());
1674  imageOrigin[t].x = u;
1675  imageOrigin[t].y = v;
1676  if(textures[t].first>=0)
1677  {
1678  cv::Mat image;
1679  std::vector<CameraModel> models;
1680 
1681  if(textures[t].first == previousTextureId)
1682  {
1683  image = previousImage;
1684  models = previousCameraModels;
1685  }
1686  else
1687  {
1688  if(images.find(textures[t].first) != images.end() &&
1689  !images.find(textures[t].first)->second.empty() &&
1690  calibrations.find(textures[t].first) != calibrations.end())
1691  {
1692  image = images.find(textures[t].first)->second;
1693  if(image.rows == 1 && image.type() == CV_8UC1)
1694  {
1695  image = uncompressImage(image);
1696  }
1697  models = calibrations.find(textures[t].first)->second;
1698  }
1699  else if(memory)
1700  {
1701  SensorData data = memory->getNodeData(textures[t].first, true, false, false, false);
1702  models = data.cameraModels();
1703  if(models.empty() && !data.stereoCameraModels().empty())
1704  {
1705  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
1706  {
1707  models.push_back(data.stereoCameraModels()[i].left());
1708  }
1709  }
1710  data.uncompressDataConst(&image, 0);
1711  }
1712  else if(dbDriver)
1713  {
1714  SensorData data;
1715  dbDriver->getNodeData(textures[t].first, data, true, false, false, false);
1716  data.uncompressDataConst(&image, 0);
1717  std::vector<StereoCameraModel> stereoModels;
1718  dbDriver->getCalibration(textures[t].first, models, stereoModels);
1719  if(models.empty() && !stereoModels.empty())
1720  {
1721  for(size_t i=0; i<stereoModels.size(); ++i)
1722  {
1723  models.push_back(stereoModels[i].left());
1724  }
1725  }
1726  }
1727 
1728  previousImage = image;
1729  previousCameraModels = models;
1730  previousTextureId = textures[t].first;
1731  }
1732 
1733  UASSERT(!image.empty());
1734 
1735  if(textures[t].second>=0)
1736  {
1737  UASSERT(textures[t].second < (int)models.size());
1738  int width = image.cols/models.size();
1739  image = image.colRange(width*textures[t].second, width*(textures[t].second+1));
1740  }
1741 
1742  cv::Mat resizedImage;
1743  cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
1744  UASSERT(resizedImage.type() == CV_8UC1 || resizedImage.type() == CV_8UC3);
1745  if(resizedImage.type() == CV_8UC1)
1746  {
1747  cv::Mat resizedImageColor;
1748  cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
1749  resizedImage = resizedImageColor;
1750  }
1751  UASSERT(resizedImage.type() == globalTextures.type());
1752  resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
1753  emptyImageMask.copyTo(globalTextureMasks(cv::Rect(u+indexMaterial*globalTextureMasks.rows, v, resizedImage.cols, resizedImage.rows)));
1754  }
1755  else
1756  {
1757  emptyImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows)));
1758  }
1759  ++oi;
1760  }
1761 
1762  if(state)
1763  {
1764  if(state->isCanceled())
1765  {
1766  return cv::Mat();
1767  }
1768  state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
1769  }
1770  }
1771 
1772  UTimer timer;
1773  if(vertexToPixels.size())
1774  {
1775  //UWARN("Saving original.png", globalTexture);
1776  //cv::imwrite("original.png", globalTexture);
1777 
1778  if(gainCompensation)
1779  {
1784  const int num_images = static_cast<int>(oi);
1785  cv::Mat_<int> N(num_images, num_images); N.setTo(0);
1786  cv::Mat_<double> I(num_images, num_images); I.setTo(0);
1787 
1788  cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
1789  cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
1790  cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);
1791 
1792  // Adjust UV coordinates to globalTexture
1793  for(unsigned int p=0; p<vertexToPixels.size(); ++p)
1794  {
1795  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
1796  {
1797  if(materialsKept.at(iter->first))
1798  {
1799  N(newCamIndex[iter->first], newCamIndex[iter->first]) +=1;
1800 
1801  std::map<int, pcl::PointXY>::const_iterator jter=iter;
1802  ++jter;
1803  int k = 1;
1804  for(; jter!=vertexToPixels[p].end(); ++jter, ++k)
1805  {
1806  if(materialsKept.at(jter->first))
1807  {
1808  int i = newCamIndex[iter->first];
1809  int j = newCamIndex[jter->first];
1810 
1811  N(i, j) += 1;
1812  N(j, i) += 1;
1813 
1814  int indexMaterial = i / (cols*rows);
1815 
1816  // uv in globalTexture
1817  int ui = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
1818  int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
1819  int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
1820  int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
1821  cv::Vec3b * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
1822  cv::Vec3b * pt2 = globalTextures.ptr<cv::Vec3b>(vj,uj+indexMaterial*globalTextures.rows);
1823 
1824  I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
1825  I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));
1826 
1827  IR(i, j) += static_cast<double>(pt1->val[2]);
1828  IR(j, i) += static_cast<double>(pt2->val[2]);
1829  IG(i, j) += static_cast<double>(pt1->val[1]);
1830  IG(j, i) += static_cast<double>(pt2->val[1]);
1831  IB(i, j) += static_cast<double>(pt1->val[0]);
1832  IB(j, i) += static_cast<double>(pt2->val[0]);
1833  }
1834  }
1835  }
1836  }
1837  }
1838 
1839  for(int i=0; i<num_images; ++i)
1840  {
1841  for(int j=i; j<num_images; ++j)
1842  {
1843  if(i == j)
1844  {
1845  if(N(i,j) == 0)
1846  {
1847  N(i,j) = 1;
1848  }
1849  }
1850  else if(N(i, j))
1851  {
1852  I(i, j) /= N(i, j);
1853  I(j, i) /= N(j, i);
1854 
1855  IR(i, j) /= N(i, j);
1856  IR(j, i) /= N(j, i);
1857  IG(i, j) /= N(i, j);
1858  IG(j, i) /= N(j, i);
1859  IB(i, j) /= N(i, j);
1860  IB(j, i) /= N(j, i);
1861  }
1862  }
1863  }
1864 
1865  cv::Mat_<double> A(num_images, num_images); A.setTo(0);
1866  cv::Mat_<double> b(num_images, 1); b.setTo(0);
1867  cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
1868  cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
1869  cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
1870  double alpha = 0.01;
1871  double beta = gainBeta;
1872  for (int i = 0; i < num_images; ++i)
1873  {
1874  for (int j = 0; j < num_images; ++j)
1875  {
1876  b(i, 0) += beta * N(i, j);
1877  A(i, i) += beta * N(i, j);
1878  AR(i, i) += beta * N(i, j);
1879  AG(i, i) += beta * N(i, j);
1880  AB(i, i) += beta * N(i, j);
1881  if (j == i) continue;
1882  A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
1883  A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
1884 
1885  AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
1886  AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);
1887 
1888  AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
1889  AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);
1890 
1891  AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
1892  AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
1893  }
1894  }
1895 
1896  cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
1897  cv::solve(A, b, gainsGray);
1898 
1899  cv::solve(AR, b, gainsR);
1900  cv::solve(AG, b, gainsG);
1901  cv::solve(AB, b, gainsB);
1902 
1903  cv::Mat_<double> gains(gainsGray.rows, 4);
1904  gainsGray.copyTo(gains.col(0));
1905  gainsR.copyTo(gains.col(1));
1906  gainsG.copyTo(gains.col(2));
1907  gainsB.copyTo(gains.col(3));
1908 
1909  for(int t=0; t<(int)textures.size(); ++t)
1910  {
1911  //break;
1912  if(materialsKept.at(t))
1913  {
1914  int u = imageOrigin[t].x;
1915  int v = imageOrigin[t].y;
1916 
1917  UDEBUG("Gain cam%d = %f", newCamIndex[t], gainsGray(newCamIndex[t], 0));
1918 
1919  int indexMaterial = newCamIndex[t] / (cols*rows);
1920  cv::Mat roi = globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows));
1921 
1922  std::vector<cv::Mat> channels;
1923  cv::split(roi, channels);
1924 
1925  // assuming BGR
1926  cv::multiply(channels[0], gains(newCamIndex[t], gainRGB?3:0), channels[0]);
1927  cv::multiply(channels[1], gains(newCamIndex[t], gainRGB?2:0), channels[1]);
1928  cv::multiply(channels[2], gains(newCamIndex[t], gainRGB?1:0), channels[2]);
1929 
1930  cv::merge(channels, roi);
1931 
1932  if(gainsOut)
1933  {
1934  cv::Vec4d g(
1935  gains(newCamIndex[t], 0),
1936  gains(newCamIndex[t], 1),
1937  gains(newCamIndex[t], 2),
1938  gains(newCamIndex[t], 3));
1939  if(gainsOut->find(textures[t].first) == gainsOut->end())
1940  {
1941  std::map<int,cv::Vec4d> value;
1942  value.insert(std::make_pair(textures[t].second, g));
1943  gainsOut->insert(std::make_pair(textures[t].first, value));
1944  }
1945  else
1946  {
1947  gainsOut->at(textures[t].first).insert(std::make_pair(textures[t].second, g));
1948  }
1949  }
1950  }
1951  }
1952  //UWARN("Saving gain.png", globalTexture);
1953  //cv::imwrite("gain.png", globalTexture);
1954  if(state) state->callback(uFormat("Gain compensation %fs", timer.ticks()));
1955  }
1956 
1957  if(blending)
1958  {
1959  // blending BGR
1960  int decimation = 1;
1961  if(blendingDecimation <= 0)
1962  {
1963  // determinate decimation to apply
1964  std::vector<float> edgeLengths;
1965  if(mesh.tex_coordinates.size() && mesh.tex_coordinates[0].size())
1966  {
1967  UASSERT(mesh.tex_polygons.size() && mesh.tex_polygons[0].size() && mesh.tex_polygons[0][0].vertices.size());
1968  int polygonSize = mesh.tex_polygons[0][0].vertices.size();
1969  UDEBUG("polygon size=%d", polygonSize);
1970 
1971  for(unsigned int k=0; k<mesh.tex_coordinates.size(); ++k)
1972  {
1973  for(unsigned int i=0; i<mesh.tex_coordinates[k].size(); i+=polygonSize)
1974  {
1975  for(int j=0; j<polygonSize; ++j)
1976  {
1977  const Eigen::Vector2f & uc1 = mesh.tex_coordinates[k][i + j];
1978  const Eigen::Vector2f & uc2 = mesh.tex_coordinates[k][i + (j+1)%polygonSize];
1979  Eigen::Vector2f edge = (uc1-uc2)*textureSize;
1980  edgeLengths.push_back(fabs(edge[0]));
1981  edgeLengths.push_back(fabs(edge[1]));
1982  }
1983  }
1984  }
1985  float edgeLength = 0.0f;
1986  if(edgeLengths.size())
1987  {
1988  std::sort(edgeLengths.begin(), edgeLengths.end());
1989  float m = uMean(edgeLengths.data(), edgeLengths.size());
1990  float stddev = std::sqrt(uVariance(edgeLengths.data(), edgeLengths.size(), m));
1991  edgeLength = m+stddev;
1992  decimation = 1 << 6;
1993  for(int i=1; i<=6; ++i)
1994  {
1995  if(float(1 << i) >= edgeLength)
1996  {
1997  decimation = 1 << i;
1998  break;
1999  }
2000  }
2001  }
2002 
2003  UDEBUG("edge length=%f decimation=%d", edgeLength, decimation);
2004  }
2005  }
2006  else
2007  {
2008  if(blendingDecimation > 1)
2009  {
2010  UASSERT(textureSize % blendingDecimation == 0);
2011  }
2012  decimation = blendingDecimation;
2013  UDEBUG("decimation=%d", decimation);
2014  }
2015 
2016  std::vector<cv::Mat> blendGains(materials);
2017  for(int i=0; i<materials;++i)
2018  {
2019  blendGains[i] = cv::Mat(globalTextures.rows/decimation, globalTextures.rows/decimation, CV_32FC3, cv::Scalar::all(1.0f));
2020  }
2021 
2022  for(unsigned int p=0; p<vertexToPixels.size(); ++p)
2023  {
2024  if(vertexToPixels[p].size() > 1)
2025  {
2026  std::vector<float> gainsB(vertexToPixels[p].size());
2027  std::vector<float> gainsG(vertexToPixels[p].size());
2028  std::vector<float> gainsR(vertexToPixels[p].size());
2029  float sumWeight = 0.0f;
2030  int k=0;
2031  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
2032  {
2033  if(materialsKept.at(iter->first))
2034  {
2035  int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
2036  int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
2037  float x = iter->second.x - 0.5f;
2038  float y = iter->second.y - 0.5f;
2039  float weight = 0.7f - sqrt(x*x+y*y);
2040  if(weight<0.0f)
2041  {
2042  weight = 0.0f;
2043  }
2044  int indexMaterial = newCamIndex[iter->first] / (cols*rows);
2045  cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
2046  gainsB[k] = static_cast<double>(pt->val[0]) * weight;
2047  gainsG[k] = static_cast<double>(pt->val[1]) * weight;
2048  gainsR[k] = static_cast<double>(pt->val[2]) * weight;
2049  sumWeight += weight;
2050  ++k;
2051  }
2052  }
2053  gainsB.resize(k);
2054  gainsG.resize(k);
2055  gainsR.resize(k);
2056 
2057  if(sumWeight > 0)
2058  {
2059  float targetColor[3];
2060  targetColor[0] = uSum(gainsB.data(), gainsB.size()) / sumWeight;
2061  targetColor[1] = uSum(gainsG.data(), gainsG.size()) / sumWeight;
2062  targetColor[2] = uSum(gainsR.data(), gainsR.size()) / sumWeight;
2063  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
2064  {
2065  if(materialsKept.at(iter->first))
2066  {
2067  int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
2068  int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
2069  int indexMaterial = newCamIndex[iter->first] / (cols*rows);
2070  cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
2071  float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
2072  float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
2073  float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
2074  cv::Vec3f * ptr = blendGains[indexMaterial].ptr<cv::Vec3f>(v/decimation, u/decimation);
2075  ptr->val[0] = (gB>1.3f)?1.3f:(gB<0.7f)?0.7f:gB;
2076  ptr->val[1] = (gG>1.3f)?1.3f:(gG<0.7f)?0.7f:gG;
2077  ptr->val[2] = (gR>1.3f)?1.3f:(gR<0.7f)?0.7f:gR;
2078  }
2079  }
2080  }
2081  }
2082  }
2083 
2084  if(blendingGainsOut)
2085  {
2086  for(int t=0; t<(int)textures.size(); ++t)
2087  {
2088  //break;
2089  if(materialsKept.at(t))
2090  {
2091  int u = imageOrigin[t].x/decimation;
2092  int v = imageOrigin[t].y/decimation;
2093 
2094  int indexMaterial = newCamIndex[t] / (cols*rows);
2095  cv::Mat roi = blendGains[indexMaterial](cv::Rect(u, v, emptyImage.cols/decimation, emptyImage.rows/decimation));
2096  if(blendingGainsOut->find(textures[t].first) == blendingGainsOut->end())
2097  {
2098  std::map<int,cv::Mat> value;
2099  value.insert(std::make_pair(textures[t].second, roi.clone()));
2100  blendingGainsOut->insert(std::make_pair(textures[t].first, value));
2101  }
2102  else
2103  {
2104  blendingGainsOut->at(textures[t].first).insert(std::make_pair(textures[t].second, roi.clone()));
2105  }
2106  }
2107  }
2108  }
2109 
2110  for(int i=0; i<materials; ++i)
2111  {
2112  /*std::vector<cv::Mat> channels;
2113  cv::split(blendGains, channels);
2114  cv::Mat img;
2115  channels[0].convertTo(img,CV_8U,128.0,0);
2116  cv::imwrite("blendSmallB.png", img);
2117  channels[1].convertTo(img,CV_8U,128.0,0);
2118  cv::imwrite("blendSmallG.png", img);
2119  channels[2].convertTo(img,CV_8U,128.0,0);
2120  cv::imwrite("blendSmallR.png", img);*/
2121 
2122  cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
2123  cv::Mat dst;
2124  cv::blur(blendGains[i], dst, cv::Size(3,3));
2125  cv::resize(dst, blendGains[i], globalTexturesROI.size(), 0, 0, cv::INTER_LINEAR);
2126 
2127  /*cv::split(blendGains, channels);
2128  channels[0].convertTo(img,CV_8U,128.0,0);
2129  cv::imwrite("blendFullB.png", img);
2130  channels[1].convertTo(img,CV_8U,128.0,0);
2131  cv::imwrite("blendFullG.png", img);
2132  channels[2].convertTo(img,CV_8U,128.0,0);
2133  cv::imwrite("blendFullR.png", img);*/
2134 
2135  cv::multiply(globalTexturesROI, blendGains[i], globalTexturesROI, 1.0, CV_8UC3);
2136 
2137  //UWARN("Saving blending.png", globalTexture);
2138  //cv::imwrite("blending.png", globalTexture);
2139  }
2140 
2141  if(state) state->callback(uFormat("Blending (decimation=%d) %fs", decimation, timer.ticks()));
2142  }
2143  }
2144 
2145  if(brightnessContrastRatioLow > 0 || brightnessContrastRatioHigh > 0)
2146  {
2147  if(exposureFusion)
2148  {
2149  std::vector<cv::Mat> images;
2150  images.push_back(globalTextures);
2151  if (brightnessContrastRatioLow > 0)
2152  {
2153  images.push_back(util2d::brightnessAndContrastAuto(
2154  globalTextures,
2155  globalTextureMasks,
2156  (float)brightnessContrastRatioLow,
2157  0.0f));
2158  }
2159  if (brightnessContrastRatioHigh > 0)
2160  {
2161  images.push_back(util2d::brightnessAndContrastAuto(
2162  globalTextures,
2163  globalTextureMasks,
2164  0.0f,
2165  (float)brightnessContrastRatioHigh));
2166  }
2167 
2168  globalTextures = util2d::exposureFusion(images);
2169  }
2170  else
2171  {
2172  float alpha, beta;
2173  globalTextures = util2d::brightnessAndContrastAuto(
2174  globalTextures,
2175  globalTextureMasks,
2176  (float)brightnessContrastRatioLow,
2177  (float)brightnessContrastRatioHigh,
2178  &alpha,
2179  &beta);
2180  if(contrastValuesOut)
2181  {
2182  contrastValuesOut->first = alpha;
2183  contrastValuesOut->second = beta;
2184  }
2185  }
2186  if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
2187  }
2188  }
2189  }
2190  }
2191  UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
2192  return globalTextures;
2193 }
2194 
2195 void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
2196 {
2197  // VTK issue:
2198  // tex_coordinates should be linked to points, not
2199  // polygon vertices. Points linked to multiple different TCoords (different textures) should
2200  // be duplicated.
2201  for (unsigned int t = 0; t < textureMesh.tex_coordinates.size(); ++t)
2202  {
2203  if(textureMesh.tex_polygons[t].size())
2204  {
2205  pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
2206  pcl::fromPCLPointCloud2(textureMesh.cloud, *originalCloud);
2207 
2208  // make a cloud with as many points than polygon vertices
2209  unsigned int nPoints = textureMesh.tex_coordinates[t].size();
2210  UASSERT(nPoints == textureMesh.tex_polygons[t].size()*textureMesh.tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
2211 
2212  pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>);
2213  newCloud->resize(nPoints);
2214 
2215  unsigned int oi = 0;
2216  for (unsigned int i = 0; i < textureMesh.tex_polygons[t].size(); ++i)
2217  {
2218  pcl::Vertices & vertices = textureMesh.tex_polygons[t][i];
2219 
2220  for(unsigned int j=0; j<vertices.vertices.size(); ++j)
2221  {
2222  UASSERT(oi < newCloud->size());
2223  UASSERT_MSG((size_t)vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
2224  newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
2225  vertices.vertices[j] = oi; // new vertex index
2226  ++oi;
2227  }
2228  }
2229  pcl::toPCLPointCloud2(*newCloud, textureMesh.cloud);
2230  }
2231  }
2232 }
2233 
2234 bool multiBandTexturing(
2235  const std::string & outputOBJPath,
2236  const pcl::PCLPointCloud2 & cloud,
2237  const std::vector<pcl::Vertices> & polygons,
2238  const std::map<int, Transform> & cameraPoses,
2239  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels, // required output of util3d::createTextureMesh()
2240  const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
2241  const std::map<int, std::vector<CameraModel> > & cameraModels, // Should match images
2242  const Memory * memory, // Should be set if images are not set
2243  const DBDriver * dbDriver, // Should be set if images and memory are not set
2244  int textureSize,
2245  const std::string & textureFormat,
2246  const std::map<int, std::map<int, cv::Vec4d> > & gains, // optional output of util3d::mergeTextures()
2247  const std::map<int, std::map<int, cv::Mat> > & blendingGains, // optional output of util3d::mergeTextures()
2248  const std::pair<float, float> & contrastValues, // optional output of util3d::mergeTextures()
2249  bool gainRGB)
2250 {
2251  return multiBandTexturing(
2252  outputOBJPath,
2253  cloud,
2254  polygons,
2255  cameraPoses,
2256  vertexToPixels,
2257  images,
2258  cameraModels,
2259  memory,
2260  dbDriver,
2261  textureSize,
2262  2,
2263  "1 5 10 0",
2264  textureFormat,
2265  gains,
2266  blendingGains,
2267  contrastValues,
2268  gainRGB);
2269 }
2270 
2271 bool multiBandTexturing(
2272  const std::string & outputOBJPath,
2273  const pcl::PCLPointCloud2 & cloud,
2274  const std::vector<pcl::Vertices> & polygons,
2275  const std::map<int, Transform> & cameraPoses,
2276  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
2277  const std::map<int, cv::Mat> & images,
2278  const std::map<int, std::vector<CameraModel> > & cameraModels,
2279  const Memory * memory,
2280  const DBDriver * dbDriver,
2281  unsigned int textureSize,
2282  unsigned int textureDownScale,
2283  const std::string & nbContrib,
2284  const std::string & textureFormat,
2285  const std::map<int, std::map<int, cv::Vec4d> > & gains,
2286  const std::map<int, std::map<int, cv::Mat> > & blendingGains,
2287  const std::pair<float, float> & contrastValues,
2288  bool gainRGB,
2289  unsigned int unwrapMethod,
2290  bool fillHoles,
2291  unsigned int padding,
2292  double bestScoreThreshold,
2293  double angleHardThreshold,
2294  bool forceVisibleByAllVertices)
2295 {
2296 
2297 #ifdef RTABMAP_ALICE_VISION
2298  if(ULogger::level() == ULogger::kDebug)
2299  {
2300  system::Logger::get()->setLogLevel(system::EVerboseLevel::Trace);
2301  }
2302  else if(ULogger::level() == ULogger::kInfo)
2303  {
2304  system::Logger::get()->setLogLevel(system::EVerboseLevel::Info);
2305  }
2306  else if(ULogger::level() == ULogger::kWarning)
2307  {
2308  system::Logger::get()->setLogLevel(system::EVerboseLevel::Warning);
2309  }
2310  else
2311  {
2312  system::Logger::get()->setLogLevel(system::EVerboseLevel::Error);
2313  }
2314 
2315  sfmData::SfMData sfmData;
2316  pcl::PointCloud<pcl::PointXYZRGB> cloud2;
2317  pcl::fromPCLPointCloud2(cloud, cloud2);
2318  UASSERT(vertexToPixels.size() == cloud2.size());
2319  UINFO("Input mesh: %d points %d polygons", (int)cloud2.size(), (int)polygons.size());
2320  mesh::Texturing texturing;
2321 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
2322  texturing.mesh = new mesh::Mesh();
2323  texturing.mesh->pts.resize(cloud2.size());
2324  texturing.mesh->pointsVisibilities.resize(cloud2.size());
2325 #else
2326  texturing.me = new mesh::Mesh();
2327  texturing.me->pts = new StaticVector<Point3d>(cloud2.size());
2328  texturing.pointsVisibilities = new mesh::PointsVisibility();
2329  texturing.pointsVisibilities->reserve(cloud2.size());
2330 #endif
2331  texturing.texParams.textureSide = textureSize;
2332  texturing.texParams.downscale = textureDownScale;
2333  std::vector<int> multiBandNbContrib;
2334  std::list<std::string> values = uSplit(nbContrib, ' ');
2335  for(std::list<std::string>::iterator iter=values.begin(); iter!=values.end(); ++iter)
2336  {
2337  multiBandNbContrib.push_back(uStr2Int(*iter));
2338  }
2339  if(multiBandNbContrib.size() != 4)
2340  {
2341  UERROR("multiband: Wrong number of nb of contribution (vaue=\"%s\", should be 4), using default values instead.", nbContrib.c_str());
2342  }
2343  else
2344  {
2345  texturing.texParams.multiBandNbContrib = multiBandNbContrib;
2346  }
2347  texturing.texParams.padding = padding;
2348  texturing.texParams.fillHoles = fillHoles;
2349  texturing.texParams.bestScoreThreshold = bestScoreThreshold;
2350  texturing.texParams.angleHardThreshold = angleHardThreshold;
2351  texturing.texParams.forceVisibleByAllVertices = forceVisibleByAllVertices;
2352  texturing.texParams.visibilityRemappingMethod = mesh::EVisibilityRemappingMethod::Pull;
2353 
2354 
2355  for(size_t i=0;i<cloud2.size();++i)
2356  {
2357  pcl::PointXYZRGB pt = cloud2.at(i);
2358 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
2359  texturing.mesh->pointsVisibilities[i].reserve(vertexToPixels[i].size());
2360  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
2361  {
2362  texturing.mesh->pointsVisibilities[i].push_back(iter->first);
2363  }
2364  texturing.mesh->pts[i] = Point3d(pt.x, pt.y, pt.z);
2365 #else
2366  mesh::PointVisibility* pointVisibility = new mesh::PointVisibility();
2367  pointVisibility->reserve(vertexToPixels[i].size());
2368  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
2369  {
2370  pointVisibility->push_back(iter->first);
2371  }
2372  texturing.pointsVisibilities->push_back(pointVisibility);
2373  (*texturing.me->pts)[i] = Point3d(pt.x, pt.y, pt.z);
2374 #endif
2375  }
2376 
2377 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
2378  texturing.mesh->tris.resize(polygons.size());
2379  texturing.mesh->trisMtlIds().resize(polygons.size());
2380 #else
2381  texturing.me->tris = new StaticVector<mesh::Mesh::triangle>(polygons.size());
2382 #endif
2383  for(size_t i=0;i<polygons.size();++i)
2384  {
2385  UASSERT(polygons[i].vertices.size() == 3);
2386 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
2387  texturing.mesh->trisMtlIds()[i] = -1;
2388  texturing.mesh->tris[i] = mesh::Mesh::triangle(
2389 #else
2390  (*texturing.me->tris)[i] = mesh::Mesh::triangle(
2391 #endif
2392  polygons[i].vertices[0],
2393  polygons[i].vertices[1],
2394  polygons[i].vertices[2]);
2395  }
2396  UTimer timer;
2397  std::string outputDirectory = UDirectory::getDir(outputOBJPath);
2398  std::string tmpImageDirectory = outputDirectory+"/rtabmap_tmp_textures";
2399  UDirectory::removeDir(tmpImageDirectory);
2400  UDirectory::makeDir(tmpImageDirectory);
2401  UINFO("Temporary saving images in directory \"%s\"...", tmpImageDirectory.c_str());
2402  int viewId = 0;
2403  for(std::map<int, Transform>::const_iterator iter = cameraPoses.lower_bound(1); iter!=cameraPoses.end(); ++iter)
2404  {
2405  int camId = iter->first;
2406  cv::Mat image;
2407  std::vector<CameraModel> models;
2408 
2409  if( images.find(camId) != images.end() &&
2410  !images.find(camId)->second.empty() &&
2411  cameraModels.find(camId) != cameraModels.end())
2412  {
2413  image = images.find(camId)->second;
2414  models = cameraModels.find(camId)->second;
2415  }
2416  else if(memory)
2417  {
2418  SensorData data = memory->getNodeData(camId, true, false, false, false);
2419  models = data.cameraModels();
2420  if(models.empty() && data.stereoCameraModels().size())
2421  {
2422  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
2423  {
2424  models.push_back(data.stereoCameraModels()[i].left());
2425  }
2426  }
2427  if(data.imageRaw().empty())
2428  {
2429  image = data.imageCompressed();
2430  }
2431  else
2432  {
2433  image = data.imageRaw();
2434  }
2435 
2436  if(models.empty() || image.empty())
2437  {
2438  Transform odomPose;
2439  int mapId;
2440  int weight=0;
2441  std::string label;
2442  double stamp;
2443  Transform gt;
2444  std::vector<float> vel;
2445  GPS gps;
2446  EnvSensors envs;
2447  memory->getNodeInfo(camId, odomPose, mapId, weight, label, stamp, gt, vel, gps, envs, true);
2448  if(weight == -1) // just ignore intermediate nodes if their data is not set
2449  continue;
2450  }
2451  }
2452  else if(dbDriver)
2453  {
2454  std::vector<StereoCameraModel> stereoModels;
2455  dbDriver->getCalibration(camId, models, stereoModels);
2456  if(models.empty() && stereoModels.size())
2457  {
2458  for(size_t i=0; i<stereoModels.size(); ++i)
2459  {
2460  models.push_back(stereoModels[i].left());
2461  }
2462  }
2463 
2464  SensorData data;
2465  dbDriver->getNodeData(camId, data, true, false, false, false);
2466  if(data.imageRaw().empty())
2467  {
2468  image = data.imageCompressed();
2469  }
2470  else
2471  {
2472  image = data.imageRaw();
2473  }
2474 
2475  if(models.empty() || image.empty())
2476  {
2477  int weight=0;
2478  dbDriver->getWeight(camId, weight);
2479  if(weight == -1) // just ignore intermediate nodes if their data is not set
2480  continue;
2481  }
2482  }
2483  if(models.empty())
2484  {
2485  UERROR("No camera models found for camera %d. Aborting multiband texturing...", iter->first);
2486  return false;
2487  }
2488  if(image.empty())
2489  {
2490  UERROR("No image found for camera %d. Aborting multiband texturing...", iter->first);
2491  return false;
2492  }
2493 
2494  if(image.rows == 1 && image.type() == CV_8UC1)
2495  {
2496  image = uncompressImage(image);
2497  }
2498  else
2499  {
2500  image = image.clone();
2501  }
2502 
2503  for(size_t i=0; i<models.size(); ++i)
2504  {
2505  const CameraModel & model = models.at(i);
2506  cv::Size imageSize = model.imageSize();
2507  if(imageSize.height == 0)
2508  {
2509  // backward compatibility
2510  imageSize.height = image.rows;
2511  imageSize.width = image.cols;
2512  }
2513  UASSERT(image.cols % imageSize.width == 0);
2514  cv::Mat imageRoi = image.colRange(i*imageSize.width, (i+1)*imageSize.width);
2515  if(gains.find(camId) != gains.end() &&
2516  gains.at(camId).find(i) != gains.at(camId).end())
2517  {
2518  const cv::Vec4d & g = gains.at(camId).at(i);
2519  std::vector<cv::Mat> channels;
2520  cv::split(imageRoi, channels);
2521 
2522  // assuming BGR
2523  cv::multiply(channels[0], g.val[gainRGB?3:0], channels[0]);
2524  cv::multiply(channels[1], g.val[gainRGB?2:0], channels[1]);
2525  cv::multiply(channels[2], g.val[gainRGB?1:0], channels[2]);
2526 
2527  cv::Mat output;
2528  cv::merge(channels, output);
2529  imageRoi = output;
2530  }
2531 
2532  if(blendingGains.find(camId) != blendingGains.end() &&
2533  blendingGains.at(camId).find(i) != blendingGains.at(camId).end())
2534  {
2535  cv::Mat g = blendingGains.at(camId).at(i);
2536  cv::Mat dst;
2537  cv::blur(g, dst, cv::Size(3,3));
2538  cv::Mat gResized;
2539  cv::resize(dst, gResized, imageRoi.size(), 0, 0, cv::INTER_LINEAR);
2540  cv::Mat output;
2541  cv::multiply(imageRoi, gResized, output, 1.0, CV_8UC3);
2542  imageRoi = output;
2543  }
2544 
2545  Transform t = (iter->second * model.localTransform()).inverse();
2546  Eigen::Matrix<double, 3, 4> m = t.toEigen3d().matrix().block<3,4>(0, 0);
2547  sfmData::CameraPose pose(geometry::Pose3(m), true);
2548  sfmData.setAbsolutePose((IndexT)viewId, pose);
2549 
2550  UDEBUG("%d %d %f %f %f %f", imageSize.width, imageSize.height, model.fx(), model.fy(), model.cx(), model.cy());
2551  std::shared_ptr<camera::IntrinsicBase> camPtr = std::make_shared<camera::Pinhole>(
2552 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=4)
2553  //https://github.com/alicevision/AliceVision/commit/9fab5c79a1c65595fe5c5001267e1c5212bc93f0#diff-b0c0a3c30de50be8e4ed283dfe4c8ae4a9bc861aa9a83bd8bfda8182e9d67c08
2554  // [all] the camera principal point is now defined as an offset relative to the image center
2555  imageSize.width, imageSize.height, model.fx(), model.fy(), model.cx() - double(imageSize.width) * 0.5, model.cy() - double(imageSize.height) * 0.5);
2556 #else
2557  imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy());
2558 #endif
2559  sfmData.intrinsics.insert(std::make_pair((IndexT)viewId, camPtr));
2560 
2561  std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", viewId);
2562 
2563  cv::imwrite(imagePath, imageRoi);
2564 
2565  std::shared_ptr<sfmData::View> viewPtr = std::make_shared<sfmData::View>(
2566  imagePath,
2567  (IndexT)viewId,
2568  (IndexT)viewId,
2569  (IndexT)viewId,
2570  imageSize.width,
2571  imageSize.height);
2572  sfmData.views.insert(std::make_pair((IndexT)viewId, viewPtr));
2573  ++viewId;
2574  }
2575  }
2576  UINFO("Temporary saving images in directory \"%s\"... done (%d images). %fs", tmpImageDirectory.c_str(), viewId, (int)cameraPoses.size(), timer.ticks());
2577 
2578  mvsUtils::MultiViewParams mp(sfmData);
2579 
2580  UINFO("Unwrapping (method=%d=%s)...", unwrapMethod, mesh::EUnwrapMethod_enumToString((mesh::EUnwrapMethod)unwrapMethod).c_str());
2581  texturing.unwrap(mp, (mesh::EUnwrapMethod)unwrapMethod);
2582  UINFO("Unwrapping done. %fs", timer.ticks());
2583 
2584  // save final obj file
2585  std::string baseName = uSplit(UFile::getName(outputOBJPath), '.').front();
2586 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=4)
2587  texturing.saveAs(outputDirectory, baseName, aliceVision::mesh::EFileType::OBJ, imageIO::EImageFileType::PNG);
2588 #else
2589  texturing.saveAsOBJ(outputDirectory, baseName);
2590 #endif
2591  UINFO("Saved %s. %fs", outputOBJPath.c_str(), timer.ticks());
2592 
2593  // generate textures
2594  UINFO("Generating textures...");
2595  texturing.generateTextures(mp, outputDirectory);
2596  UINFO("Generating textures done. %fs", timer.ticks());
2597 
2598  UINFO("Cleanup temporary directory \"%s\"...", tmpImageDirectory.c_str());
2599  UDirectory dir(tmpImageDirectory);
2600  std::string fp = dir.getNextFilePath();
2601  while(!fp.empty())
2602  {
2603  UFile::erase(fp);
2604  fp = dir.getNextFilePath();
2605  }
2606  UDirectory::removeDir(tmpImageDirectory);
2607  UINFO("Cleanup temporary directory \"%s\"... done.", tmpImageDirectory.c_str());
2608 
2609  UINFO("Rename/convert textures...");
2610  dir.setPath(outputDirectory, "png");
2611  std::map<std::string, std::string> texNames; // <old, new>
2612  std::string outputFormat = textureFormat;
2613  if(outputFormat.front() == '.')
2614  {
2615  outputFormat = outputFormat.substr(1, std::string::npos);
2616  }
2617  for(std::list<std::string>::const_iterator iter=dir.getFileNames().begin(); iter!=dir.getFileNames().end(); ++iter)
2618  {
2619  // Textures are called "texture_1001.png", "texture_1002.png", ...
2620  if(uStrContains(*iter, "texture_10"))
2621  {
2622  cv::Mat img = cv::imread(outputDirectory+"/"+*iter);
2623  if(contrastValues.first != 0.0f || contrastValues.second != 0.0f)
2624  {
2625  UASSERT(img.channels() == 3);
2626  // Re-use same contrast values with all images
2627  UINFO("Apply contrast values %f %f", contrastValues.first, contrastValues.second);
2628  img.convertTo(img, -1, contrastValues.first, contrastValues.second);
2629  }
2630  std::string newName = *iter;
2631  boost::replace_all(newName, "png", outputFormat);
2632  boost::replace_all(newName, "texture", baseName);
2633  texNames.insert(std::make_pair(*iter, newName));
2634  cv::imwrite(outputDirectory+"/"+newName, img);
2635  UFile::erase(outputDirectory+"/"+*iter);
2636  }
2637  }
2638  std::ifstream fi(outputDirectory+"/"+baseName+".mtl");
2639  std::string mtlStr((std::istreambuf_iterator<char>(fi)),
2640  std::istreambuf_iterator<char>());
2641  fi.close();
2642  UFile::erase(outputDirectory+"/"+baseName);
2643  for(std::map<std::string, std::string>::iterator iter=texNames.begin(); iter!=texNames.end(); ++iter)
2644  {
2645  boost::replace_all(mtlStr, iter->first, iter->second);
2646  }
2647  std::ofstream fo(outputDirectory+"/"+baseName+".mtl");
2648  fo.write(mtlStr.c_str(), mtlStr.size());
2649  fo.close();
2650  UINFO("Rename/convert textures... done. %fs", timer.ticks());
2651 
2652 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
2653  UINFO("Cleanup sfmdata...");
2654  sfmData.clear();
2655  UINFO("Cleanup sfmdata... done. %fs", timer.ticks());
2656 #endif
2657 
2658  return true;
2659 #else
2660  UERROR("Cannot unwrap texture mesh. RTAB-Map is not built with Alice Vision support! Returning false.");
2661  return false;
2662 #endif
2663 }
2664 
2665 LaserScan computeNormals(
2666  const LaserScan & laserScan,
2667  int searchK,
2668  float searchRadius)
2669 {
2670  if(laserScan.isEmpty())
2671  {
2672  return laserScan;
2673  }
2674 
2675  pcl::PointCloud<pcl::Normal>::Ptr normals;
2676  // convert to compatible PCL format and filter it
2677  if(laserScan.hasRGB())
2678  {
2679  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(laserScan);
2680  if(cloud->size())
2681  {
2682  UASSERT(!laserScan.is2d());
2683  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
2684  return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
2685  }
2686  }
2687  else if(laserScan.hasIntensity())
2688  {
2689  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(laserScan);
2690  if(cloud->size())
2691  {
2692  if(laserScan.is2d())
2693  {
2694  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
2695  if(laserScan.angleIncrement() > 0.0f)
2696  {
2697  return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
2698  }
2699  else
2700  {
2701  return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
2702  }
2703 
2704  }
2705  else
2706  {
2707  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
2708  return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
2709  }
2710  }
2711  }
2712  else
2713  {
2714  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(laserScan);
2715  if(cloud->size())
2716  {
2717  if(laserScan.is2d())
2718  {
2719  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
2720  if(laserScan.angleIncrement() > 0.0f)
2721  {
2722  return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
2723  }
2724  else
2725  {
2726  return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
2727  }
2728  }
2729  else
2730  {
2731  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
2732  return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
2733  }
2734  }
2735  }
2736  return LaserScan();
2737 }
2738 
2739 template<typename PointT>
2740 pcl::PointCloud<pcl::Normal>::Ptr computeNormalsImpl(
2741  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2742  const pcl::IndicesPtr & indices,
2743  int searchK,
2744  float searchRadius,
2745  const Eigen::Vector3f & viewPoint)
2746 {
2747  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
2748  if(indices->size())
2749  {
2750  tree->setInputCloud(cloud, indices);
2751  }
2752  else
2753  {
2754  tree->setInputCloud (cloud);
2755  }
2756 
2757  // Normal estimation*
2758 #ifdef PCL_OMP
2759  pcl::NormalEstimationOMP<PointT, pcl::Normal> n;
2760 #else
2761  pcl::NormalEstimation<PointT, pcl::Normal> n;
2762 #endif
2763  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2764  n.setInputCloud (cloud);
2765  // Commented: Keep the output normals size the same as the input cloud
2766  //if(indices->size())
2767  //{
2768  // n.setIndices(indices);
2769  //}
2770  n.setSearchMethod (tree);
2771  n.setKSearch (searchK);
2772  n.setRadiusSearch (searchRadius);
2773  n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
2774  n.compute (*normals);
2775 
2776  return normals;
2777 }
2778 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2779  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2780  int searchK,
2781  float searchRadius,
2782  const Eigen::Vector3f & viewPoint)
2783 {
2784  pcl::IndicesPtr indices(new std::vector<int>);
2785  return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
2786 }
2787 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2788  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2789  int searchK,
2790  float searchRadius,
2791  const Eigen::Vector3f & viewPoint)
2792 {
2793  pcl::IndicesPtr indices(new std::vector<int>);
2794  return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
2795 }
2796 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2797  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2798  int searchK,
2799  float searchRadius,
2800  const Eigen::Vector3f & viewPoint)
2801 {
2802  pcl::IndicesPtr indices(new std::vector<int>);
2803  return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
2804 }
2805 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2806  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2807  const pcl::IndicesPtr & indices,
2808  int searchK,
2809  float searchRadius,
2810  const Eigen::Vector3f & viewPoint)
2811 {
2812  return computeNormalsImpl<pcl::PointXYZ>(cloud, indices, searchK, searchRadius, viewPoint);
2813 }
2814 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2815  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2816  const pcl::IndicesPtr & indices,
2817  int searchK,
2818  float searchRadius,
2819  const Eigen::Vector3f & viewPoint)
2820 {
2821  return computeNormalsImpl<pcl::PointXYZRGB>(cloud, indices, searchK, searchRadius, viewPoint);
2822 }
2823 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2824  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2825  const pcl::IndicesPtr & indices,
2826  int searchK,
2827  float searchRadius,
2828  const Eigen::Vector3f & viewPoint)
2829 {
2830  return computeNormalsImpl<pcl::PointXYZI>(cloud, indices, searchK, searchRadius, viewPoint);
2831 }
2832 
2833 template<typename PointT>
2834 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2DImpl(
2835  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2836  int searchK,
2837  float searchRadius,
2838  const Eigen::Vector3f & viewPoint)
2839 {
2840  UASSERT(searchK>0 || searchRadius>0.0f);
2841  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2842 
2843  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
2844  tree->setInputCloud (cloud);
2845 
2846  normals->resize(cloud->size());
2847 
2848  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2849 
2850  // assuming that points are ordered
2851  for(unsigned int i=0; i<cloud->size(); ++i)
2852  {
2853  const PointT & pt = cloud->at(i);
2854  std::vector<Eigen::Vector3f> neighborNormals;
2855  Eigen::Vector3f direction;
2856  direction[0] = viewPoint[0] - pt.x;
2857  direction[1] = viewPoint[1] - pt.y;
2858  direction[2] = viewPoint[2] - pt.z;
2859 
2860  std::vector<int> k_indices;
2861  std::vector<float> k_sqr_distances;
2862  if(searchRadius>0.0f)
2863  {
2864  tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
2865  }
2866  else
2867  {
2868  tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
2869  }
2870 
2871  for(unsigned int j=0; j<k_indices.size(); ++j)
2872  {
2873  if(k_indices.at(j) != (int)i)
2874  {
2875  const PointT & pt2 = cloud->at(k_indices.at(j));
2876  Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2877  Eigen::Vector3f up = v.cross(direction);
2878  Eigen::Vector3f n = up.cross(v);
2879  n.normalize();
2880  neighborNormals.push_back(n);
2881  }
2882  }
2883 
2884  if(neighborNormals.empty())
2885  {
2886  normals->at(i).normal_x = bad_point;
2887  normals->at(i).normal_y = bad_point;
2888  normals->at(i).normal_z = bad_point;
2889  }
2890  else
2891  {
2892  Eigen::Vector3f meanNormal(0,0,0);
2893  for(unsigned int j=0; j<neighborNormals.size(); ++j)
2894  {
2895  meanNormal+=neighborNormals[j];
2896  }
2897  meanNormal /= (float)neighborNormals.size();
2898  meanNormal.normalize();
2899  normals->at(i).normal_x = meanNormal[0];
2900  normals->at(i).normal_y = meanNormal[1];
2901  normals->at(i).normal_z = meanNormal[2];
2902  }
2903  }
2904 
2905  return normals;
2906 }
2907 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
2908  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2909  int searchK,
2910  float searchRadius,
2911  const Eigen::Vector3f & viewPoint)
2912 {
2913  return computeNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
2914 }
2915 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
2916  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2917  int searchK,
2918  float searchRadius,
2919  const Eigen::Vector3f & viewPoint)
2920 {
2921  return computeNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
2922 }
2923 
2924 template<typename PointT>
2925 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2DImpl(
2926  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2927  int searchK,
2928  float searchRadius,
2929  const Eigen::Vector3f & viewPoint)
2930 {
2931  UASSERT(searchK>0);
2932  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2933 
2934  normals->resize(cloud->size());
2935  searchRadius *= searchRadius; // squared distance
2936 
2937  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2938 
2939  // assuming that points are ordered
2940  for(int i=0; i<(int)cloud->size(); ++i)
2941  {
2942  int li = i-searchK;
2943  if(li<0)
2944  {
2945  li=0;
2946  }
2947  int hi = i+searchK;
2948  if(hi>=(int)cloud->size())
2949  {
2950  hi=(int)cloud->size()-1;
2951  }
2952 
2953  // get points before not too far
2954  const PointT & pt = cloud->at(i);
2955  std::vector<Eigen::Vector3f> neighborNormals;
2956  Eigen::Vector3f direction;
2957  direction[0] = viewPoint[0] - cloud->at(i).x;
2958  direction[1] = viewPoint[1] - cloud->at(i).y;
2959  direction[2] = viewPoint[2] - cloud->at(i).z;
2960  for(int j=i-1; j>=li; --j)
2961  {
2962  const PointT & pt2 = cloud->at(j);
2963  Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2964  if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
2965  {
2966  Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2967  Eigen::Vector3f up = v.cross(direction);
2968  Eigen::Vector3f n = up.cross(v);
2969  n.normalize();
2970  neighborNormals.push_back(n);
2971  }
2972  else
2973  {
2974  break;
2975  }
2976  }
2977  for(int j=i+1; j<=hi; ++j)
2978  {
2979  const PointT & pt2 = cloud->at(j);
2980  Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2981  if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
2982  {
2983  Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2984  Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
2985  Eigen::Vector3f n = up.cross(v);
2986  n.normalize();
2987  neighborNormals.push_back(n);
2988  }
2989  else
2990  {
2991  break;
2992  }
2993  }
2994 
2995  if(neighborNormals.empty())
2996  {
2997  normals->at(i).normal_x = bad_point;
2998  normals->at(i).normal_y = bad_point;
2999  normals->at(i).normal_z = bad_point;
3000  }
3001  else
3002  {
3003  Eigen::Vector3f meanNormal(0,0,0);
3004  for(unsigned int j=0; j<neighborNormals.size(); ++j)
3005  {
3006  meanNormal+=neighborNormals[j];
3007  }
3008  meanNormal /= (float)neighborNormals.size();
3009  meanNormal.normalize();
3010  normals->at(i).normal_x = meanNormal[0];
3011  normals->at(i).normal_y = meanNormal[1];
3012  normals->at(i).normal_z = meanNormal[2];
3013  }
3014  }
3015 
3016  return normals;
3017 }
3018 
3019 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
3020  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
3021  int searchK,
3022  float searchRadius,
3023  const Eigen::Vector3f & viewPoint)
3024 {
3025  return computeFastOrganizedNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
3026 }
3027 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
3028  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
3029  int searchK,
3030  float searchRadius,
3031  const Eigen::Vector3f & viewPoint)
3032 {
3033  return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
3034 }
3035 
3036 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
3037  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
3038  float maxDepthChangeFactor,
3039  float normalSmoothingSize,
3040  const Eigen::Vector3f & viewPoint)
3041 {
3042  pcl::IndicesPtr indices(new std::vector<int>);
3043  return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
3044 }
3045 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
3046  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
3047  const pcl::IndicesPtr & indices,
3048  float maxDepthChangeFactor,
3049  float normalSmoothingSize,
3050  const Eigen::Vector3f & viewPoint)
3051 {
3052  UASSERT(cloud->isOrganized());
3053 
3054  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
3055  if(indices->size())
3056  {
3057  tree->setInputCloud(cloud, indices);
3058  }
3059  else
3060  {
3061  tree->setInputCloud (cloud);
3062  }
3063 
3064  // Normal estimation
3065  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
3066  pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
3067  ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
3068  ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
3069  ne.setNormalSmoothingSize(normalSmoothingSize);
3070  ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
3071  ne.setInputCloud(cloud);
3072  // Commented: Keep the output normals size the same as the input cloud
3073  //if(indices->size())
3074  //{
3075  // ne.setIndices(indices);
3076  //}
3077  ne.setSearchMethod(tree);
3078  ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
3079  ne.compute(*normals);
3080 
3081  return normals;
3082 }
3083 
3084 float computeNormalsComplexity(
3085  const LaserScan & scan,
3086  const Transform & t,
3087  cv::Mat * pcaEigenVectors,
3088  cv::Mat * pcaEigenValues)
3089 {
3090  if(!scan.isEmpty() && (scan.hasNormals()))
3091  {
3092  //Construct a buffer used by the pca analysis
3093  int sz = static_cast<int>(scan.size()*2);
3094  bool is2d = scan.is2d();
3095  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
3096  int oi = 0;
3097  int nOffset = scan.getNormalsOffset();
3098  bool doTransform = false;
3099  Transform tn;
3100  if(!t.isIdentity() || !scan.localTransform().isIdentity())
3101  {
3102  tn = (t*scan.localTransform()).rotation();
3103  doTransform = true;
3104  }
3105  for (int i = 0; i < scan.size(); ++i)
3106  {
3107  const float * ptrScan = scan.data().ptr<float>(0, i);
3108 
3109  if(is2d)
3110  {
3111  if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]))
3112  {
3113  cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], 0);
3114  if(doTransform)
3115  {
3116  n = util3d::transformPoint(n, tn);
3117  }
3118  float * ptr = data_normals.ptr<float>(oi++, 0);
3119  ptr[0] = n.x;
3120  ptr[1] = n.y;
3121  }
3122  }
3123  else
3124  {
3125  if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]) && uIsFinite(ptrScan[nOffset+2]))
3126  {
3127  cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], ptrScan[nOffset+2]);
3128  if(doTransform)
3129  {
3130  n = util3d::transformPoint(n, tn);
3131  }
3132  float * ptr = data_normals.ptr<float>(oi++, 0);
3133  ptr[0] = n.x;
3134  ptr[1] = n.y;
3135  ptr[2] = n.z;
3136  }
3137  }
3138  }
3139  if(oi>1)
3140  {
3141  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
3142 
3143  if(pcaEigenVectors)
3144  {
3145  *pcaEigenVectors = pca_analysis.eigenvectors;
3146  }
3147  if(pcaEigenValues)
3148  {
3149  *pcaEigenValues = pca_analysis.eigenvalues;
3150  }
3151  UASSERT((is2d && pca_analysis.eigenvalues.total()>=2) || (!is2d && pca_analysis.eigenvalues.total()>=3));
3152  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
3153  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
3154  }
3155  }
3156  else if(!scan.isEmpty())
3157  {
3158  UERROR("Scan doesn't have normals!");
3159  }
3160  return 0.0f;
3161 }
3162 
3163 float computeNormalsComplexity(
3164  const pcl::PointCloud<pcl::PointNormal> & cloud,
3165  const Transform & t,
3166  bool is2d,
3167  cv::Mat * pcaEigenVectors,
3168  cv::Mat * pcaEigenValues)
3169 {
3170  //Construct a buffer used by the pca analysis
3171  int sz = static_cast<int>(cloud.size()*2);
3172  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
3173  int oi = 0;
3174  bool doTransform = false;
3175  Transform tn;
3176  if(!t.isIdentity() && !t.isNull())
3177  {
3178  tn = t.rotation();
3179  doTransform = true;
3180  }
3181  for (unsigned int i = 0; i < cloud.size(); ++i)
3182  {
3183  const pcl::PointNormal & pt = cloud.at(i);
3184  cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
3185  if(doTransform)
3186  {
3187  n = util3d::transformPoint(n, tn);
3188  }
3189  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
3190  {
3191  float * ptr = data_normals.ptr<float>(oi++, 0);
3192  ptr[0] = n.x;
3193  ptr[1] = n.y;
3194  if(!is2d)
3195  {
3196  ptr[2] = n.z;
3197  }
3198  }
3199  }
3200  if(oi>1)
3201  {
3202  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
3203 
3204  if(pcaEigenVectors)
3205  {
3206  *pcaEigenVectors = pca_analysis.eigenvectors;
3207  }
3208  if(pcaEigenValues)
3209  {
3210  *pcaEigenValues = pca_analysis.eigenvalues;
3211  }
3212 
3213  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
3214  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
3215  }
3216  return 0.0f;
3217 }
3218 
3219 float computeNormalsComplexity(
3220  const pcl::PointCloud<pcl::Normal> & normals,
3221  const Transform & t,
3222  bool is2d,
3223  cv::Mat * pcaEigenVectors,
3224  cv::Mat * pcaEigenValues)
3225 {
3226  //Construct a buffer used by the pca analysis
3227  int sz = static_cast<int>(normals.size()*2);
3228  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
3229  int oi = 0;
3230  bool doTransform = false;
3231  Transform tn;
3232  if(!t.isIdentity())
3233  {
3234  tn = t.rotation();
3235  doTransform = true;
3236  }
3237  for (unsigned int i = 0; i < normals.size(); ++i)
3238  {
3239  const pcl::Normal & pt = normals.at(i);
3240  cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
3241  if(doTransform)
3242  {
3243  n = util3d::transformPoint(n, tn);
3244  }
3245  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
3246  {
3247  float * ptr = data_normals.ptr<float>(oi++, 0);
3248  ptr[0] = n.x;
3249  ptr[1] = n.y;
3250  if(!is2d)
3251  {
3252  ptr[2] = n.z;
3253  }
3254  }
3255  }
3256  if(oi>1)
3257  {
3258  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
3259 
3260  if(pcaEigenVectors)
3261  {
3262  *pcaEigenVectors = pca_analysis.eigenvectors;
3263  }
3264  if(pcaEigenValues)
3265  {
3266  *pcaEigenValues = pca_analysis.eigenvalues;
3267  }
3268 
3269  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
3270  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
3271  }
3272  return 0.0f;
3273 }
3274 
3275 float computeNormalsComplexity(
3276  const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
3277  const Transform & t,
3278  bool is2d,
3279  cv::Mat * pcaEigenVectors,
3280  cv::Mat * pcaEigenValues)
3281 {
3282  //Construct a buffer used by the pca analysis
3283  int sz = static_cast<int>(cloud.size()*2);
3284  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
3285  int oi = 0;
3286  bool doTransform = false;
3287  Transform tn;
3288  if(!t.isIdentity())
3289  {
3290  tn = t.rotation();
3291  doTransform = true;
3292  }
3293  for (unsigned int i = 0; i < cloud.size(); ++i)
3294  {
3295  const pcl::PointXYZINormal & pt = cloud.at(i);
3296  cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
3297  if(doTransform)
3298  {
3299  n = util3d::transformPoint(n, tn);
3300  }
3301  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
3302  {
3303  float * ptr = data_normals.ptr<float>(oi++, 0);
3304  ptr[0] = n.x;
3305  ptr[1] = n.y;
3306  if(!is2d)
3307  {
3308  ptr[2] = n.z;
3309  }
3310  }
3311  }
3312  if(oi>1)
3313  {
3314  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
3315 
3316  if(pcaEigenVectors)
3317  {
3318  *pcaEigenVectors = pca_analysis.eigenvectors;
3319  }
3320  if(pcaEigenValues)
3321  {
3322  *pcaEigenValues = pca_analysis.eigenvalues;
3323  }
3324 
3325  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
3326  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
3327  }
3328  return 0.0f;
3329 }
3330 
3331 float computeNormalsComplexity(
3332  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
3333  const Transform & t,
3334  bool is2d,
3335  cv::Mat * pcaEigenVectors,
3336  cv::Mat * pcaEigenValues)
3337 {
3338  //Construct a buffer used by the pca analysis
3339  int sz = static_cast<int>(cloud.size()*2);
3340  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
3341  int oi = 0;
3342  bool doTransform = false;
3343  Transform tn;
3344  if(!t.isIdentity())
3345  {
3346  tn = t.rotation();
3347  doTransform = true;
3348  }
3349  for (unsigned int i = 0; i < cloud.size(); ++i)
3350  {
3351  const pcl::PointXYZRGBNormal & pt = cloud.at(i);
3352  cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
3353  if(doTransform)
3354  {
3355  n = util3d::transformPoint(n, tn);
3356  }
3357  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
3358  {
3359  float * ptr = data_normals.ptr<float>(oi++, 0);
3360  ptr[0] = n.x;
3361  ptr[1] = n.y;
3362  if(!is2d)
3363  {
3364  ptr[2] = n.z;
3365  }
3366  }
3367  }
3368  if(oi>1)
3369  {
3370  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
3371 
3372  if(pcaEigenVectors)
3373  {
3374  *pcaEigenVectors = pca_analysis.eigenvectors;
3375  }
3376  if(pcaEigenValues)
3377  {
3378  *pcaEigenValues = pca_analysis.eigenvalues;
3379  }
3380 
3381  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
3382  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
3383  }
3384  return 0.0f;
3385 }
3386 
3387 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
3388  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
3389  float searchRadius,
3390  int polygonialOrder,
3391  int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
3392  float upsamplingRadius, // SAMPLE_LOCAL_PLANE
3393  float upsamplingStep, // SAMPLE_LOCAL_PLANE
3394  int pointDensity, // RANDOM_UNIFORM_DENSITY
3395  float dilationVoxelSize, // VOXEL_GRID_DILATION
3396  int dilationIterations) // VOXEL_GRID_DILATION
3397 {
3398  pcl::IndicesPtr indices(new std::vector<int>);
3399  return mls(cloud,
3400  indices,
3401  searchRadius,
3402  polygonialOrder,
3403  upsamplingMethod,
3404  upsamplingRadius,
3405  upsamplingStep,
3406  pointDensity,
3407  dilationVoxelSize,
3408  dilationIterations);
3409 }
3410 
3411 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
3412  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
3413  const pcl::IndicesPtr & indices,
3414  float searchRadius,
3415  int polygonialOrder,
3416  int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
3417  float upsamplingRadius, // SAMPLE_LOCAL_PLANE
3418  float upsamplingStep, // SAMPLE_LOCAL_PLANE
3419  int pointDensity, // RANDOM_UNIFORM_DENSITY
3420  float dilationVoxelSize, // VOXEL_GRID_DILATION
3421  int dilationIterations) // VOXEL_GRID_DILATION
3422 {
3423  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3424  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
3425  if(indices->size())
3426  {
3427  tree->setInputCloud (cloud, indices);
3428  }
3429  else
3430  {
3431  tree->setInputCloud (cloud);
3432  }
3433 
3434  // Init object (second point type is for the normals)
3435  pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
3436 
3437  // Set parameters
3438  mls.setComputeNormals (true);
3439  if(polygonialOrder > 0)
3440  {
3441 #if PCL_VERSION_COMPARE(<, 1, 10, 0)
3442  mls.setPolynomialFit (true);
3443 #endif
3444  mls.setPolynomialOrder(polygonialOrder);
3445  }
3446  else
3447  {
3448 #if PCL_VERSION_COMPARE(<, 1, 10, 0)
3449  mls.setPolynomialFit (false);
3450 #else
3451  mls.setPolynomialOrder(1);
3452 #endif
3453  }
3454  UASSERT(upsamplingMethod >= mls.NONE &&
3455  upsamplingMethod <= mls.VOXEL_GRID_DILATION);
3456  mls.setUpsamplingMethod((pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::UpsamplingMethod)upsamplingMethod);
3457  mls.setSearchRadius(searchRadius);
3458  mls.setUpsamplingRadius(upsamplingRadius);
3459  mls.setUpsamplingStepSize(upsamplingStep);
3460  mls.setPointDensity(pointDensity);
3461  mls.setDilationVoxelSize(dilationVoxelSize);
3462  mls.setDilationIterations(dilationIterations);
3463 
3464  // Reconstruct
3465  mls.setInputCloud (cloud);
3466  if(indices->size())
3467  {
3468  mls.setIndices(indices);
3469  }
3470  mls.setSearchMethod (tree);
3471  mls.process (*cloud_with_normals);
3472 
3473  // It seems that returned normals are not normalized!? FIXME: Is it a bug only in PCL 1.7.1?
3474  for(unsigned int i=0; i<cloud_with_normals->size(); ++i)
3475  {
3476  Eigen::Vector3f normal(cloud_with_normals->at(i).normal_x, cloud_with_normals->at(i).normal_y, cloud_with_normals->at(i).normal_z);
3477  normal.normalize();
3478  cloud_with_normals->at(i).normal_x = normal[0];
3479  cloud_with_normals->at(i).normal_y = normal[1];
3480  cloud_with_normals->at(i).normal_z = normal[2];
3481  }
3482 
3483  return cloud_with_normals;
3484 }
3485 
3486 LaserScan adjustNormalsToViewPoint(
3487  const LaserScan & scan,
3488  const Eigen::Vector3f & viewpoint,
3489  bool forceGroundNormalsUp)
3490 {
3491  return adjustNormalsToViewPoint(scan, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
3492 }
3493 LaserScan adjustNormalsToViewPoint(
3494  const LaserScan & scan,
3495  const Eigen::Vector3f & viewpoint,
3496  float groundNormalsUp)
3497 {
3498  if(scan.size() && !scan.is2d() && scan.hasNormals())
3499  {
3500  int nx = scan.getNormalsOffset();
3501  int ny = nx+1;
3502  int nz = ny+1;
3503  cv::Mat output = scan.data().clone();
3504  #pragma omp parallel for
3505  for(int i=0; i<scan.size(); ++i)
3506  {
3507  float * ptr = output.ptr<float>(0, i);
3508  if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
3509  {
3510  Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
3511  Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
3512 
3513  float result = v.dot(n);
3514  if(result < 0
3515  || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
3516  {
3517  //reverse normal
3518  ptr[nx] *= -1.0f;
3519  ptr[ny] *= -1.0f;
3520  ptr[nz] *= -1.0f;
3521  }
3522  }
3523  }
3524  if(scan.angleIncrement() > 0.0f)
3525  {
3526  return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
3527  }
3528  else
3529  {
3530  return LaserScan(output, scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
3531  }
3532  }
3533  return scan;
3534 }
3535 
3536 template<typename PointNormalT>
3537 void adjustNormalsToViewPointImpl(
3538  typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
3539  const Eigen::Vector3f & viewpoint,
3540  float groundNormalsUp)
3541 {
3542  #pragma omp parallel for
3543  for(int i=0; i<(int)cloud->size(); ++i)
3544  {
3545  pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
3546  if(pcl::isFinite(normal))
3547  {
3548  Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
3549  Eigen::Vector3f n(normal.x, normal.y, normal.z);
3550 
3551  float result = v.dot(n);
3552  if(result < 0
3553  || (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
3554  {
3555  //reverse normal
3556  cloud->points[i].normal_x *= -1.0f;
3557  cloud->points[i].normal_y *= -1.0f;
3558  cloud->points[i].normal_z *= -1.0f;
3559  }
3560  }
3561  }
3562 }
3563 
3564 void adjustNormalsToViewPoint(
3565  pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
3566  const Eigen::Vector3f & viewpoint,
3567  bool forceGroundNormalsUp)
3568 {
3569  adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
3570 }
3571 void adjustNormalsToViewPoint(
3572  pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
3573  const Eigen::Vector3f & viewpoint,
3574  float groundNormalsUp)
3575 {
3576  adjustNormalsToViewPointImpl<pcl::PointNormal>(cloud, viewpoint, groundNormalsUp);
3577 }
3578 
3579 void adjustNormalsToViewPoint(
3580  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
3581  const Eigen::Vector3f & viewpoint,
3582  bool forceGroundNormalsUp)
3583 {
3584  adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
3585 }
3586 void adjustNormalsToViewPoint(
3587  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
3588  const Eigen::Vector3f & viewpoint,
3589  float groundNormalsUp)
3590 {
3591  adjustNormalsToViewPointImpl<pcl::PointXYZRGBNormal>(cloud, viewpoint, groundNormalsUp);
3592 }
3593 
3594 void adjustNormalsToViewPoint(
3595  pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
3596  const Eigen::Vector3f & viewpoint,
3597  bool forceGroundNormalsUp)
3598 {
3599  adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
3600 }
3601 void adjustNormalsToViewPoint(
3602  pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
3603  const Eigen::Vector3f & viewpoint,
3604  float groundNormalsUp)
3605 {
3606  adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
3607 }
3608 
3609 
3610 template<typename PointT>
3611 void adjustNormalsToViewPointsImpl(
3612  const std::map<int, Transform> & poses,
3613  const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
3614  const std::vector<int> & rawCameraIndices,
3615  typename pcl::PointCloud<PointT>::Ptr & cloud,
3616  float groundNormalsUp)
3617 {
3618  if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
3619  {
3620  pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
3621  rawTree->setInputCloud (rawCloud);
3622 
3623  #pragma omp parallel for
3624  for(int i=0; i<(int)cloud->size(); ++i)
3625  {
3626  pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
3627  if(pcl::isFinite(normal))
3628  {
3629  std::vector<int> indices;
3630  std::vector<float> dist;
3631  rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
3632  UASSERT(indices.size() == 1);
3633  if(indices.size() && indices[0]>=0)
3634  {
3635  const Transform & p = poses.at(rawCameraIndices[indices[0]]);
3636  pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
3637  Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
3638 
3639  Eigen::Vector3f n(normal.x, normal.y, normal.z);
3640 
3641  float result = v.dot(n);
3642  if(result < 0 ||
3643  (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
3644  {
3645  //reverse normal
3646  cloud->points[i].normal_x *= -1.0f;
3647  cloud->points[i].normal_y *= -1.0f;
3648  cloud->points[i].normal_z *= -1.0f;
3649  }
3650  }
3651  else
3652  {
3653  UWARN("Not found camera viewpoint for point %d", i);
3654  }
3655  }
3656  }
3657  }
3658 }
3659 
3660 void adjustNormalsToViewPoints(
3661  const std::map<int, Transform> & poses,
3662  const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
3663  const std::vector<int> & rawCameraIndices,
3664  pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
3665  float groundNormalsUp)
3666 {
3667  adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
3668 }
3669 
3670 void adjustNormalsToViewPoints(
3671  const std::map<int, Transform> & poses,
3672  const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
3673  const std::vector<int> & rawCameraIndices,
3674  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
3675  float groundNormalsUp)
3676 {
3677  adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
3678 }
3679 
3680 void adjustNormalsToViewPoints(
3681  const std::map<int, Transform> & poses,
3682  const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
3683  const std::vector<int> & rawCameraIndices,
3684  pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
3685  float groundNormalsUp)
3686 {
3687  adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
3688 }
3689 
3690 void adjustNormalsToViewPoints(
3691  const std::map<int, Transform> & viewpoints,
3692  const LaserScan & rawScan,
3693  const std::vector<int> & viewpointIds,
3694  LaserScan & scan,
3695  float groundNormalsUp)
3696 {
3697  UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)viewpoints.size(), (int)rawScan.size(), (int)viewpointIds.size(), (int)scan.size());
3698  if(viewpoints.size() && rawScan.size() && rawScan.size() == (int)viewpointIds.size() && scan.size() && scan.hasNormals())
3699  {
3700  pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud = util3d::laserScanToPointCloud(rawScan);
3701  pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
3702  rawTree->setInputCloud (rawCloud);
3703  #pragma omp parallel for
3704  for(int i=0; i<scan.size(); ++i)
3705  {
3706  pcl::PointNormal point = util3d::laserScanToPointNormal(scan, i);
3707  pcl::PointXYZ normal(point.normal_x, point.normal_y, point.normal_z);
3708  if(pcl::isFinite(normal))
3709  {
3710  std::vector<int> indices;
3711  std::vector<float> dist;
3712  rawTree->nearestKSearch(pcl::PointXYZ(point.x, point.y, point.z), 1, indices, dist);
3713  if(indices.size() && indices[0]>=0)
3714  {
3715  UASSERT_MSG(indices[0]<(int)viewpointIds.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)viewpointIds.size()).c_str());
3716  UASSERT(uContains(viewpoints, viewpointIds[indices[0]]));
3717  Transform p = viewpoints.at(viewpointIds[indices[0]]);
3718  pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
3719  Eigen::Vector3f v = viewpoint.getVector3fMap() - point.getVector3fMap();
3720 
3721  Eigen::Vector3f n(normal.x, normal.y, normal.z);
3722 
3723  float result = v.dot(n);
3724  if(result < 0 ||
3725  (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && point.z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground))
3726  {
3727  //reverse normal
3728  scan.field(i, scan.getNormalsOffset()) *= -1.0f;
3729  scan.field(i, scan.getNormalsOffset()+1) *= -1.0f;
3730  scan.field(i, scan.getNormalsOffset()+2) *= -1.0f;
3731  }
3732  }
3733  else
3734  {
3735  UWARN("Not found camera viewpoint for point %d!?", i);
3736  }
3737  }
3738  }
3739  }
3740 }
3741 
3742 pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
3743 {
3744  pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
3745 #ifndef DISABLE_VTK
3746  pcl::MeshQuadricDecimationVTK mqd;
3747  mqd.setTargetReductionFactor(factor);
3748  mqd.setInputMesh(mesh);
3749  mqd.process (*output);
3750 #else
3751  UWARN("RTAB-Map is not built with VTK module so mesh decimation cannot be used!");
3752  *output = *mesh;
3753 #endif
3754  return output;
3755 }
3756 
3757 bool intersectRayTriangle(
3758  const Eigen::Vector3f & p,
3759  const Eigen::Vector3f & dir,
3760  const Eigen::Vector3f & v0,
3761  const Eigen::Vector3f & v1,
3762  const Eigen::Vector3f & v2,
3763  float & distance,
3764  Eigen::Vector3f & normal)
3765 {
3766  // get triangle edge cv::Vec3fs and plane normal
3767  const Eigen::Vector3f u = v1-v0;
3768  const Eigen::Vector3f v = v2-v0;
3769  normal = u.cross(v); // cross product
3770  if (normal == Eigen::Vector3f(0,0,0)) // triangle is degenerate
3771  return false; // do not deal with this case
3772 
3773  const float denomimator = normal.dot(dir);
3774  if (fabs(denomimator) < 10e-9) // ray is parallel to triangle plane
3775  return false;
3776 
3777  // get intersect of ray with triangle plane
3778  distance = normal.dot(v0 - p) / denomimator;
3779  if (distance < 0.0) // ray goes away from triangle
3780  return false;
3781 
3782  // is I inside T?
3783  float uu, uv, vv, wu, wv, D;
3784  uu = u.dot(u);
3785  uv = u.dot(v);
3786  vv = v.dot(v);
3787  const Eigen::Vector3f w = p + dir * distance - v0;
3788  wu = w.dot(u);
3789  wv = w.dot(v);
3790  D = uv * uv - uu * vv;
3791 
3792  // get and test parametric coords
3793  float s, t;
3794  s = (uv * wv - vv * wu) / D;
3795  if (s < 0.0 || s > 1.0) // I is outside T
3796  return false;
3797  t = (uv * wu - uu * wv) / D;
3798  if (t < 0.0 || (s + t) > 1.0) // I is outside T
3799  return false;
3800 
3801  return true; // I is in T
3802 }
3803 
3804 }
3805 
3806 }
uIsInteger
bool uIsInteger(const std::string &str, bool checkForSign=true)
Definition: UStl.h:632
rtabmap::Memory::getNodeInfo
bool getNodeInfo(int signatureId, Transform &odomPose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruth, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors, bool lookInDatabase=false) const
Definition: Memory.cpp:4021
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
rtabmap::util3d::filterNaNPointsFromMesh
std::vector< int > RTABMAP_EXP filterNaNPointsFromMesh(const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGB > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
Definition: util3d_surface.cpp:415
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:491
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
pcl::texture_mapping::Camera::height
double height
Definition: texture_mapping.h:78
pcl::OrganizedFastMesh::setViewpoint
void setViewpoint(const Eigen::Vector3f &viewpoint)
Set the viewpoint from where the input point cloud has been acquired.
Definition: organized_fast_mesh.h:178
cam
rtabmap::CameraThread * cam
Definition: tools/DataRecorder/main.cpp:58
rtabmap::util3d::laserScanToPointNormal
pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2316
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::DBDriver::getWeight
void getWeight(int signatureId, int &weight) const
Definition: DBDriver.cpp:849
epsilon
double epsilon
UTimer
Definition: UTimer.h:46
UDirectory::makeDir
static bool makeDir(const std::string &dirPath)
Definition: UDirectory.cpp:333
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
rtabmap::util3d::concatenateTextureMeshes
pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(const std::list< pcl::TextureMesh::Ptr > &meshes)
Definition: util3d_surface.cpp:1006
rtabmap::util3d::computeNormals2DImpl
pcl::PointCloud< pcl::Normal >::Ptr computeNormals2DImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int searchK, float searchRadius, const Eigen::Vector3f &viewPoint)
Definition: util3d_surface.cpp:2834
UFile::getName
std::string getName()
Definition: UFile.h:135
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::util3d::computeNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:2907
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
f
f
x
x
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:119
pcl::TextureMapping::textureMeshwithMultipleCameras2
bool textureMeshwithMultipleCameras2(pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras, const rtabmap::ProgressState *callback=0, std::vector< std::map< int, pcl::PointXY > > *vertexToPixels=0, bool distanceToCamPolicy=false)
Definition: texture_mapping.hpp:1053
pcl::OrganizedFastMesh::setTriangulationType
void setTriangulationType(TriangulationType type)
Set the triangulation type (see TriangulationType)
Definition: organized_fast_mesh.h:170
pcl::OrganizedFastMesh
Simple triangulation/surface reconstruction for organized point clouds. Neighboring points (pixels in...
Definition: organized_fast_mesh.h:65
pcl::texture_mapping::CameraVector
std::vector< Camera, Eigen::aligned_allocator< Camera > > CameraVector
Definition: texture_mapping.h:96
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::util3d::filterCloseVerticesFromMesh
std::vector< pcl::Vertices > RTABMAP_EXP filterCloseVerticesFromMesh(const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr cloud, const std::vector< pcl::Vertices > &polygons, float radius, float angle, bool keepLatestInRadius)
Definition: util3d_surface.cpp:458
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
rtabmap::maxDepthError
const float maxDepthError
Definition: CameraTango.cpp:43
rtflann::uchar
unsigned char uchar
Definition: matrix.h:41
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2268
rtabmap::util3d::computeNormalsImpl
pcl::PointCloud< pcl::Normal >::Ptr computeNormalsImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, int searchK, float searchRadius, const Eigen::Vector3f &viewPoint)
Definition: util3d_surface.cpp:2740
rtabmap::util3d::filterNotUsedVerticesFromMesh
std::vector< int > RTABMAP_EXP filterNotUsedVerticesFromMesh(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGBNormal > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
Definition: util3d_surface.cpp:335
pcl::TextureMapping
The texture mapping algorithm.
Definition: texture_mapping.h:105
data
data
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
pcl::OrganizedFastMesh::setAngleTolerance
void setAngleTolerance(float angle_tolerance)
Set the angle tolerance used for checking whether or not an edge is occluded. Standard values are 5de...
Definition: organized_fast_mesh.h:204
glm::e
GLM_FUNC_DECL genType e()
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
pcl::texture_mapping::Camera::focal_length
double focal_length
Definition: texture_mapping.h:73
tango_gl::vertices
static const float vertices[]
Definition: quad.cpp:39
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
dst
char * dst
Definition: lz4.h:354
pcl::texture_mapping::Camera::texture_file
std::string texture_file
Definition: texture_mapping.h:80
pcl::TextureMapping::setMinClusterSize
void setMinClusterSize(int size)
Definition: texture_mapping.h:193
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::util3d::computeFastOrganizedNormals
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float maxDepthChangeFactor=0.02f, float normalSmoothingSize=10.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:3036
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UMath.h
Basic mathematics functions.
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:137
UConversion.h
Some conversion functions.
rtabmap::util3d::computeNormals
LaserScan computeNormals(const LaserScan &laserScan, int searchK, float searchRadius)
Definition: util3d_surface.cpp:2665
rtabmap::util3d::assembleTextureMesh
pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(const cv::Mat &cloudMat, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, cv::Mat &textures, bool mergeTextures=false)
Definition: util3d_surface.cpp:1272
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
rtabmap::util3d::gcd
int gcd(int a, int b)
Definition: util3d_surface.cpp:1075
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
glm::intersectRayTriangle
GLM_FUNC_DECL bool intersectRayTriangle(genType const &orig, genType const &dir, genType const &vert0, genType const &vert1, genType const &vert2, genType &baryPosition)
organized_fast_mesh.h
rtabmap::util3d::computeFastOrganizedNormals2DImpl
pcl::PointCloud< pcl::Normal >::Ptr computeFastOrganizedNormals2DImpl(const typename pcl::PointCloud< PointT >::Ptr &cloud, int searchK, float searchRadius, const Eigen::Vector3f &viewPoint)
Definition: util3d_surface.cpp:2925
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:787
rtabmap::util3d::computeFastOrganizedNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:3019
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2197
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:130
rtabmap::LaserScan::field
float & field(unsigned int pointIndex, unsigned int channelOffset)
Definition: LaserScan.cpp:369
pcl::texture_mapping::Camera::roi
std::vector< double > roi
Definition: texture_mapping.h:81
rtabmap::util3d::mls
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP mls(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, float searchRadius=0.0f, int polygonialOrder=2, int upsamplingMethod=0, float upsamplingRadius=0.0f, float upsamplingStep=0.0f, int pointDensity=0, float dilationVoxelSize=1.0f, int dilationIterations=0)
Definition: util3d_surface.cpp:3387
rtabmap::util3d::sqr
double sqr(uchar v)
Definition: util3d_surface.cpp:1433
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
rtabmap::util3d::createTextureCameras
pcl::texture_mapping::CameraVector createTextureCameras(const std::map< int, Transform > &poses, const std::map< int, std::vector< CameraModel > > &cameraModels, const std::map< int, cv::Mat > &cameraDepths, const std::vector< float > &roiRatios)
Definition: util3d_surface.cpp:598
UASSERT
#define UASSERT(condition)
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
pcl::texture_mapping::Camera
Structure to store camera pose and focal length.
Definition: texture_mapping.h:68
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
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
texture_mapping.h
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_EXP clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:131
pcl::OrganizedFastMesh::setTrianglePixelSize
void setTrianglePixelSize(int triangle_size)
Set the edge length (in pixels) used for constructing the fixed mesh.
Definition: organized_fast_mesh.h:139
rtabmap::util3d::meshDecimation
pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr &mesh, float factor)
Definition: util3d_surface.cpp:3742
rtabmap::util3d::convertPolygonsToPCL
std::vector< pcl::Vertices > RTABMAP_EXP convertPolygonsToPCL(const std::vector< std::vector< RTABMAP_PCL_INDEX > > &polygons)
Definition: util3d_surface.cpp:1249
rtabmap::ProgressState::callback
virtual bool callback(const std::string &msg) const
Definition: ProgressState.h:39
rtabmap::util3d::appendMesh
void RTABMAP_EXP appendMesh(pcl::PointCloud< pcl::PointXYZRGBNormal > &cloudA, std::vector< pcl::Vertices > &polygonsA, const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloudB, const std::vector< pcl::Vertices > &polygonsB)
Definition: util3d_surface.cpp:283
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
pcl::texture_mapping::Camera::center_w
double center_w
Definition: texture_mapping.h:76
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
util3d_filtering.h
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:726
pcl::texture_mapping::Camera::focal_length_h
double focal_length_h
Definition: texture_mapping.h:75
glm::pi
GLM_FUNC_DECL genType pi()
pcl::texture_mapping::Camera::width
double width
Definition: texture_mapping.h:79
rtabmap::util3d::adjustNormalsToViewPoint
void adjustNormalsToViewPoint(pcl::PointCloud< pcl::PointXYZINormal >::Ptr &cloud, const Eigen::Vector3f &viewpoint, bool forceGroundNormalsUp)
Definition: util3d_surface.cpp:3594
rtabmap::DBDriver::getNodeData
void getNodeData(int signatureId, SensorData &data, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:675
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
tree
tree
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
pcl::TextureMapping::setMaxDepthError
void setMaxDepthError(float maxDepthError)
Definition: texture_mapping.h:181
pcl::texture_mapping::Camera::focal_length_w
double focal_length_w
Definition: texture_mapping.h:74
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:117
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
util3d_transforms.h
rtabmap::util3d::createMesh
pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(const pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr &cloudWithNormals, float gp3SearchRadius=0.025, float gp3Mu=2.5, int gp3MaximumNearestNeighbors=100, float gp3MaximumSurfaceAngle=M_PI/4, float gp3MinimumAngle=M_PI/18, float gp3MaximumAngle=2 *M_PI/3, bool gp3NormalConsistency=true)
Definition: util3d_surface.cpp:555
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:168
rtabmap::util3d::concatenateTextureMaterials
void RTABMAP_EXP concatenateTextureMaterials(pcl::TextureMesh &mesh, const cv::Size &imageSize, int textureSize, int maxTextures, float &scale, std::vector< bool > *materialsKept=0)
Definition: util3d_surface.cpp:1079
pcl::texture_mapping::Camera::center_h
double center_h
Definition: texture_mapping.h:77
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::Memory::getNodeData
SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
Definition: Memory.cpp:4071
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
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::util2d::exposureFusion
cv::Mat RTABMAP_EXP exposureFusion(const std::vector< cv::Mat > &images)
Definition: util2d.cpp:2027
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
pcl::texture_mapping::Camera::depth
cv::Mat depth
Definition: texture_mapping.h:82
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:120
rtabmap::util3d::adjustNormalsToViewPointImpl
void adjustNormalsToViewPointImpl(typename pcl::PointCloud< PointNormalT >::Ptr &cloud, const Eigen::Vector3f &viewpoint, float groundNormalsUp)
Definition: util3d_surface.cpp:3537
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::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::util3d::fixTextureMeshForVisualization
void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh &textureMesh)
Definition: util3d_surface.cpp:2195
rtabmap::util3d::filterInvalidPolygons
std::vector< pcl::Vertices > RTABMAP_EXP filterInvalidPolygons(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:531
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
UDEBUG
#define UDEBUG(...)
rtabmap::ProgressState::isCanceled
bool isCanceled() const
Definition: ProgressState.h:51
rtabmap::util3d::cleanTextureMesh
void RTABMAP_EXP cleanTextureMesh(pcl::TextureMesh &textureMesh, int minClusterSize)
Definition: util3d_surface.cpp:873
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2215
dist
dist
rtabmap::util3d::createPolygonIndexes
void RTABMAP_EXP createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons...
Definition: util3d_surface.cpp:94
UERROR
#define UERROR(...)
rtabmap::util3d::multiBandTexturing
bool 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, const DBDriver *dbDriver, int textureSize, const std::string &textureFormat, const std::map< int, std::map< int, cv::Vec4d > > &gains, const std::map< int, std::map< int, cv::Mat > > &blendingGains, const std::pair< float, float > &contrastValues, bool gainRGB)
Definition: util3d_surface.cpp:2234
UDirectory::removeDir
static bool removeDir(const std::string &dirPath)
Definition: UDirectory.cpp:344
kdtree
kdtree
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
pcl::TextureMapping::setMaxAngle
void setMaxAngle(float maxAngle)
Definition: texture_mapping.h:187
trace.model
model
Definition: trace.py:4
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:131
rtabmap::util3d::removeNaNNormalsFromPointCloud
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
Definition: util3d_filtering.cpp:1060
reference
reference
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::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:121
pcl::TextureMapping::setMaxDistance
void setMaxDistance(float maxDistance)
Definition: texture_mapping.h:175
tango_gl::kIndices
static const GLushort kIndices[]
Definition: video_overlay.cpp:28
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
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:320
rtabmap::util2d::brightnessAndContrastAuto
cv::Mat RTABMAP_EXP brightnessAndContrastAuto(const cv::Mat &src, const cv::Mat &mask, float clipLowHistPercent=0, float clipHighHistPercent=0, float *alphaOut=0, float *betaOut=0)
Automatic brightness and contrast optimization with optional histogram clipping.
Definition: util2d.cpp:1942
rtabmap::util3d::adjustNormalsToViewPointsImpl
void adjustNormalsToViewPointsImpl(const std::map< int, Transform > &poses, const pcl::PointCloud< pcl::PointXYZ >::Ptr &rawCloud, const std::vector< int > &rawCameraIndices, typename pcl::PointCloud< PointT >::Ptr &cloud, float groundNormalsUp)
Definition: util3d_surface.cpp:3611
rtabmap::util3d::concatenate
pcl::IndicesPtr RTABMAP_EXP concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenate a vector of indices to a single vector.
Definition: util3d.cpp:3219
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::SensorData::uncompressDataConst
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:623
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:179
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1920
pcl::texture_mapping::Camera::pose
Eigen::Affine3f pose
Definition: texture_mapping.h:72
rtabmap::util3d::computeNormalsComplexity
float RTABMAP_EXP computeNormalsComplexity(const LaserScan &scan, const Transform &t=Transform::getIdentity(), cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
Definition: util3d_surface.cpp:3084
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< RTABMAP_PCL_INDEX > > &polygons)
Definition: util3d_surface.cpp:1395
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::uncompressImage
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat &bytes)
Definition: Compression.cpp:130
util3d_surface.h
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:386
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58