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.h"
31 #include "rtabmap/core/util2d.h"
32 #include "rtabmap/core/Memory.h"
33 #include "rtabmap/core/DBDriver.h"
34 #include "rtabmap/core/Compression.h"
35 #include "rtabmap/utilite/ULogger.h"
36 #include "rtabmap/utilite/UDirectory.h"
37 #include "rtabmap/utilite/UConversion.h"
38 #include "rtabmap/utilite/UMath.h"
39 #include "rtabmap/utilite/UTimer.h"
40 #include <opencv2/core/core_c.h>
41 #include <opencv2/imgproc/types_c.h>
42 #include <pcl/search/kdtree.h>
43 #include <pcl/surface/gp3.h>
44 #include <pcl/features/normal_3d_omp.h>
45 #include <pcl/surface/mls.h>
46 #include <pcl18/surface/texture_mapping.h>
47 #include <pcl/features/integral_image_normal.h>
48 
49 #ifndef DISABLE_VTK
50 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
51 #endif
52 
53 #if PCL_VERSION_COMPARE(<, 1, 8, 0)
54 #include "pcl18/surface/organized_fast_mesh.h"
55 #else
56 #include <pcl/surface/organized_fast_mesh.h>
57 #include <pcl/surface/impl/marching_cubes.hpp>
58 #include <pcl/surface/impl/organized_fast_mesh.hpp>
59 #include <pcl/impl/instantiate.hpp>
60 #include <pcl/point_types.h>
61 
62 // Instantiations of specific point types
63 PCL_INSTANTIATE(OrganizedFastMesh, (pcl::PointXYZRGBNormal))
64 
65 #include <pcl/features/impl/normal_3d_omp.hpp>
66 #if PCL_VERSION_COMPARE(<=, 1, 8, 0)
67 #ifdef PCL_ONLY_CORE_POINT_TYPES
68 PCL_INSTANTIATE_PRODUCT(NormalEstimationOMP, ((pcl::PointXYZRGB))((pcl::Normal)))
69 #endif
70 #endif
71 #endif
72 
73 namespace rtabmap
74 {
75 
76 namespace util3d
77 {
78 
79 void createPolygonIndexes(
80  const std::vector<pcl::Vertices> & polygons,
81  int cloudSize,
82  std::vector<std::set<int> > & neighbors,
83  std::vector<std::set<int> > & vertexToPolygons)
84 {
85  vertexToPolygons = std::vector<std::set<int> >(cloudSize);
86  neighbors = std::vector<std::set<int> >(polygons.size());
87 
88  for(unsigned int i=0; i<polygons.size(); ++i)
89  {
90  std::set<int> vertices(polygons[i].vertices.begin(), polygons[i].vertices.end());
91 
92  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
93  {
94  int v = polygons[i].vertices.at(j);
95  for(std::set<int>::iterator iter=vertexToPolygons[v].begin(); iter!=vertexToPolygons[v].end(); ++iter)
96  {
97  int numSharedVertices = 0;
98  for(unsigned int k=0; k<polygons.at(*iter).vertices.size() && numSharedVertices<2; ++k)
99  {
100  if(vertices.find(polygons.at(*iter).vertices.at(k)) != vertices.end())
101  {
102  ++numSharedVertices;
103  }
104  }
105  if(numSharedVertices >= 2)
106  {
107  neighbors[*iter].insert(i);
108  neighbors[i].insert(*iter);
109  }
110  }
111  vertexToPolygons[v].insert(i);
112  }
113  }
114 }
115 
116 std::list<std::list<int> > clusterPolygons(
117  const std::vector<std::set<int> > & neighborPolygons,
118  int minClusterSize)
119 {
120  std::set<int> polygonsChecked;
121 
122  std::list<std::list<int> > clusters;
123 
124  for(unsigned int i=0; i<neighborPolygons.size(); ++i)
125  {
126  if(polygonsChecked.find(i) == polygonsChecked.end())
127  {
128  std::list<int> currentCluster;
129  currentCluster.push_back(i);
130  polygonsChecked.insert(i);
131 
132  for(std::list<int>::iterator iter=currentCluster.begin(); iter!=currentCluster.end(); ++iter)
133  {
134  // get neighbor polygons
135  std::set<int> neighbors = neighborPolygons[*iter];
136  for(std::set<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
137  {
138  if(polygonsChecked.insert(*jter).second)
139  {
140  currentCluster.push_back(*jter);
141  }
142  }
143  }
144  if((int)currentCluster.size() > minClusterSize)
145  {
146  clusters.push_back(currentCluster);
147  }
148  }
149  }
150  return clusters;
151 }
152 
153 std::vector<pcl::Vertices> organizedFastMesh(
154  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
155  double angleTolerance,
156  bool quad,
157  int trianglePixelSize,
158  const Eigen::Vector3f & viewpoint)
159 {
160  UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
161  UASSERT(cloud->is_dense == false);
162  UASSERT(cloud->width > 1 && cloud->height > 1);
163 
164  pcl::OrganizedFastMesh<pcl::PointXYZ> ofm;
165  ofm.setTrianglePixelSize (trianglePixelSize);
166  ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZ>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_RIGHT_CUT);
167  ofm.setInputCloud (cloud);
168  ofm.setAngleTolerance(angleTolerance);
169  ofm.setViewpoint(viewpoint);
170 
171  std::vector<pcl::Vertices> vertices;
172  ofm.reconstruct (vertices);
173 
174  if(quad)
175  {
176  //flip all polygons (right handed)
177  std::vector<pcl::Vertices> output(vertices.size());
178  for(unsigned int i=0; i<vertices.size(); ++i)
179  {
180  output[i].vertices.resize(4);
181  output[i].vertices[0] = vertices[i].vertices[0];
182  output[i].vertices[3] = vertices[i].vertices[1];
183  output[i].vertices[2] = vertices[i].vertices[2];
184  output[i].vertices[1] = vertices[i].vertices[3];
185  }
186  return output;
187  }
188 
189  return vertices;
190 }
191 std::vector<pcl::Vertices> organizedFastMesh(
192  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
193  double angleTolerance,
194  bool quad,
195  int trianglePixelSize,
196  const Eigen::Vector3f & viewpoint)
197 {
198  UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
199  UASSERT(cloud->is_dense == false);
200  UASSERT(cloud->width > 1 && cloud->height > 1);
201 
202  pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
203  ofm.setTrianglePixelSize (trianglePixelSize);
204  ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
205  ofm.setInputCloud (cloud);
206  ofm.setAngleTolerance(angleTolerance);
207  ofm.setViewpoint(viewpoint);
208 
209  std::vector<pcl::Vertices> vertices;
210  ofm.reconstruct (vertices);
211 
212  if(quad)
213  {
214  //flip all polygons (right handed)
215  std::vector<pcl::Vertices> output(vertices.size());
216  for(unsigned int i=0; i<vertices.size(); ++i)
217  {
218  output[i].vertices.resize(4);
219  output[i].vertices[0] = vertices[i].vertices[0];
220  output[i].vertices[3] = vertices[i].vertices[1];
221  output[i].vertices[2] = vertices[i].vertices[2];
222  output[i].vertices[1] = vertices[i].vertices[3];
223  }
224  return output;
225  }
226 
227  return vertices;
228 }
229 std::vector<pcl::Vertices> organizedFastMesh(
230  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
231  double angleTolerance,
232  bool quad,
233  int trianglePixelSize,
234  const Eigen::Vector3f & viewpoint)
235 {
236  UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
237  UASSERT(cloud->is_dense == false);
238  UASSERT(cloud->width > 1 && cloud->height > 1);
239 
240  pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal> ofm;
241  ofm.setTrianglePixelSize (trianglePixelSize);
242  ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
243  ofm.setInputCloud (cloud);
244  ofm.setAngleTolerance(angleTolerance);
245  ofm.setViewpoint(viewpoint);
246 
247  std::vector<pcl::Vertices> vertices;
248  ofm.reconstruct (vertices);
249 
250  if(quad)
251  {
252  //flip all polygons (right handed)
253  std::vector<pcl::Vertices> output(vertices.size());
254  for(unsigned int i=0; i<vertices.size(); ++i)
255  {
256  output[i].vertices.resize(4);
257  output[i].vertices[0] = vertices[i].vertices[0];
258  output[i].vertices[3] = vertices[i].vertices[1];
259  output[i].vertices[2] = vertices[i].vertices[2];
260  output[i].vertices[1] = vertices[i].vertices[3];
261  }
262  return output;
263  }
264 
265  return vertices;
266 }
267 
268 void appendMesh(
269  pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
270  std::vector<pcl::Vertices> & polygonsA,
271  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
272  const std::vector<pcl::Vertices> & polygonsB)
273 {
274  UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
275  UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
276 
277  int sizeA = (int)cloudA.size();
278  cloudA += cloudB;
279 
280  int sizePolygonsA = (int)polygonsA.size();
281  polygonsA.resize(sizePolygonsA+polygonsB.size());
282 
283  for(unsigned int i=0; i<polygonsB.size(); ++i)
284  {
285  pcl::Vertices vertices = polygonsB[i];
286  for(unsigned int j=0; j<vertices.vertices.size(); ++j)
287  {
288  vertices.vertices[j] += sizeA;
289  }
290  polygonsA[i+sizePolygonsA] = vertices;
291  }
292 }
293 
294 void appendMesh(
295  pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
296  std::vector<pcl::Vertices> & polygonsA,
297  const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
298  const std::vector<pcl::Vertices> & polygonsB)
299 {
300  UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
301  UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
302 
303  int sizeA = (int)cloudA.size();
304  cloudA += cloudB;
305 
306  int sizePolygonsA = (int)polygonsA.size();
307  polygonsA.resize(sizePolygonsA+polygonsB.size());
308 
309  for(unsigned int i=0; i<polygonsB.size(); ++i)
310  {
311  pcl::Vertices vertices = polygonsB[i];
312  for(unsigned int j=0; j<vertices.vertices.size(); ++j)
313  {
314  vertices.vertices[j] += sizeA;
315  }
316  polygonsA[i+sizePolygonsA] = vertices;
317  }
318 }
319 
320 std::vector<int> filterNotUsedVerticesFromMesh(
321  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
322  const std::vector<pcl::Vertices> & polygons,
323  pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
324  std::vector<pcl::Vertices> & outputPolygons)
325 {
326  UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
327  std::map<int, int> addedVertices; //<oldIndex, newIndex>
328  std::vector<int> output; //<oldIndex>
329  output.resize(cloud.size());
330  outputCloud.resize(cloud.size());
331  outputCloud.is_dense = true;
332  outputPolygons.resize(polygons.size());
333  int oi = 0;
334  for(unsigned int i=0; i<polygons.size(); ++i)
335  {
336  pcl::Vertices & v = outputPolygons[i];
337  v.vertices.resize(polygons[i].vertices.size());
338  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
339  {
340  std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
341  if(iter == addedVertices.end())
342  {
343  outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
344  addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
345  output[oi] = polygons[i].vertices[j];
346  v.vertices[j] = oi++;
347  }
348  else
349  {
350  v.vertices[j] = iter->second;
351  }
352  }
353  }
354  outputCloud.resize(oi);
355  output.resize(oi);
356 
357  return output;
358 }
359 
360 std::vector<int> filterNotUsedVerticesFromMesh(
361  const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
362  const std::vector<pcl::Vertices> & polygons,
363  pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
364  std::vector<pcl::Vertices> & outputPolygons)
365 {
366  UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
367  std::map<int, int> addedVertices; //<oldIndex, newIndex>
368  std::vector<int> output; //<oldIndex>
369  output.resize(cloud.size());
370  outputCloud.resize(cloud.size());
371  outputCloud.is_dense = true;
372  outputPolygons.resize(polygons.size());
373  int oi = 0;
374  for(unsigned int i=0; i<polygons.size(); ++i)
375  {
376  pcl::Vertices & v = outputPolygons[i];
377  v.vertices.resize(polygons[i].vertices.size());
378  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
379  {
380  std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
381  if(iter == addedVertices.end())
382  {
383  outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
384  addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
385  output[oi] = polygons[i].vertices[j];
386  v.vertices[j] = oi++;
387  }
388  else
389  {
390  v.vertices[j] = iter->second;
391  }
392  }
393  }
394  outputCloud.resize(oi);
395  output.resize(oi);
396 
397  return output;
398 }
399 
400 std::vector<int> filterNaNPointsFromMesh(
401  const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
402  const std::vector<pcl::Vertices> & polygons,
403  pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
404  std::vector<pcl::Vertices> & outputPolygons)
405 {
406  UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
407  std::map<int, int> addedVertices; //<oldIndex, newIndex>
408  std::vector<int> output; //<oldIndex>
409  output.resize(cloud.size());
410  outputCloud.resize(cloud.size());
411  outputCloud.is_dense = true;
412  std::vector<int> organizedToDense(cloud.size(), -1);
413 
414  int oi = 0;
415  for(unsigned int i=0; i<cloud.size(); ++i)
416  {
417  if(pcl::isFinite(cloud.at(i)))
418  {
419  outputCloud.at(oi) = cloud.at(i);
420  output[oi] = i;
421  organizedToDense[i] = oi;
422  ++oi;
423  }
424  }
425  outputCloud.resize(oi);
426  output.resize(oi);
427 
428  // remap polygons to dense cloud
429  outputPolygons = polygons;
430  for(unsigned int i=0; i<outputPolygons.size(); ++i)
431  {
432  pcl::Vertices & v = outputPolygons[i];
433  for(unsigned int j=0; j<v.vertices.size(); ++j)
434  {
435  UASSERT(organizedToDense[v.vertices[j]] >= 0);
436  v.vertices[j] = organizedToDense[v.vertices[j]];
437  }
438  }
439 
440  return output;
441 }
442 
443 std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
444  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
445  const std::vector<pcl::Vertices> & polygons,
446  float radius,
447  float angle, // FIXME angle not used
448  bool keepLatestInRadius)
449 {
450  UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
451  (int)cloud->size(), (int)polygons.size(), radius, angle, keepLatestInRadius?1:0);
452  std::vector<pcl::Vertices> outputPolygons;
453  pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
454  kdtree->setInputCloud(cloud);
455 
456  std::map<int, int> verticesDone;
457  outputPolygons = polygons;
458  for(unsigned int i=0; i<outputPolygons.size(); ++i)
459  {
460  pcl::Vertices & polygon = outputPolygons[i];
461  for(unsigned int j=0; j<polygon.vertices.size(); ++j)
462  {
463  std::map<int, int>::iterator iter = verticesDone.find(polygon.vertices[j]);
464  if(iter != verticesDone.end())
465  {
466  polygon.vertices[j] = iter->second;
467  }
468  else
469  {
470  std::vector<int> kIndices;
471  std::vector<float> kDistances;
472  kdtree->radiusSearch(polygon.vertices[j], radius, kIndices, kDistances);
473  if(kIndices.size())
474  {
475  int reference = -1;
476  for(unsigned int z=0; z<kIndices.size(); ++z)
477  {
478  if(reference == -1)
479  {
480  reference = kIndices[z];
481  }
482  else if(keepLatestInRadius)
483  {
484  if(kIndices[z] < reference)
485  {
486  reference = kIndices[z];
487  }
488  }
489  else
490  {
491  if(kIndices[z] > reference)
492  {
493  reference = kIndices[z];
494  }
495  }
496  }
497  if(reference >= 0)
498  {
499  for(unsigned int z=0; z<kIndices.size(); ++z)
500  {
501  verticesDone.insert(std::make_pair(kIndices[j], reference));
502  }
503  polygon.vertices[j] = reference;
504  }
505  }
506  else
507  {
508  verticesDone.insert(std::make_pair(polygon.vertices[j], polygon.vertices[j]));
509  }
510  }
511  }
512  }
513  return outputPolygons;
514 }
515 
516 std::vector<pcl::Vertices> filterInvalidPolygons(const std::vector<pcl::Vertices> & polygons)
517 {
518  std::vector<pcl::Vertices> output(polygons.size());
519  int oi=0;
520  for(unsigned int i=0; i<polygons.size(); ++i)
521  {
522  bool valid = true;
523  for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
524  {
525  if(polygons[i].vertices[j] == polygons[i].vertices[(j+1)%polygons[i].vertices.size()])
526  {
527  valid = false;
528  break;
529  }
530  }
531  if(valid)
532  {
533  output[oi++] = polygons[i];
534  }
535  }
536  output.resize(oi);
537  return output;
538 }
539 
540 pcl::PolygonMesh::Ptr createMesh(
541  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
542  float gp3SearchRadius,
543  float gp3Mu,
544  int gp3MaximumNearestNeighbors,
545  float gp3MaximumSurfaceAngle,
546  float gp3MinimumAngle,
547  float gp3MaximumAngle,
548  bool gp3NormalConsistency)
549 {
550  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
551 
552  // Create search tree*
553  pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
554  tree2->setInputCloud (cloudWithNormalsNoNaN);
555 
556  // Initialize objects
557  pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
558  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
559 
560  // Set the maximum distance between connected points (maximum edge length)
561  gp3.setSearchRadius (gp3SearchRadius);
562 
563  // Set typical values for the parameters
564  gp3.setMu (gp3Mu);
565  gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
566  gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle); // 45 degrees
567  gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
568  gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
569  gp3.setNormalConsistency(gp3NormalConsistency);
570  gp3.setConsistentVertexOrdering(gp3NormalConsistency);
571 
572  // Get result
573  gp3.setInputCloud (cloudWithNormalsNoNaN);
574  gp3.setSearchMethod (tree2);
575  gp3.reconstruct (*mesh);
576 
577  //UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size());
578  //mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons);
579 
580  return mesh;
581 }
582 
583 pcl::texture_mapping::CameraVector createTextureCameras(
584  const std::map<int, Transform> & poses,
585  const std::map<int, std::vector<CameraModel> > & cameraModels,
586  const std::map<int, cv::Mat> & cameraDepths,
587  const std::vector<float> & roiRatios)
588 {
589  UASSERT_MSG(poses.size() == cameraModels.size(), uFormat("%d vs %d", (int)poses.size(), (int)cameraModels.size()).c_str());
590  UASSERT(roiRatios.empty() || roiRatios.size() == 4);
591  pcl::texture_mapping::CameraVector cameras;
592  std::map<int, Transform>::const_iterator poseIter=poses.begin();
593  std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.begin();
594  for(; poseIter!=poses.end(); ++poseIter, ++modelIter)
595  {
596  UASSERT(poseIter->first == modelIter->first);
597 
598  std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);
599 
600  // for each sub camera
601  for(unsigned int i=0; i<modelIter->second.size(); ++i)
602  {
603  pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
604  // should be in camera frame
605  UASSERT(!modelIter->second[i].localTransform().isNull() && !poseIter->second.isNull());
606  Transform t = poseIter->second*modelIter->second[i].localTransform();
607 
608  cam.pose = t.toEigen3f();
609 
610  if(modelIter->second[i].imageHeight() <=0 || modelIter->second[i].imageWidth() <=0)
611  {
612  UERROR("Should have camera models with width/height set to create texture cameras!");
613  return pcl::texture_mapping::CameraVector();
614  }
615 
616  UASSERT(modelIter->second[i].fx()>0 && modelIter->second[i].imageHeight()>0 && modelIter->second[i].imageWidth()>0);
617  cam.focal_length=modelIter->second[i].fx();
618  cam.height=modelIter->second[i].imageHeight();
619  cam.width=modelIter->second[i].imageWidth();
620  if(modelIter->second.size() == 1)
621  {
622  cam.texture_file = uFormat("%d", poseIter->first); // camera index
623  }
624  else
625  {
626  cam.texture_file = uFormat("%d_%d", poseIter->first, (int)i); // camera index, sub camera model index
627  }
628  if(!roiRatios.empty())
629  {
630  cam.roi.resize(4);
631  cam.roi[0] = cam.width * roiRatios[0]; // left -> x
632  cam.roi[1] = cam.height * roiRatios[2]; // top -> y
633  cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
634  cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
635  }
636 
637  if(depthIter != cameraDepths.end() && !depthIter->second.empty())
638  {
639  UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
640  UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
641  int subWidth = depthIter->second.cols/(modelIter->second.size());
642  cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
643  }
644 
645  UDEBUG("%f", cam.focal_length);
646  UDEBUG("%f", cam.height);
647  UDEBUG("%f", cam.width);
648  UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
649 
650  cameras.push_back(cam);
651  }
652  }
653  return cameras;
654 }
655 
656 pcl::TextureMesh::Ptr createTextureMesh(
657  const pcl::PolygonMesh::Ptr & mesh,
658  const std::map<int, Transform> & poses,
659  const std::map<int, CameraModel> & cameraModels,
660  const std::map<int, cv::Mat> & cameraDepths,
661  float maxDistance,
662  float maxDepthError,
663  float maxAngle,
664  int minClusterSize,
665  const std::vector<float> & roiRatios,
666  const ProgressState * state,
667  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
668 {
669  std::map<int, std::vector<CameraModel> > cameraSubModels;
670  for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
671  {
672  std::vector<CameraModel> models;
673  models.push_back(iter->second);
674  cameraSubModels.insert(std::make_pair(iter->first, models));
675  }
676 
677  return createTextureMesh(
678  mesh,
679  poses,
680  cameraSubModels,
681  cameraDepths,
682  maxDistance,
683  maxDepthError,
684  maxAngle,
685  minClusterSize,
686  roiRatios,
687  state,
688  vertexToPixels);
689 }
690 
691 pcl::TextureMesh::Ptr createTextureMesh(
692  const pcl::PolygonMesh::Ptr & mesh,
693  const std::map<int, Transform> & poses,
694  const std::map<int, std::vector<CameraModel> > & cameraModels,
695  const std::map<int, cv::Mat> & cameraDepths,
696  float maxDistance,
697  float maxDepthError,
698  float maxAngle,
699  int minClusterSize,
700  const std::vector<float> & roiRatios,
701  const ProgressState * state,
702  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
703 {
704  UASSERT(mesh->polygons.size());
705  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
706  textureMesh->cloud = mesh->cloud;
707  textureMesh->tex_polygons.push_back(mesh->polygons);
708 
709  // Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
710  // Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
711 
712  // Create the texturemesh object that will contain our UV-mapped mesh
713 
714  // create cameras
715  pcl::texture_mapping::CameraVector cameras = createTextureCameras(
716  poses,
717  cameraModels,
718  cameraDepths,
719  roiRatios);
720 
721  // Create materials for each texture (and one extra for occluded faces)
722  textureMesh->tex_materials.resize (cameras.size () + 1);
723  for(unsigned int i = 0 ; i <= cameras.size() ; ++i)
724  {
725  pcl::TexMaterial mesh_material;
726  mesh_material.tex_Ka.r = 0.2f;
727  mesh_material.tex_Ka.g = 0.2f;
728  mesh_material.tex_Ka.b = 0.2f;
729 
730  mesh_material.tex_Kd.r = 0.8f;
731  mesh_material.tex_Kd.g = 0.8f;
732  mesh_material.tex_Kd.b = 0.8f;
733 
734  mesh_material.tex_Ks.r = 1.0f;
735  mesh_material.tex_Ks.g = 1.0f;
736  mesh_material.tex_Ks.b = 1.0f;
737 
738  mesh_material.tex_d = 1.0f;
739  mesh_material.tex_Ns = 75.0f;
740  mesh_material.tex_illum = 2;
741 
742  std::stringstream tex_name;
743  tex_name << "material_" << i;
744  tex_name >> mesh_material.tex_name;
745 
746  if(i < cameras.size ())
747  {
748  mesh_material.tex_file = cameras[i].texture_file;
749  }
750  else
751  {
752  mesh_material.tex_file = "occluded";
753  }
754 
755  textureMesh->tex_materials[i] = mesh_material;
756  }
757 
758  // Texture by projection
759  pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
760  tm.setMaxDistance(maxDistance);
761  tm.setMaxAngle(maxAngle);
762  if(maxDepthError > 0.0f)
763  {
764  tm.setMaxDepthError(maxDepthError);
765  }
766  tm.setMinClusterSize(minClusterSize);
767  if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels))
768  {
769  // compute normals for the mesh if not already here
770  bool hasNormals = false;
771  bool hasColors = false;
772  for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
773  {
774  if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
775  {
776  hasNormals = true;
777  }
778  else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
779  {
780  hasColors = true;
781  }
782  }
783  if(!hasNormals)
784  {
785  // use polygons
786  if(hasColors)
787  {
788  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
789  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
790 
791  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
792  {
793  pcl::Vertices & v = mesh->polygons[i];
794  UASSERT(v.vertices.size()>2);
795  Eigen::Vector3f v0(
796  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
797  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
798  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
799  int last = v.vertices.size()-1;
800  Eigen::Vector3f v1(
801  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
802  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
803  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
804  Eigen::Vector3f normal = v0.cross(v1);
805  normal.normalize();
806  // flat normal (per face)
807  for(unsigned int j=0; j<v.vertices.size(); ++j)
808  {
809  cloud->at(v.vertices[j]).normal_x = normal[0];
810  cloud->at(v.vertices[j]).normal_y = normal[1];
811  cloud->at(v.vertices[j]).normal_z = normal[2];
812  }
813  }
814  pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
815  }
816  else
817  {
818  pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
819  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
820 
821  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
822  {
823  pcl::Vertices & v = mesh->polygons[i];
824  UASSERT(v.vertices.size()>2);
825  Eigen::Vector3f v0(
826  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
827  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
828  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
829  int last = v.vertices.size()-1;
830  Eigen::Vector3f v1(
831  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
832  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
833  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
834  Eigen::Vector3f normal = v0.cross(v1);
835  normal.normalize();
836  // flat normal (per face)
837  for(unsigned int j=0; j<v.vertices.size(); ++j)
838  {
839  cloud->at(v.vertices[j]).normal_x = normal[0];
840  cloud->at(v.vertices[j]).normal_y = normal[1];
841  cloud->at(v.vertices[j]).normal_z = normal[2];
842  }
843  }
844  pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
845  }
846  }
847  }
848  return textureMesh;
849 }
850 
851 void cleanTextureMesh(
852  pcl::TextureMesh & textureMesh,
853  int minClusterSize)
854 {
855  UDEBUG("minClusterSize=%d", minClusterSize);
856  // Remove occluded polygons (polygons with no texture)
857  if(textureMesh.tex_coordinates.size())
858  {
859  // assume last texture is the occluded texture
860  textureMesh.tex_coordinates.pop_back();
861  textureMesh.tex_polygons.pop_back();
862  textureMesh.tex_materials.pop_back();
863 
864  if(minClusterSize!=0)
865  {
866  // concatenate all polygons
867  unsigned int totalSize = 0;
868  for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
869  {
870  totalSize+=textureMesh.tex_polygons[t].size();
871  }
872  std::vector<pcl::Vertices> allPolygons(totalSize);
873  int oi=0;
874  for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
875  {
876  for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
877  {
878  allPolygons[oi++] = textureMesh.tex_polygons[t][i];
879  }
880  }
881 
882  // filter polygons
883  std::vector<std::set<int> > neighbors;
884  std::vector<std::set<int> > vertexToPolygons;
885  util3d::createPolygonIndexes(allPolygons,
886  (int)textureMesh.cloud.data.size()/textureMesh.cloud.point_step,
887  neighbors,
888  vertexToPolygons);
889 
890  std::list<std::list<int> > clusters = util3d::clusterPolygons(
891  neighbors,
892  minClusterSize<0?0:minClusterSize);
893 
894  std::set<int> validPolygons;
895  if(minClusterSize < 0)
896  {
897  // only keep the biggest cluster
898  std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
899  unsigned int biggestClusterSize = 0;
900  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
901  {
902  if(iter->size() > biggestClusterSize)
903  {
904  biggestClusterIndex = iter;
905  biggestClusterSize = iter->size();
906  }
907  }
908  if(biggestClusterIndex != clusters.end())
909  {
910  for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
911  {
912  validPolygons.insert(*jter);
913  }
914  }
915  }
916  else
917  {
918  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
919  {
920  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
921  {
922  validPolygons.insert(*jter);
923  }
924  }
925  }
926 
927  if(validPolygons.size() == 0)
928  {
929  UWARN("All %d polygons filtered after polygon cluster filtering. Cluster minimum size is %d.",totalSize, minClusterSize);
930  }
931 
932  // for each texture
933  unsigned int allPolygonsIndex = 0;
934  for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
935  {
936  std::vector<pcl::Vertices> filteredPolygons(textureMesh.tex_polygons[t].size());
937 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
938  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > filteredCoordinates(textureMesh.tex_coordinates[t].size());
939 #else
940  std::vector<Eigen::Vector2f> filteredCoordinates(textureMesh.tex_coordinates[t].size());
941 #endif
942 
943  if(textureMesh.tex_polygons[t].size())
944  {
945  UASSERT_MSG(allPolygonsIndex < allPolygons.size(), uFormat("%d vs %d", (int)allPolygonsIndex, (int)allPolygons.size()).c_str());
946 
947  // make index polygon to coordinate
948  std::vector<unsigned int> polygonToCoord(textureMesh.tex_polygons[t].size());
949  unsigned int totalCoord = 0;
950  for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
951  {
952  polygonToCoord[i] = totalCoord;
953  totalCoord+=textureMesh.tex_polygons[t][i].vertices.size();
954  }
955  UASSERT_MSG(totalCoord == textureMesh.tex_coordinates[t].size(), uFormat("%d vs %d", totalCoord, (int)textureMesh.tex_coordinates[t].size()).c_str());
956 
957  int oi=0;
958  int ci=0;
959  for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
960  {
961  if(validPolygons.find(allPolygonsIndex) != validPolygons.end())
962  {
963  filteredPolygons[oi] = textureMesh.tex_polygons[t].at(i);
964  for(unsigned int j=0; j<filteredPolygons[oi].vertices.size(); ++j)
965  {
966  UASSERT(polygonToCoord[i] < textureMesh.tex_coordinates[t].size());
967  filteredCoordinates[ci] = textureMesh.tex_coordinates[t][polygonToCoord[i]+j];
968  ++ci;
969  }
970  ++oi;
971  }
972  ++allPolygonsIndex;
973  }
974  filteredPolygons.resize(oi);
975  filteredCoordinates.resize(ci);
976  textureMesh.tex_polygons[t] = filteredPolygons;
977  textureMesh.tex_coordinates[t] = filteredCoordinates;
978  }
979  }
980  }
981  }
982 }
983 
984 pcl::TextureMesh::Ptr concatenateTextureMeshes(const std::list<pcl::TextureMesh::Ptr> & meshes)
985 {
986  pcl::TextureMesh::Ptr output(new pcl::TextureMesh);
987  std::map<std::string, int> addedMaterials; //<file, index>
988  for(std::list<pcl::TextureMesh::Ptr>::const_iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
989  {
990  if((*iter)->cloud.point_step &&
991  (*iter)->cloud.data.size()/(*iter)->cloud.point_step &&
992  (*iter)->tex_polygons.size() &&
993  (*iter)->tex_coordinates.size())
994  {
995  // append point cloud
996  int polygonStep = output->cloud.height * output->cloud.width;
997  pcl::PCLPointCloud2 tmp;
998  pcl::concatenatePointCloud(output->cloud, iter->get()->cloud, tmp);
999  output->cloud = tmp;
1000 
1001  UASSERT((*iter)->tex_polygons.size() == (*iter)->tex_coordinates.size() &&
1002  (*iter)->tex_polygons.size() == (*iter)->tex_materials.size());
1003 
1004  int materialCount = (*iter)->tex_polygons.size();
1005  for(int i=0; i<materialCount; ++i)
1006  {
1007  std::map<std::string, int>::iterator jter = addedMaterials.find((*iter)->tex_materials[i].tex_file);
1008  int index;
1009  if(jter != addedMaterials.end())
1010  {
1011  index = jter->second;
1012  }
1013  else
1014  {
1015  addedMaterials.insert(std::make_pair((*iter)->tex_materials[i].tex_file, output->tex_materials.size()));
1016  index = output->tex_materials.size();
1017  output->tex_materials.push_back((*iter)->tex_materials[i]);
1018  output->tex_materials.back().tex_name = uFormat("material_%d", index);
1019  output->tex_polygons.resize(output->tex_polygons.size() + 1);
1020  output->tex_coordinates.resize(output->tex_coordinates.size() + 1);
1021  }
1022 
1023  // update and append polygon indices
1024  int oi = output->tex_polygons[index].size();
1025  output->tex_polygons[index].resize(output->tex_polygons[index].size() + (*iter)->tex_polygons[i].size());
1026  for(unsigned int j=0; j<(*iter)->tex_polygons[i].size(); ++j)
1027  {
1028  pcl::Vertices polygon = (*iter)->tex_polygons[i][j];
1029  for(unsigned int k=0; k<polygon.vertices.size(); ++k)
1030  {
1031  polygon.vertices[k] += polygonStep;
1032  }
1033  output->tex_polygons[index][oi+j] = polygon;
1034  }
1035 
1036  // append uv coordinates
1037  oi = output->tex_coordinates[index].size();
1038  output->tex_coordinates[index].resize(output->tex_coordinates[index].size() + (*iter)->tex_coordinates[i].size());
1039  for(unsigned int j=0; j<(*iter)->tex_coordinates[i].size(); ++j)
1040  {
1041  output->tex_coordinates[index][oi+j] = (*iter)->tex_coordinates[i][j];
1042  }
1043  }
1044  }
1045  }
1046  return output;
1047 }
1048 
1049 int gcd(int a, int b) {
1050  return b == 0 ? a : gcd(b, a % b);
1051 }
1052 
1053 void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept)
1054 {
1055  UASSERT(textureSize>0 && imageSize.width>0 && imageSize.height>0);
1056  if(maxTextures < 1)
1057  {
1058  maxTextures = 1;
1059  }
1060  int materials = 0;
1061  for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
1062  {
1063  if(mesh.tex_polygons.size())
1064  {
1065  ++materials;
1066  }
1067  }
1068  if(materials)
1069  {
1070  int w = imageSize.width; // 640
1071  int h = imageSize.height; // 480
1072  int g = gcd(w,h); // 160
1073  int a = w/g; // 4=640/160
1074  int b = h/g; // 3=480/160
1075  UDEBUG("w=%d h=%d g=%d a=%d b=%d", w, h, g, a, b);
1076  int colCount = 0;
1077  int rowCount = 0;
1078  float factor = 0.1f;
1079  float epsilon = 0.001f;
1080  scale = 1.0f;
1081  while((colCount*rowCount)*maxTextures < materials || (factor == 0.1f || scale > 1.0f))
1082  {
1083  // first run try scale = 1 (no scaling)
1084  if(factor!=0.1f)
1085  {
1086  scale = float(textureSize)/float(w*b*factor);
1087  }
1088  colCount = float(textureSize)/(scale*float(w));
1089  rowCount = float(textureSize)/(scale*float(h));
1090  factor+=epsilon; // search the maximum perfect fit
1091  }
1092  int outputTextures = (materials / (colCount*rowCount)) + (materials % (colCount*rowCount) > 0?1:0);
1093  UDEBUG("materials=%d col=%d row=%d output textures=%d factor=%f scale=%f", materials, colCount, rowCount, outputTextures, factor-epsilon, scale);
1094 
1095  UASSERT(mesh.tex_coordinates.size() == mesh.tex_materials.size() && mesh.tex_polygons.size() == mesh.tex_materials.size());
1096 
1097  // prepare size
1098  std::vector<int> totalPolygons(outputTextures, 0);
1099  std::vector<int> totalCoordinates(outputTextures, 0);
1100  int count = 0;
1101  for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
1102  {
1103  if(mesh.tex_polygons[i].size())
1104  {
1105  int indexMaterial = count / (colCount*rowCount);
1106  UASSERT(indexMaterial < outputTextures);
1107 
1108  totalPolygons[indexMaterial]+=mesh.tex_polygons[i].size();
1109  totalCoordinates[indexMaterial]+=mesh.tex_coordinates[i].size();
1110 
1111  ++count;
1112  }
1113  }
1114 
1115  pcl::TextureMesh outputMesh;
1116 
1117  int pi = 0;
1118  int ci = 0;
1119  int ti=0;
1120  float scaledHeight = float(int(scale*float(h)))/float(textureSize);
1121  float scaledWidth = float(int(scale*float(w)))/float(textureSize);
1122  float lowerBorderSize = 1.0f - scaledHeight*float(rowCount);
1123  UDEBUG("scaledWidth=%f scaledHeight=%f lowerBorderSize=%f", scaledWidth, scaledHeight, lowerBorderSize);
1124  if(materialsKept)
1125  {
1126  materialsKept->resize(mesh.tex_materials.size(), false);
1127  }
1128  for(unsigned int t=0; t<mesh.tex_materials.size(); ++t)
1129  {
1130  if(mesh.tex_polygons[t].size())
1131  {
1132  int indexMaterial = ti / (colCount*rowCount);
1133  UASSERT(indexMaterial < outputTextures);
1134  if((int)outputMesh.tex_polygons.size() <= indexMaterial)
1135  {
1136  std::vector<pcl::Vertices> newPolygons(totalPolygons[indexMaterial]);
1137 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1138  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > newCoordinates(totalCoordinates[indexMaterial]); // UV coordinates
1139 #else
1140  std::vector<Eigen::Vector2f> newCoordinates(totalCoordinates[indexMaterial]); // UV coordinates
1141 #endif
1142  outputMesh.tex_polygons.push_back(newPolygons);
1143  outputMesh.tex_coordinates.push_back(newCoordinates);
1144 
1145  pi=0;
1146  ci=0;
1147  }
1148 
1149  int row = (ti/colCount) % rowCount;
1150  int col = ti%colCount;
1151  float offsetU = scaledWidth * float(col);
1152  float offsetV = scaledHeight * float((rowCount - 1) - row) + lowerBorderSize;
1153  // Texture coords have lower-left origin
1154 
1155  for(unsigned int i=0; i<mesh.tex_polygons[t].size(); ++i)
1156  {
1157  UASSERT(pi < (int)outputMesh.tex_polygons[indexMaterial].size());
1158  outputMesh.tex_polygons[indexMaterial][pi++] = mesh.tex_polygons[t].at(i);
1159  }
1160 
1161  for(unsigned int i=0; i<mesh.tex_coordinates[t].size(); ++i)
1162  {
1163  const Eigen::Vector2f & v = mesh.tex_coordinates[t].at(i);
1164  if(v[0] >= 0 && v[1] >=0)
1165  {
1166  outputMesh.tex_coordinates[indexMaterial][ci][0] = v[0]*scaledWidth + offsetU;
1167  outputMesh.tex_coordinates[indexMaterial][ci][1] = v[1]*scaledHeight + offsetV;
1168  }
1169  else
1170  {
1171  outputMesh.tex_coordinates[indexMaterial][ci] = v;
1172  }
1173  ++ci;
1174  }
1175  ++ti;
1176  if(materialsKept)
1177  {
1178  materialsKept->at(t) = true;
1179  }
1180  }
1181  }
1182  pcl::TexMaterial m = mesh.tex_materials.front();
1183  mesh.tex_materials.clear();
1184  for(int i=0; i<outputTextures; ++i)
1185  {
1186  m.tex_file = "texture";
1187  m.tex_name = "material";
1188  if(outputTextures > 1)
1189  {
1190  m.tex_file += uNumber2Str(i);
1191  m.tex_name += uNumber2Str(i);
1192  }
1193 
1194  mesh.tex_materials.push_back(m);
1195  }
1196  mesh.tex_coordinates = outputMesh.tex_coordinates;
1197  mesh.tex_polygons = outputMesh.tex_polygons;
1198  }
1199 }
1200 
1201 std::vector<std::vector<unsigned int> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
1202 {
1203  std::vector<std::vector<unsigned int> > polygonsOut(polygons.size());
1204  for(unsigned int p=0; p<polygons.size(); ++p)
1205  {
1206  polygonsOut[p] = polygons[p].vertices;
1207  }
1208  return polygonsOut;
1209 }
1210 std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
1211 {
1212  std::vector<std::vector<std::vector<unsigned int> > > polygonsOut(tex_polygons.size());
1213  for(unsigned int t=0; t<tex_polygons.size(); ++t)
1214  {
1215  polygonsOut[t].resize(tex_polygons[t].size());
1216  for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
1217  {
1218  polygonsOut[t][p] = tex_polygons[t][p].vertices;
1219  }
1220  }
1221  return polygonsOut;
1222 }
1223 std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<unsigned int> > & polygons)
1224 {
1225  std::vector<pcl::Vertices> polygonsOut(polygons.size());
1226  for(unsigned int p=0; p<polygons.size(); ++p)
1227  {
1228  polygonsOut[p].vertices = polygons[p];
1229  }
1230  return polygonsOut;
1231 }
1232 std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons)
1233 {
1234  std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
1235  for(unsigned int t=0; t<tex_polygons.size(); ++t)
1236  {
1237  polygonsOut[t].resize(tex_polygons[t].size());
1238  for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
1239  {
1240  polygonsOut[t][p].vertices = tex_polygons[t][p];
1241  }
1242  }
1243  return polygonsOut;
1244 }
1245 
1246 pcl::TextureMesh::Ptr assembleTextureMesh(
1247  const cv::Mat & cloudMat,
1248  const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
1249 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1250  const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
1251 #else
1252  const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
1253 #endif
1254  cv::Mat & textures,
1255  bool mergeTextures)
1256 {
1257  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
1258 
1259  if(cloudMat.channels() <= 3)
1260  {
1261  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
1262  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1263  }
1264  else if(cloudMat.channels() == 4)
1265  {
1266  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
1267  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1268  }
1269  else if(cloudMat.channels() == 6)
1270  {
1271  pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
1272  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1273  }
1274  else if(cloudMat.channels() == 7)
1275  {
1276  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
1277  pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1278  }
1279 
1280  if(textureMesh->cloud.data.size() && polygons.size())
1281  {
1282  textureMesh->tex_polygons.resize(polygons.size());
1283  for(unsigned int t=0; t<polygons.size(); ++t)
1284  {
1285  textureMesh->tex_polygons[t].resize(polygons[t].size());
1286  for(unsigned int p=0; p<polygons[t].size(); ++p)
1287  {
1288  textureMesh->tex_polygons[t][p].vertices = polygons[t][p];
1289  }
1290  }
1291 
1292  if(!texCoords.empty() && !textures.empty())
1293  {
1294  textureMesh->tex_coordinates = texCoords;
1295 
1296  textureMesh->tex_materials.resize (textureMesh->tex_coordinates.size());
1297  for(unsigned int i = 0 ; i < textureMesh->tex_coordinates.size() ; ++i)
1298  {
1299  pcl::TexMaterial mesh_material;
1300  mesh_material.tex_Ka.r = 0.2f;
1301  mesh_material.tex_Ka.g = 0.2f;
1302  mesh_material.tex_Ka.b = 0.2f;
1303 
1304  mesh_material.tex_Kd.r = 0.8f;
1305  mesh_material.tex_Kd.g = 0.8f;
1306  mesh_material.tex_Kd.b = 0.8f;
1307 
1308  mesh_material.tex_Ks.r = 1.0f;
1309  mesh_material.tex_Ks.g = 1.0f;
1310  mesh_material.tex_Ks.b = 1.0f;
1311 
1312  mesh_material.tex_d = 1.0f;
1313  mesh_material.tex_Ns = 75.0f;
1314  mesh_material.tex_illum = 2;
1315 
1316  std::stringstream tex_name;
1317  tex_name << "material_" << i;
1318  tex_name >> mesh_material.tex_name;
1319 
1320  mesh_material.tex_file = uFormat("%d", i);
1321 
1322  textureMesh->tex_materials[i] = mesh_material;
1323  }
1324 
1325  if(mergeTextures && textures.cols/textures.rows > 1)
1326  {
1327  UASSERT(textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)textureMesh->tex_coordinates.size());
1328  std::vector<bool> materialsKept;
1329  float scale = 0.0f;
1330  cv::Size imageSize(textures.rows, textures.rows);
1331  int imageType = textures.type();
1332  rtabmap::util3d::concatenateTextureMaterials(*textureMesh, imageSize, textures.rows, 1, scale, &materialsKept);
1333  if(scale && textureMesh->tex_materials.size() == 1)
1334  {
1335  int cols = float(textures.rows)/(scale*imageSize.width);
1336  int rows = float(textures.rows)/(scale*imageSize.height);
1337 
1338  cv::Mat mergedTextures = cv::Mat(textures.rows, textures.rows, imageType, cv::Scalar::all(255));
1339 
1340  // make a blank texture
1341  cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
1342  int oi=0;
1343  for(int i=0; i<(int)materialsKept.size(); ++i)
1344  {
1345  if(materialsKept.at(i))
1346  {
1347  int u = oi%cols * resizedImageSize.width;
1348  int v = ((oi/cols) % rows ) * resizedImageSize.height;
1349  UASSERT(u < textures.rows-resizedImageSize.width);
1350  UASSERT(v < textures.rows-resizedImageSize.height);
1351 
1352  cv::Mat resizedImage;
1353  cv::resize(textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
1354 
1355  UASSERT(resizedImage.type() == mergedTextures.type());
1356  resizedImage.copyTo(mergedTextures(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
1357 
1358  ++oi;
1359  }
1360  }
1361  textures = mergedTextures;
1362  }
1363  }
1364  }
1365  }
1366  return textureMesh;
1367 }
1368 
1369 pcl::PolygonMesh::Ptr assemblePolygonMesh(
1370  const cv::Mat & cloudMat,
1371  const std::vector<std::vector<unsigned int> > & polygons)
1372 {
1373  pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
1374 
1375  if(cloudMat.channels() <= 3)
1376  {
1377  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
1378  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1379  }
1380  else if(cloudMat.channels() == 4)
1381  {
1382  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
1383  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1384  }
1385  else if(cloudMat.channels() == 6)
1386  {
1387  pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
1388  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1389  }
1390  else if(cloudMat.channels() == 7)
1391  {
1392  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
1393  pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
1394  }
1395 
1396  if(polygonMesh->cloud.data.size() && polygons.size())
1397  {
1398  polygonMesh->polygons.resize(polygons.size());
1399  for(unsigned int p=0; p<polygons.size(); ++p)
1400  {
1401  polygonMesh->polygons[p].vertices = polygons[p];
1402  }
1403  }
1404  return polygonMesh;
1405 }
1406 
1407 double sqr(uchar v)
1408 {
1409  return double(v)*double(v);
1410 }
1411 
1412 cv::Mat mergeTextures(
1413  pcl::TextureMesh & mesh,
1414  const std::map<int, cv::Mat> & images,
1415  const std::map<int, CameraModel> & calibrations,
1416  const Memory * memory,
1417  const DBDriver * dbDriver,
1418  int textureSize,
1419  int textureCount,
1420  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
1421  bool gainCompensation,
1422  float gainBeta,
1423  bool gainRGB,
1424  bool blending,
1425  int blendingDecimation,
1426  int brightnessContrastRatioLow,
1427  int brightnessContrastRatioHigh,
1428  bool exposureFusion,
1429  const ProgressState * state)
1430 {
1431  std::map<int, std::vector<CameraModel> > calibVectors;
1432  for(std::map<int, CameraModel>::const_iterator iter=calibrations.begin(); iter!=calibrations.end(); ++iter)
1433  {
1434  std::vector<CameraModel> m;
1435  m.push_back(iter->second);
1436  calibVectors.insert(std::make_pair(iter->first, m));
1437  }
1438  return mergeTextures(mesh,
1439  images,
1440  calibVectors,
1441  memory,
1442  dbDriver,
1443  textureSize,
1444  textureCount,
1445  vertexToPixels,
1446  gainCompensation,
1447  gainBeta,
1448  gainRGB,
1449  blending,
1450  blendingDecimation,
1451  brightnessContrastRatioLow,
1452  brightnessContrastRatioHigh,
1453  exposureFusion,
1454  state);
1455 }
1456 cv::Mat mergeTextures(
1457  pcl::TextureMesh & mesh,
1458  const std::map<int, cv::Mat> & images,
1459  const std::map<int, std::vector<CameraModel> > & calibrations,
1460  const Memory * memory,
1461  const DBDriver * dbDriver,
1462  int textureSize,
1463  int textureCount,
1464  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
1465  bool gainCompensation,
1466  float gainBeta,
1467  bool gainRGB,
1468  bool blending,
1469  int blendingDecimation,
1470  int brightnessContrastRatioLow,
1471  int brightnessContrastRatioHigh,
1472  bool exposureFusion,
1473  const ProgressState * state)
1474 {
1475  //get texture size, if disabled use default 1024
1476  UASSERT(textureSize%256 == 0);
1477  UDEBUG("textureSize = %d", textureSize);
1478  cv::Mat globalTextures;
1479  if(mesh.tex_materials.size() > 1)
1480  {
1481  std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,-1));
1482  cv::Size imageSize;
1483  const int imageType=CV_8UC3;
1484 
1485  UDEBUG("");
1486  for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
1487  {
1488  std::list<std::string> texFileSplit = uSplit(mesh.tex_materials[i].tex_file, '_');
1489  if(!mesh.tex_materials[i].tex_file.empty() &&
1490  mesh.tex_polygons[i].size() &&
1491  uIsInteger(texFileSplit.front(), false))
1492  {
1493  textures[i].first = uStr2Int(texFileSplit.front());
1494  if(texFileSplit.size() == 2 &&
1495  uIsInteger(texFileSplit.back(), false) )
1496  {
1497  textures[i].second = uStr2Int(texFileSplit.back());
1498  }
1499 
1500  int textureId = textures[i].first;
1501  if(imageSize.width == 0 || imageSize.height == 0)
1502  {
1503  if(images.find(textureId) != images.end() &&
1504  !images.find(textureId)->second.empty() &&
1505  calibrations.find(textureId) != calibrations.end())
1506  {
1507  const std::vector<CameraModel> & models = calibrations.find(textureId)->second;
1508  UASSERT(models.size()>=1);
1509  if( models[0].imageHeight()>0 &&
1510  models[0].imageWidth()>0)
1511  {
1512  imageSize = models[0].imageSize();
1513  }
1514  else if(images.find(textureId)!=images.end())
1515  {
1516  // backward compatibility for image size not set in CameraModel
1517  cv::Mat image = images.find(textureId)->second;
1518  if(image.rows == 1 && image.type() == CV_8UC1)
1519  {
1520  image = uncompressImage(image);
1521  }
1522  UASSERT(!image.empty());
1523  imageSize = image.size();
1524  if(models.size()>1)
1525  {
1526  imageSize.width/=models.size();
1527  }
1528  }
1529  }
1530  else if(memory)
1531  {
1532  SensorData data = memory->getSignatureDataConst(textureId, true, false, false, false);
1533  std::vector<CameraModel> models = data.cameraModels();
1534  StereoCameraModel stereoModel = data.stereoCameraModel();
1535  if(models.size()>=1 &&
1536  models[0].imageHeight()>0 &&
1537  models[0].imageWidth()>0)
1538  {
1539  imageSize = models[0].imageSize();
1540  }
1541  else if(stereoModel.left().imageHeight() > 0 &&
1542  stereoModel.left().imageWidth() > 0)
1543  {
1544  imageSize = stereoModel.left().imageSize();
1545  }
1546  else // backward compatibility for image size not set in CameraModel
1547  {
1548  cv::Mat image;
1549  data.uncompressDataConst(&image, 0);
1550  UASSERT(!image.empty());
1551  imageSize = image.size();
1552  if(data.cameraModels().size()>1)
1553  {
1554  imageSize.width/=data.cameraModels().size();
1555  }
1556  }
1557  }
1558  else if(dbDriver)
1559  {
1560  std::vector<CameraModel> models;
1561  StereoCameraModel stereoModel;
1562  dbDriver->getCalibration(textureId, models, stereoModel);
1563  if(models.size()>=1 &&
1564  models[0].imageHeight()>0 &&
1565  models[0].imageWidth()>0)
1566  {
1567  imageSize = models[0].imageSize();
1568  }
1569  else if(stereoModel.left().imageHeight() > 0 &&
1570  stereoModel.left().imageWidth() > 0)
1571  {
1572  imageSize = stereoModel.left().imageSize();
1573  }
1574  else // backward compatibility for image size not set in CameraModel
1575  {
1576  SensorData data;
1577  dbDriver->getNodeData(textureId, data, true, false, false, false);
1578  cv::Mat image;
1579  data.uncompressDataConst(&image, 0);
1580  UASSERT(!image.empty());
1581  imageSize = image.size();
1582  if(data.cameraModels().size()>1)
1583  {
1584  imageSize.width/=data.cameraModels().size();
1585  }
1586  }
1587  }
1588  }
1589  }
1590  else if(mesh.tex_polygons[i].size() && mesh.tex_materials[i].tex_file.compare("occluded")!=0)
1591  {
1592  UWARN("Failed parsing texture file name: %s", mesh.tex_materials[i].tex_file.c_str());
1593  }
1594  }
1595  UDEBUG("textures=%d imageSize=%dx%d", (int)textures.size(), imageSize.height, imageSize.width);
1596  if(textures.size() && imageSize.height>0 && imageSize.width>0)
1597  {
1598  float scale = 0.0f;
1599  UDEBUG("");
1600  std::vector<bool> materialsKept;
1601  util3d::concatenateTextureMaterials(mesh, imageSize, textureSize, textureCount, scale, &materialsKept);
1602  if(scale && mesh.tex_materials.size())
1603  {
1604  int materials = (int)mesh.tex_materials.size();
1605  int cols = float(textureSize)/(scale*imageSize.width);
1606  int rows = float(textureSize)/(scale*imageSize.height);
1607 
1608  globalTextures = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(255));
1609  cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));
1610 
1611  // used for multi camera texturing, to avoid reloading same texture for sub cameras
1612  cv::Mat previousImage;
1613  int previousTextureId = 0;
1614  std::vector<CameraModel> previousCameraModels;
1615 
1616  // make a blank texture
1617  cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(255));
1618  cv::Mat emptyImageMask(int(imageSize.height*scale), int(imageSize.width*scale), CV_8UC1, cv::Scalar::all(255));
1619  int oi=0;
1620  std::vector<cv::Point2i> imageOrigin(textures.size());
1621  std::vector<int> newCamIndex(textures.size(), -1);
1622  for(int t=0; t<(int)textures.size(); ++t)
1623  {
1624  if(materialsKept.at(t))
1625  {
1626  int indexMaterial = oi / (cols*rows);
1627  UASSERT(indexMaterial < materials);
1628 
1629  newCamIndex[t] = oi;
1630  int u = oi%cols * emptyImage.cols;
1631  int v = ((oi/cols) % rows ) * emptyImage.rows;
1632  UASSERT_MSG(u < textureSize-emptyImage.cols, uFormat("u=%d textureSize=%d emptyImage.cols=%d", u, textureSize, emptyImage.cols).c_str());
1633  UASSERT_MSG(v < textureSize-emptyImage.rows, uFormat("v=%d textureSize=%d emptyImage.rows=%d", v, textureSize, emptyImage.rows).c_str());
1634  imageOrigin[t].x = u;
1635  imageOrigin[t].y = v;
1636  if(textures[t].first>=0)
1637  {
1638  cv::Mat image;
1639  std::vector<CameraModel> models;
1640 
1641  if(textures[t].first == previousTextureId)
1642  {
1643  image = previousImage;
1644  models = previousCameraModels;
1645  }
1646  else
1647  {
1648  if(images.find(textures[t].first) != images.end() &&
1649  !images.find(textures[t].first)->second.empty() &&
1650  calibrations.find(textures[t].first) != calibrations.end())
1651  {
1652  image = images.find(textures[t].first)->second;
1653  if(image.rows == 1 && image.type() == CV_8UC1)
1654  {
1655  image = uncompressImage(image);
1656  }
1657  models = calibrations.find(textures[t].first)->second;
1658  }
1659  else if(memory)
1660  {
1661  SensorData data = memory->getSignatureDataConst(textures[t].first, true, false, false, false);
1662  models = data.cameraModels();
1663  data.uncompressDataConst(&image, 0);
1664  }
1665  else if(dbDriver)
1666  {
1667  SensorData data;
1668  dbDriver->getNodeData(textures[t].first, data, true, false, false, false);
1669  data.uncompressDataConst(&image, 0);
1670  StereoCameraModel stereoModel;
1671  dbDriver->getCalibration(textures[t].first, models, stereoModel);
1672  }
1673 
1674  previousImage = image;
1675  previousCameraModels = models;
1676  previousTextureId = textures[t].first;
1677  }
1678 
1679  UASSERT(!image.empty());
1680 
1681  if(textures[t].second>=0)
1682  {
1683  UASSERT(textures[t].second < (int)models.size());
1684  int width = image.cols/models.size();
1685  image = image.colRange(width*textures[t].second, width*(textures[t].second+1));
1686  }
1687 
1688  cv::Mat resizedImage;
1689  cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
1690  UASSERT(resizedImage.type() == CV_8UC1 || resizedImage.type() == CV_8UC3);
1691  if(resizedImage.type() == CV_8UC1)
1692  {
1693  cv::Mat resizedImageColor;
1694  cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
1695  resizedImage = resizedImageColor;
1696  }
1697  UASSERT(resizedImage.type() == globalTextures.type());
1698  resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
1699  emptyImageMask.copyTo(globalTextureMasks(cv::Rect(u+indexMaterial*globalTextureMasks.rows, v, resizedImage.cols, resizedImage.rows)));
1700  }
1701  else
1702  {
1703  emptyImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows)));
1704  }
1705  ++oi;
1706  }
1707 
1708  if(state)
1709  {
1710  if(state->isCanceled())
1711  {
1712  return cv::Mat();
1713  }
1714  state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
1715  }
1716  }
1717 
1718  UTimer timer;
1719  if(vertexToPixels.size())
1720  {
1721  //UWARN("Saving original.png", globalTexture);
1722  //cv::imwrite("original.png", globalTexture);
1723 
1724  if(gainCompensation)
1725  {
1730  const int num_images = static_cast<int>(oi);
1731  cv::Mat_<int> N(num_images, num_images); N.setTo(0);
1732  cv::Mat_<double> I(num_images, num_images); I.setTo(0);
1733 
1734  cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
1735  cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
1736  cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);
1737 
1738  // Adjust UV coordinates to globalTexture
1739  for(unsigned int p=0; p<vertexToPixels.size(); ++p)
1740  {
1741  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
1742  {
1743  if(materialsKept.at(iter->first))
1744  {
1745  N(newCamIndex[iter->first], newCamIndex[iter->first]) +=1;
1746 
1747  std::map<int, pcl::PointXY>::const_iterator jter=iter;
1748  ++jter;
1749  int k = 1;
1750  for(; jter!=vertexToPixels[p].end(); ++jter, ++k)
1751  {
1752  if(materialsKept.at(jter->first))
1753  {
1754  int i = newCamIndex[iter->first];
1755  int j = newCamIndex[jter->first];
1756 
1757  N(i, j) += 1;
1758  N(j, i) += 1;
1759 
1760  int indexMaterial = i / (cols*rows);
1761 
1762  // uv in globalTexture
1763  int ui = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
1764  int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
1765  int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
1766  int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
1767  cv::Vec3b * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
1768  cv::Vec3b * pt2 = globalTextures.ptr<cv::Vec3b>(vj,uj+indexMaterial*globalTextures.rows);
1769 
1770  I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
1771  I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));
1772 
1773  IR(i, j) += static_cast<double>(pt1->val[2]);
1774  IR(j, i) += static_cast<double>(pt2->val[2]);
1775  IG(i, j) += static_cast<double>(pt1->val[1]);
1776  IG(j, i) += static_cast<double>(pt2->val[1]);
1777  IB(i, j) += static_cast<double>(pt1->val[0]);
1778  IB(j, i) += static_cast<double>(pt2->val[0]);
1779  }
1780  }
1781  }
1782  }
1783  }
1784 
1785  for(int i=0; i<num_images; ++i)
1786  {
1787  for(int j=i; j<num_images; ++j)
1788  {
1789  if(i == j)
1790  {
1791  if(N(i,j) == 0)
1792  {
1793  N(i,j) = 1;
1794  }
1795  }
1796  else if(N(i, j))
1797  {
1798  I(i, j) /= N(i, j);
1799  I(j, i) /= N(j, i);
1800 
1801  IR(i, j) /= N(i, j);
1802  IR(j, i) /= N(j, i);
1803  IG(i, j) /= N(i, j);
1804  IG(j, i) /= N(j, i);
1805  IB(i, j) /= N(i, j);
1806  IB(j, i) /= N(j, i);
1807  }
1808  }
1809  }
1810 
1811  cv::Mat_<double> A(num_images, num_images); A.setTo(0);
1812  cv::Mat_<double> b(num_images, 1); b.setTo(0);
1813  cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
1814  cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
1815  cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
1816  double alpha = 0.01;
1817  double beta = gainBeta;
1818  for (int i = 0; i < num_images; ++i)
1819  {
1820  for (int j = 0; j < num_images; ++j)
1821  {
1822  b(i, 0) += beta * N(i, j);
1823  A(i, i) += beta * N(i, j);
1824  AR(i, i) += beta * N(i, j);
1825  AG(i, i) += beta * N(i, j);
1826  AB(i, i) += beta * N(i, j);
1827  if (j == i) continue;
1828  A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
1829  A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
1830 
1831  AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
1832  AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);
1833 
1834  AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
1835  AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);
1836 
1837  AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
1838  AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
1839  }
1840  }
1841 
1842  cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
1843  cv::solve(A, b, gainsGray);
1844 
1845  cv::solve(AR, b, gainsR);
1846  cv::solve(AG, b, gainsG);
1847  cv::solve(AB, b, gainsB);
1848 
1849  cv::Mat_<double> gains(gainsGray.rows, 4);
1850  gainsGray.copyTo(gains.col(0));
1851  gainsR.copyTo(gains.col(1));
1852  gainsG.copyTo(gains.col(2));
1853  gainsB.copyTo(gains.col(3));
1854 
1855  for(int t=0; t<(int)textures.size(); ++t)
1856  {
1857  //break;
1858  if(materialsKept.at(t))
1859  {
1860  int u = imageOrigin[t].x;
1861  int v = imageOrigin[t].y;
1862 
1863  UDEBUG("Gain cam%d = %f", newCamIndex[t], gainsGray(newCamIndex[t], 0));
1864 
1865  int indexMaterial = newCamIndex[t] / (cols*rows);
1866  cv::Mat roi = globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows));
1867 
1868  std::vector<cv::Mat> channels;
1869  cv::split(roi, channels);
1870 
1871  // assuming BGR
1872  cv::multiply(channels[0], gains(newCamIndex[t], gainRGB?3:0), channels[0]);
1873  cv::multiply(channels[1], gains(newCamIndex[t], gainRGB?2:0), channels[1]);
1874  cv::multiply(channels[2], gains(newCamIndex[t], gainRGB?1:0), channels[2]);
1875 
1876  cv::merge(channels, roi);
1877  }
1878  }
1879  //UWARN("Saving gain.png", globalTexture);
1880  //cv::imwrite("gain.png", globalTexture);
1881  if(state) state->callback(uFormat("Gain compensation %fs", timer.ticks()));
1882  }
1883 
1884  if(blending)
1885  {
1886  // blending BGR
1887  int decimation = 1;
1888  if(blendingDecimation <= 0)
1889  {
1890  // determinate decimation to apply
1891  std::vector<float> edgeLengths;
1892  if(mesh.tex_coordinates.size() && mesh.tex_coordinates[0].size())
1893  {
1894  UASSERT(mesh.tex_polygons.size() && mesh.tex_polygons[0].size() && mesh.tex_polygons[0][0].vertices.size());
1895  int polygonSize = mesh.tex_polygons[0][0].vertices.size();
1896  UDEBUG("polygon size=%d", polygonSize);
1897 
1898  for(unsigned int k=0; k<mesh.tex_coordinates.size(); ++k)
1899  {
1900  for(unsigned int i=0; i<mesh.tex_coordinates[k].size(); i+=polygonSize)
1901  {
1902  for(int j=0; j<polygonSize; ++j)
1903  {
1904  const Eigen::Vector2f & uc1 = mesh.tex_coordinates[k][i + j];
1905  const Eigen::Vector2f & uc2 = mesh.tex_coordinates[k][i + (j+1)%polygonSize];
1906  Eigen::Vector2f edge = (uc1-uc2)*textureSize;
1907  edgeLengths.push_back(fabs(edge[0]));
1908  edgeLengths.push_back(fabs(edge[1]));
1909  }
1910  }
1911  }
1912  float edgeLength = 0.0f;
1913  if(edgeLengths.size())
1914  {
1915  std::sort(edgeLengths.begin(), edgeLengths.end());
1916  float m = uMean(edgeLengths.data(), edgeLengths.size());
1917  float stddev = std::sqrt(uVariance(edgeLengths.data(), edgeLengths.size(), m));
1918  edgeLength = m+stddev;
1919  decimation = 1 << 6;
1920  for(int i=1; i<=6; ++i)
1921  {
1922  if(float(1 << i) >= edgeLength)
1923  {
1924  decimation = 1 << i;
1925  break;
1926  }
1927  }
1928  }
1929 
1930  UDEBUG("edge length=%f decimation=%d", edgeLength, decimation);
1931  }
1932  }
1933  else
1934  {
1935  if(blendingDecimation > 1)
1936  {
1937  UASSERT(textureSize % blendingDecimation == 0);
1938  }
1939  decimation = blendingDecimation;
1940  UDEBUG("decimation=%d", decimation);
1941  }
1942 
1943  std::vector<cv::Mat> blendGains(materials);
1944  for(int i=0; i<materials;++i)
1945  {
1946  blendGains[i] = cv::Mat(globalTextures.rows/decimation, globalTextures.rows/decimation, CV_32FC3, cv::Scalar::all(1.0f));
1947  }
1948 
1949  for(unsigned int p=0; p<vertexToPixels.size(); ++p)
1950  {
1951  if(vertexToPixels[p].size() > 1)
1952  {
1953  std::vector<float> gainsB(vertexToPixels[p].size());
1954  std::vector<float> gainsG(vertexToPixels[p].size());
1955  std::vector<float> gainsR(vertexToPixels[p].size());
1956  float sumWeight = 0.0f;
1957  int k=0;
1958  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
1959  {
1960  if(materialsKept.at(iter->first))
1961  {
1962  int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
1963  int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
1964  float x = iter->second.x - 0.5f;
1965  float y = iter->second.y - 0.5f;
1966  float weight = 0.7f - sqrt(x*x+y*y);
1967  if(weight<0.0f)
1968  {
1969  weight = 0.0f;
1970  }
1971  int indexMaterial = newCamIndex[iter->first] / (cols*rows);
1972  cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
1973  gainsB[k] = static_cast<double>(pt->val[0]) * weight;
1974  gainsG[k] = static_cast<double>(pt->val[1]) * weight;
1975  gainsR[k] = static_cast<double>(pt->val[2]) * weight;
1976  sumWeight += weight;
1977  ++k;
1978  }
1979  }
1980  gainsB.resize(k);
1981  gainsG.resize(k);
1982  gainsR.resize(k);
1983 
1984  if(sumWeight > 0)
1985  {
1986  float targetColor[3];
1987  targetColor[0] = uSum(gainsB.data(), gainsB.size()) / sumWeight;
1988  targetColor[1] = uSum(gainsG.data(), gainsG.size()) / sumWeight;
1989  targetColor[2] = uSum(gainsR.data(), gainsR.size()) / sumWeight;
1990  for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
1991  {
1992  if(materialsKept.at(iter->first))
1993  {
1994  int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
1995  int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
1996  int indexMaterial = newCamIndex[iter->first] / (cols*rows);
1997  cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
1998  float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
1999  float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
2000  float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
2001  cv::Vec3f * ptr = blendGains[indexMaterial].ptr<cv::Vec3f>(v/decimation, u/decimation);
2002  ptr->val[0] = (gB>1.3f)?1.3f:(gB<0.7f)?0.7f:gB;
2003  ptr->val[1] = (gG>1.3f)?1.3f:(gG<0.7f)?0.7f:gG;
2004  ptr->val[2] = (gR>1.3f)?1.3f:(gR<0.7f)?0.7f:gR;
2005  }
2006  }
2007  }
2008  }
2009  }
2010 
2011  for(int i=0; i<materials; ++i)
2012  {
2013  /*std::vector<cv::Mat> channels;
2014  cv::split(blendGains, channels);
2015  cv::Mat img;
2016  channels[0].convertTo(img,CV_8U,128.0,0);
2017  cv::imwrite("blendSmallB.png", img);
2018  channels[1].convertTo(img,CV_8U,128.0,0);
2019  cv::imwrite("blendSmallG.png", img);
2020  channels[2].convertTo(img,CV_8U,128.0,0);
2021  cv::imwrite("blendSmallR.png", img);*/
2022 
2023  cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
2024  cv::Mat dst;
2025  cv::blur(blendGains[i], dst, cv::Size(3,3));
2026  cv::resize(dst, blendGains[i], globalTexturesROI.size(), 0, 0, cv::INTER_LINEAR);
2027 
2028  /*cv::split(blendGains, channels);
2029  channels[0].convertTo(img,CV_8U,128.0,0);
2030  cv::imwrite("blendFullB.png", img);
2031  channels[1].convertTo(img,CV_8U,128.0,0);
2032  cv::imwrite("blendFullG.png", img);
2033  channels[2].convertTo(img,CV_8U,128.0,0);
2034  cv::imwrite("blendFullR.png", img);*/
2035 
2036  cv::multiply(globalTexturesROI, blendGains[i], globalTexturesROI, 1.0, CV_8UC3);
2037 
2038  //UWARN("Saving blending.png", globalTexture);
2039  //cv::imwrite("blending.png", globalTexture);
2040  }
2041 
2042  if(state) state->callback(uFormat("Blending (decimation=%d) %fs", decimation, timer.ticks()));
2043  }
2044  }
2045 
2046  if(brightnessContrastRatioLow > 0 || brightnessContrastRatioHigh > 0)
2047  {
2048  for(int i=0; i<materials; ++i)
2049  {
2050  cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
2051  cv::Mat globalTextureMasksROI = globalTextureMasks(cv::Range::all(), cv::Range(i*globalTextureMasks.rows, (i+1)*globalTextureMasks.rows));
2052  if(exposureFusion)
2053  {
2054  std::vector<cv::Mat> images;
2055  images.push_back(globalTexturesROI);
2056  if (brightnessContrastRatioLow > 0)
2057  {
2058  images.push_back(util2d::brightnessAndContrastAuto(
2059  globalTexturesROI,
2060  globalTextureMasksROI,
2061  (float)brightnessContrastRatioLow,
2062  0.0f));
2063  }
2064  if (brightnessContrastRatioHigh > 0)
2065  {
2066  images.push_back(util2d::brightnessAndContrastAuto(
2067  globalTexturesROI,
2068  globalTextureMasksROI,
2069  0.0f,
2070  (float)brightnessContrastRatioHigh));
2071  }
2072 
2073  util2d::exposureFusion(images).copyTo(globalTexturesROI);
2074  }
2075  else
2076  {
2077  util2d::brightnessAndContrastAuto(
2078  globalTexturesROI,
2079  globalTextureMasksROI,
2080  (float)brightnessContrastRatioLow,
2081  (float)brightnessContrastRatioHigh).copyTo(globalTexturesROI);
2082  }
2083  }
2084  if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
2085  }
2086  }
2087  }
2088  }
2089  UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
2090  return globalTextures;
2091 }
2092 
2093 void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
2094 {
2095  // VTK issue:
2096  // tex_coordinates should be linked to points, not
2097  // polygon vertices. Points linked to multiple different TCoords (different textures) should
2098  // be duplicated.
2099  for (unsigned int t = 0; t < textureMesh.tex_coordinates.size(); ++t)
2100  {
2101  if(textureMesh.tex_polygons[t].size())
2102  {
2103  pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
2104  pcl::fromPCLPointCloud2(textureMesh.cloud, *originalCloud);
2105 
2106  // make a cloud with as many points than polygon vertices
2107  unsigned int nPoints = textureMesh.tex_coordinates[t].size();
2108  UASSERT(nPoints == textureMesh.tex_polygons[t].size()*textureMesh.tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
2109 
2110  pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>);
2111  newCloud->resize(nPoints);
2112 
2113  unsigned int oi = 0;
2114  for (unsigned int i = 0; i < textureMesh.tex_polygons[t].size(); ++i)
2115  {
2116  pcl::Vertices & vertices = textureMesh.tex_polygons[t][i];
2117 
2118  for(unsigned int j=0; j<vertices.vertices.size(); ++j)
2119  {
2120  UASSERT(oi < newCloud->size());
2121  UASSERT_MSG(vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
2122  newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
2123  vertices.vertices[j] = oi; // new vertex index
2124  ++oi;
2125  }
2126  }
2127  pcl::toPCLPointCloud2(*newCloud, textureMesh.cloud);
2128  }
2129  }
2130 }
2131 
2132 LaserScan computeNormals(
2133  const LaserScan & laserScan,
2134  int searchK,
2135  float searchRadius)
2136 {
2137  if(laserScan.isEmpty())
2138  {
2139  return laserScan;
2140  }
2141 
2142  pcl::PointCloud<pcl::Normal>::Ptr normals;
2143  // convert to compatible PCL format and filter it
2144  if(laserScan.hasRGB())
2145  {
2146  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(laserScan);
2147  if(cloud->size())
2148  {
2149  UASSERT(!laserScan.is2d());
2150  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
2151  return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
2152  }
2153  }
2154  else if(laserScan.hasIntensity())
2155  {
2156  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(laserScan);
2157  if(cloud->size())
2158  {
2159  if(laserScan.is2d())
2160  {
2161  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
2162  return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
2163  }
2164  else
2165  {
2166  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
2167  return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
2168  }
2169  }
2170  }
2171  else
2172  {
2173  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(laserScan);
2174  if(cloud->size())
2175  {
2176  if(laserScan.is2d())
2177  {
2178  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
2179  return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
2180  }
2181  else
2182  {
2183  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
2184  return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
2185  }
2186  }
2187  }
2188  return LaserScan();
2189 }
2190 
2191 template<typename PointT>
2192 pcl::PointCloud<pcl::Normal>::Ptr computeNormalsImpl(
2193  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2194  const pcl::IndicesPtr & indices,
2195  int searchK,
2196  float searchRadius,
2197  const Eigen::Vector3f & viewPoint)
2198 {
2199  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
2200  if(indices->size())
2201  {
2202  tree->setInputCloud(cloud, indices);
2203  }
2204  else
2205  {
2206  tree->setInputCloud (cloud);
2207  }
2208 
2209  // Normal estimation*
2210 #ifdef PCL_OMP
2211  pcl::NormalEstimationOMP<PointT, pcl::Normal> n;
2212 #else
2213  pcl::NormalEstimation<PointT, pcl::Normal> n;
2214 #endif
2215  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2216  n.setInputCloud (cloud);
2217  // Commented: Keep the output normals size the same as the input cloud
2218  //if(indices->size())
2219  //{
2220  // n.setIndices(indices);
2221  //}
2222  n.setSearchMethod (tree);
2223  n.setKSearch (searchK);
2224  n.setRadiusSearch (searchRadius);
2225  n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
2226  n.compute (*normals);
2227 
2228  return normals;
2229 }
2230 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2231  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2232  int searchK,
2233  float searchRadius,
2234  const Eigen::Vector3f & viewPoint)
2235 {
2236  pcl::IndicesPtr indices(new std::vector<int>);
2237  return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
2238 }
2239 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2240  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2241  int searchK,
2242  float searchRadius,
2243  const Eigen::Vector3f & viewPoint)
2244 {
2245  pcl::IndicesPtr indices(new std::vector<int>);
2246  return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
2247 }
2248 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2249  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2250  int searchK,
2251  float searchRadius,
2252  const Eigen::Vector3f & viewPoint)
2253 {
2254  pcl::IndicesPtr indices(new std::vector<int>);
2255  return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
2256 }
2257 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2258  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2259  const pcl::IndicesPtr & indices,
2260  int searchK,
2261  float searchRadius,
2262  const Eigen::Vector3f & viewPoint)
2263 {
2264  return computeNormalsImpl<pcl::PointXYZ>(cloud, indices, searchK, searchRadius, viewPoint);
2265 }
2266 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2267  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2268  const pcl::IndicesPtr & indices,
2269  int searchK,
2270  float searchRadius,
2271  const Eigen::Vector3f & viewPoint)
2272 {
2273  return computeNormalsImpl<pcl::PointXYZRGB>(cloud, indices, searchK, searchRadius, viewPoint);
2274 }
2275 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
2276  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2277  const pcl::IndicesPtr & indices,
2278  int searchK,
2279  float searchRadius,
2280  const Eigen::Vector3f & viewPoint)
2281 {
2282  return computeNormalsImpl<pcl::PointXYZI>(cloud, indices, searchK, searchRadius, viewPoint);
2283 }
2284 
2285 template<typename PointT>
2286 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2DImpl(
2287  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2288  int searchK,
2289  float searchRadius,
2290  const Eigen::Vector3f & viewPoint)
2291 {
2292  UASSERT(searchK>0 || searchRadius>0.0f);
2293  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2294 
2295  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
2296  tree->setInputCloud (cloud);
2297 
2298  normals->resize(cloud->size());
2299 
2300  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2301 
2302  // assuming that points are ordered
2303  for(unsigned int i=0; i<cloud->size(); ++i)
2304  {
2305  const PointT & pt = cloud->at(i);
2306  std::vector<Eigen::Vector3f> neighborNormals;
2307  Eigen::Vector3f direction;
2308  direction[0] = viewPoint[0] - pt.x;
2309  direction[1] = viewPoint[1] - pt.y;
2310  direction[2] = viewPoint[2] - pt.z;
2311 
2312  std::vector<int> k_indices;
2313  std::vector<float> k_sqr_distances;
2314  if(searchRadius>0.0f)
2315  {
2316  tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
2317  }
2318  else
2319  {
2320  tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
2321  }
2322 
2323  for(unsigned int j=0; j<k_indices.size(); ++j)
2324  {
2325  if(k_indices.at(j) != (int)i)
2326  {
2327  const PointT & pt2 = cloud->at(k_indices.at(j));
2328  Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2329  Eigen::Vector3f up = v.cross(direction);
2330  Eigen::Vector3f n = up.cross(v);
2331  n.normalize();
2332  neighborNormals.push_back(n);
2333  }
2334  }
2335 
2336  if(neighborNormals.empty())
2337  {
2338  normals->at(i).normal_x = bad_point;
2339  normals->at(i).normal_y = bad_point;
2340  normals->at(i).normal_z = bad_point;
2341  }
2342  else
2343  {
2344  Eigen::Vector3f meanNormal(0,0,0);
2345  for(unsigned int j=0; j<neighborNormals.size(); ++j)
2346  {
2347  meanNormal+=neighborNormals[j];
2348  }
2349  meanNormal /= (float)neighborNormals.size();
2350  meanNormal.normalize();
2351  normals->at(i).normal_x = meanNormal[0];
2352  normals->at(i).normal_y = meanNormal[1];
2353  normals->at(i).normal_z = meanNormal[2];
2354  }
2355  }
2356 
2357  return normals;
2358 }
2359 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
2360  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2361  int searchK,
2362  float searchRadius,
2363  const Eigen::Vector3f & viewPoint)
2364 {
2365  return computeNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
2366 }
2367 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
2368  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2369  int searchK,
2370  float searchRadius,
2371  const Eigen::Vector3f & viewPoint)
2372 {
2373  return computeNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
2374 }
2375 
2376 template<typename PointT>
2377 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2DImpl(
2378  const typename pcl::PointCloud<PointT>::Ptr & cloud,
2379  int searchK,
2380  float searchRadius,
2381  const Eigen::Vector3f & viewPoint)
2382 {
2383  UASSERT(searchK>0);
2384  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2385 
2386  normals->resize(cloud->size());
2387  searchRadius *= searchRadius; // squared distance
2388 
2389  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2390 
2391  // assuming that points are ordered
2392  for(int i=0; i<(int)cloud->size(); ++i)
2393  {
2394  int li = i-searchK;
2395  if(li<0)
2396  {
2397  li=0;
2398  }
2399  int hi = i+searchK;
2400  if(hi>=(int)cloud->size())
2401  {
2402  hi=(int)cloud->size()-1;
2403  }
2404 
2405  // get points before not too far
2406  const PointT & pt = cloud->at(i);
2407  std::vector<Eigen::Vector3f> neighborNormals;
2408  Eigen::Vector3f direction;
2409  direction[0] = viewPoint[0] - cloud->at(i).x;
2410  direction[1] = viewPoint[1] - cloud->at(i).y;
2411  direction[2] = viewPoint[2] - cloud->at(i).z;
2412  for(int j=i-1; j>=li; --j)
2413  {
2414  const PointT & pt2 = cloud->at(j);
2415  Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2416  if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
2417  {
2418  Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2419  Eigen::Vector3f up = v.cross(direction);
2420  Eigen::Vector3f n = up.cross(v);
2421  n.normalize();
2422  neighborNormals.push_back(n);
2423  }
2424  else
2425  {
2426  break;
2427  }
2428  }
2429  for(int j=i+1; j<=hi; ++j)
2430  {
2431  const PointT & pt2 = cloud->at(j);
2432  Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2433  if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
2434  {
2435  Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
2436  Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
2437  Eigen::Vector3f n = up.cross(v);
2438  n.normalize();
2439  neighborNormals.push_back(n);
2440  }
2441  else
2442  {
2443  break;
2444  }
2445  }
2446 
2447  if(neighborNormals.empty())
2448  {
2449  normals->at(i).normal_x = bad_point;
2450  normals->at(i).normal_y = bad_point;
2451  normals->at(i).normal_z = bad_point;
2452  }
2453  else
2454  {
2455  Eigen::Vector3f meanNormal(0,0,0);
2456  for(unsigned int j=0; j<neighborNormals.size(); ++j)
2457  {
2458  meanNormal+=neighborNormals[j];
2459  }
2460  meanNormal /= (float)neighborNormals.size();
2461  meanNormal.normalize();
2462  normals->at(i).normal_x = meanNormal[0];
2463  normals->at(i).normal_y = meanNormal[1];
2464  normals->at(i).normal_z = meanNormal[2];
2465  }
2466  }
2467 
2468  return normals;
2469 }
2470 
2471 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
2472  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
2473  int searchK,
2474  float searchRadius,
2475  const Eigen::Vector3f & viewPoint)
2476 {
2477  return computeFastOrganizedNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
2478 }
2479 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
2480  const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
2481  int searchK,
2482  float searchRadius,
2483  const Eigen::Vector3f & viewPoint)
2484 {
2485  return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
2486 }
2487 
2488 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
2489  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2490  float maxDepthChangeFactor,
2491  float normalSmoothingSize,
2492  const Eigen::Vector3f & viewPoint)
2493 {
2494  pcl::IndicesPtr indices(new std::vector<int>);
2495  return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
2496 }
2497 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
2498  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2499  const pcl::IndicesPtr & indices,
2500  float maxDepthChangeFactor,
2501  float normalSmoothingSize,
2502  const Eigen::Vector3f & viewPoint)
2503 {
2504  UASSERT(cloud->isOrganized());
2505 
2506  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
2507  if(indices->size())
2508  {
2509  tree->setInputCloud(cloud, indices);
2510  }
2511  else
2512  {
2513  tree->setInputCloud (cloud);
2514  }
2515 
2516  // Normal estimation
2517  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
2518  pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
2519  ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
2520  ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
2521  ne.setNormalSmoothingSize(normalSmoothingSize);
2522  ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
2523  ne.setInputCloud(cloud);
2524  // Commented: Keep the output normals size the same as the input cloud
2525  //if(indices->size())
2526  //{
2527  // ne.setIndices(indices);
2528  //}
2529  ne.setSearchMethod(tree);
2530  ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
2531  ne.compute(*normals);
2532 
2533  return normals;
2534 }
2535 
2536 float computeNormalsComplexity(
2537  const LaserScan & scan,
2538  cv::Mat * pcaEigenVectors,
2539  cv::Mat * pcaEigenValues)
2540 {
2541  if(!scan.isEmpty() && (scan.hasNormals()))
2542  {
2543  //Construct a buffer used by the pca analysis
2544  int sz = static_cast<int>(scan.size()*2);
2545  bool is2d = scan.is2d();
2546  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
2547  int oi = 0;
2548  int nOffset = 0;
2549  if(!scan.is2d())
2550  {
2551  nOffset+=1;
2552  }
2553  if(scan.hasIntensity() || scan.hasRGB())
2554  {
2555  nOffset+=1;
2556  }
2557  for (int i = 0; i < scan.size(); ++i)
2558  {
2559  const float * ptrScan = scan.data().ptr<float>(0, i);
2560 
2561  if(is2d)
2562  {
2563  if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]))
2564  {
2565  float * ptr = data_normals.ptr<float>(oi++, 0);
2566  ptr[0] = ptrScan[2];
2567  ptr[1] = ptrScan[3];
2568  }
2569  }
2570  else
2571  {
2572  if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]) && uIsFinite(ptrScan[nOffset+4]))
2573  {
2574  float * ptr = data_normals.ptr<float>(oi++, 0);
2575  ptr[0] = ptrScan[3];
2576  ptr[1] = ptrScan[4];
2577  ptr[2] = ptrScan[5];
2578  }
2579  }
2580  }
2581  if(oi>1)
2582  {
2583  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
2584 
2585  if(pcaEigenVectors)
2586  {
2587  *pcaEigenVectors = pca_analysis.eigenvectors;
2588  }
2589  if(pcaEigenValues)
2590  {
2591  *pcaEigenValues = pca_analysis.eigenvalues;
2592  }
2593 
2594  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
2595  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
2596  }
2597  }
2598  else if(!scan.isEmpty())
2599  {
2600  UERROR("Scan doesn't have normals!");
2601  }
2602  return 0.0f;
2603 }
2604 
2605 float computeNormalsComplexity(
2606  const pcl::PointCloud<pcl::PointNormal> & cloud,
2607  bool is2d,
2608  cv::Mat * pcaEigenVectors,
2609  cv::Mat * pcaEigenValues)
2610 {
2611  //Construct a buffer used by the pca analysis
2612  int sz = static_cast<int>(cloud.size()*2);
2613  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
2614  int oi = 0;
2615  for (unsigned int i = 0; i < cloud.size(); ++i)
2616  {
2617  const pcl::PointNormal & pt = cloud.at(i);
2618  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
2619  {
2620  float * ptr = data_normals.ptr<float>(oi++, 0);
2621  ptr[0] = pt.normal_x;
2622  ptr[1] = pt.normal_y;
2623  if(!is2d)
2624  {
2625  ptr[2] = pt.normal_z;
2626  }
2627  }
2628  }
2629  if(oi>1)
2630  {
2631  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
2632 
2633  if(pcaEigenVectors)
2634  {
2635  *pcaEigenVectors = pca_analysis.eigenvectors;
2636  }
2637  if(pcaEigenValues)
2638  {
2639  *pcaEigenValues = pca_analysis.eigenvalues;
2640  }
2641 
2642  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
2643  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
2644  }
2645  return 0.0f;
2646 }
2647 
2648 float computeNormalsComplexity(
2649  const pcl::PointCloud<pcl::Normal> & normals,
2650  bool is2d,
2651  cv::Mat * pcaEigenVectors,
2652  cv::Mat * pcaEigenValues)
2653 {
2654  //Construct a buffer used by the pca analysis
2655  int sz = static_cast<int>(normals.size()*2);
2656  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
2657  int oi = 0;
2658  for (unsigned int i = 0; i < normals.size(); ++i)
2659  {
2660  const pcl::Normal & pt = normals.at(i);
2661  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
2662  {
2663  float * ptr = data_normals.ptr<float>(oi++, 0);
2664  ptr[0] = pt.normal_x;
2665  ptr[1] = pt.normal_y;
2666  if(!is2d)
2667  {
2668  ptr[2] = pt.normal_z;
2669  }
2670  }
2671  }
2672  if(oi>1)
2673  {
2674  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
2675 
2676  if(pcaEigenVectors)
2677  {
2678  *pcaEigenVectors = pca_analysis.eigenvectors;
2679  }
2680  if(pcaEigenValues)
2681  {
2682  *pcaEigenValues = pca_analysis.eigenvalues;
2683  }
2684 
2685  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
2686  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
2687  }
2688  return 0.0f;
2689 }
2690 
2691 float computeNormalsComplexity(
2692  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
2693  bool is2d,
2694  cv::Mat * pcaEigenVectors,
2695  cv::Mat * pcaEigenValues)
2696 {
2697  //Construct a buffer used by the pca analysis
2698  int sz = static_cast<int>(cloud.size()*2);
2699  cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
2700  int oi = 0;
2701  for (unsigned int i = 0; i < cloud.size(); ++i)
2702  {
2703  const pcl::PointXYZRGBNormal & pt = cloud.at(i);
2704  if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
2705  {
2706  float * ptr = data_normals.ptr<float>(oi++, 0);
2707  ptr[0] = pt.normal_x;
2708  ptr[1] = pt.normal_y;
2709  if(!is2d)
2710  {
2711  ptr[2] = pt.normal_z;
2712  }
2713  }
2714  }
2715  if(oi>1)
2716  {
2717  cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
2718 
2719  if(pcaEigenVectors)
2720  {
2721  *pcaEigenVectors = pca_analysis.eigenvectors;
2722  }
2723  if(pcaEigenValues)
2724  {
2725  *pcaEigenValues = pca_analysis.eigenvalues;
2726  }
2727 
2728  // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
2729  return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
2730  }
2731  return 0.0f;
2732 }
2733 
2734 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
2735  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2736  float searchRadius,
2737  int polygonialOrder,
2738  int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
2739  float upsamplingRadius, // SAMPLE_LOCAL_PLANE
2740  float upsamplingStep, // SAMPLE_LOCAL_PLANE
2741  int pointDensity, // RANDOM_UNIFORM_DENSITY
2742  float dilationVoxelSize, // VOXEL_GRID_DILATION
2743  int dilationIterations) // VOXEL_GRID_DILATION
2744 {
2745  pcl::IndicesPtr indices(new std::vector<int>);
2746  return mls(cloud,
2747  indices,
2748  searchRadius,
2749  polygonialOrder,
2750  upsamplingMethod,
2751  upsamplingRadius,
2752  upsamplingStep,
2753  pointDensity,
2754  dilationVoxelSize,
2755  dilationIterations);
2756 }
2757 
2758 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
2759  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
2760  const pcl::IndicesPtr & indices,
2761  float searchRadius,
2762  int polygonialOrder,
2763  int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
2764  float upsamplingRadius, // SAMPLE_LOCAL_PLANE
2765  float upsamplingStep, // SAMPLE_LOCAL_PLANE
2766  int pointDensity, // RANDOM_UNIFORM_DENSITY
2767  float dilationVoxelSize, // VOXEL_GRID_DILATION
2768  int dilationIterations) // VOXEL_GRID_DILATION
2769 {
2770  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2771  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
2772  if(indices->size())
2773  {
2774  tree->setInputCloud (cloud, indices);
2775  }
2776  else
2777  {
2778  tree->setInputCloud (cloud);
2779  }
2780 
2781  // Init object (second point type is for the normals)
2782  pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
2783 
2784  // Set parameters
2785  mls.setComputeNormals (true);
2786  if(polygonialOrder > 0)
2787  {
2788  mls.setPolynomialFit (true);
2789  mls.setPolynomialOrder(polygonialOrder);
2790  }
2791  else
2792  {
2793  mls.setPolynomialFit (false);
2794  }
2795  UASSERT(upsamplingMethod >= mls.NONE &&
2796  upsamplingMethod <= mls.VOXEL_GRID_DILATION);
2797  mls.setUpsamplingMethod((pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::UpsamplingMethod)upsamplingMethod);
2798  mls.setSearchRadius(searchRadius);
2799  mls.setUpsamplingRadius(upsamplingRadius);
2800  mls.setUpsamplingStepSize(upsamplingStep);
2801  mls.setPointDensity(pointDensity);
2802  mls.setDilationVoxelSize(dilationVoxelSize);
2803  mls.setDilationIterations(dilationIterations);
2804 
2805  // Reconstruct
2806  mls.setInputCloud (cloud);
2807  if(indices->size())
2808  {
2809  mls.setIndices(indices);
2810  }
2811  mls.setSearchMethod (tree);
2812  mls.process (*cloud_with_normals);
2813 
2814  // It seems that returned normals are not normalized!? FIXME: Is it a bug only in PCL 1.7.1?
2815  for(unsigned int i=0; i<cloud_with_normals->size(); ++i)
2816  {
2817  Eigen::Vector3f normal(cloud_with_normals->at(i).normal_x, cloud_with_normals->at(i).normal_y, cloud_with_normals->at(i).normal_z);
2818  normal.normalize();
2819  cloud_with_normals->at(i).normal_x = normal[0];
2820  cloud_with_normals->at(i).normal_y = normal[1];
2821  cloud_with_normals->at(i).normal_z = normal[2];
2822  }
2823 
2824  return cloud_with_normals;
2825 }
2826 
2827 LaserScan adjustNormalsToViewPoint(
2828  const LaserScan & scan,
2829  const Eigen::Vector3f & viewpoint,
2830  bool forceGroundNormalsUp)
2831 {
2832  if(scan.size() && !scan.is2d() && scan.hasNormals())
2833  {
2834  int nx = scan.getNormalsOffset();
2835  int ny = nx+1;
2836  int nz = ny+1;
2837  cv::Mat output = scan.data().clone();
2838  for(int i=0; i<scan.size(); ++i)
2839  {
2840  float * ptr = output.ptr<float>(0, i);
2841  if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
2842  {
2843  Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
2844  Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
2845 
2846  float result = v.dot(n);
2847  if(result < 0
2848  || (forceGroundNormalsUp && ptr[nz] < -0.8 && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
2849  {
2850  //reverse normal
2851  ptr[nx] *= -1.0f;
2852  ptr[ny] *= -1.0f;
2853  ptr[nz] *= -1.0f;
2854  }
2855  }
2856  }
2857  return LaserScan(output, scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
2858  }
2859  return scan;
2860 }
2861 
2862 void adjustNormalsToViewPoint(
2863  pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
2864  const Eigen::Vector3f & viewpoint,
2865  bool forceGroundNormalsUp)
2866 {
2867  for(unsigned int i=0; i<cloud->size(); ++i)
2868  {
2869  pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
2870  if(pcl::isFinite(normal))
2871  {
2872  Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
2873  Eigen::Vector3f n(normal.x, normal.y, normal.z);
2874 
2875  float result = v.dot(n);
2876  if(result < 0
2877  || (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
2878  {
2879  //reverse normal
2880  cloud->points[i].normal_x *= -1.0f;
2881  cloud->points[i].normal_y *= -1.0f;
2882  cloud->points[i].normal_z *= -1.0f;
2883  }
2884  }
2885  }
2886 }
2887 
2888 void adjustNormalsToViewPoint(
2889  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
2890  const Eigen::Vector3f & viewpoint,
2891  bool forceGroundNormalsUp)
2892 {
2893  for(unsigned int i=0; i<cloud->size(); ++i)
2894  {
2895  pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
2896  if(pcl::isFinite(normal))
2897  {
2898  Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
2899  Eigen::Vector3f n(normal.x, normal.y, normal.z);
2900 
2901  float result = v.dot(n);
2902  if(result < 0
2903  || (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
2904  {
2905  //reverse normal
2906  cloud->points[i].normal_x *= -1.0f;
2907  cloud->points[i].normal_y *= -1.0f;
2908  cloud->points[i].normal_z *= -1.0f;
2909  }
2910  }
2911  }
2912 }
2913 
2914 void adjustNormalsToViewPoints(
2915  const std::map<int, Transform> & poses,
2916  const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
2917  const std::vector<int> & rawCameraIndices,
2918  pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
2919 {
2920  if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
2921  {
2922  pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
2923  rawTree->setInputCloud (rawCloud);
2924 
2925  for(unsigned int i=0; i<cloud->size(); ++i)
2926  {
2927  pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
2928  if(pcl::isFinite(normal))
2929  {
2930  std::vector<int> indices;
2931  std::vector<float> dist;
2932  rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
2933  UASSERT(indices.size() == 1);
2934  if(indices.size() && indices[0]>=0)
2935  {
2936  Transform p = poses.at(rawCameraIndices[indices[0]]);
2937  pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
2938  Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
2939 
2940  Eigen::Vector3f n(normal.x, normal.y, normal.z);
2941 
2942  float result = v.dot(n);
2943  if(result < 0)
2944  {
2945  //reverse normal
2946  cloud->points[i].normal_x *= -1.0f;
2947  cloud->points[i].normal_y *= -1.0f;
2948  cloud->points[i].normal_z *= -1.0f;
2949  }
2950  }
2951  else
2952  {
2953  UWARN("Not found camera viewpoint for point %d", i);
2954  }
2955  }
2956  }
2957  }
2958 }
2959 
2960 void adjustNormalsToViewPoints(
2961  const std::map<int, Transform> & poses,
2962  const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
2963  const std::vector<int> & rawCameraIndices,
2964  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
2965 {
2966  UASSERT(rawCloud.get() && cloud.get());
2967  UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)poses.size(), (int)rawCloud->size(), (int)rawCameraIndices.size(), (int)cloud->size());
2968  if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
2969  {
2970  pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
2971  rawTree->setInputCloud (rawCloud);
2972  for(unsigned int i=0; i<cloud->size(); ++i)
2973  {
2974  pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
2975  if(pcl::isFinite(normal))
2976  {
2977  std::vector<int> indices;
2978  std::vector<float> dist;
2979  rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
2980  if(indices.size() && indices[0]>=0)
2981  {
2982  UASSERT_MSG(indices[0]<(int)rawCameraIndices.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)rawCameraIndices.size()).c_str());
2983  UASSERT(uContains(poses, rawCameraIndices[indices[0]]));
2984  Transform p = poses.at(rawCameraIndices[indices[0]]);
2985  pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
2986  Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
2987 
2988  Eigen::Vector3f n(normal.x, normal.y, normal.z);
2989 
2990  float result = v.dot(n);
2991  if(result < 0)
2992  {
2993  //reverse normal
2994  cloud->points[i].normal_x *= -1.0f;
2995  cloud->points[i].normal_y *= -1.0f;
2996  cloud->points[i].normal_z *= -1.0f;
2997  }
2998  }
2999  else
3000  {
3001  UWARN("Not found camera viewpoint for point %d!?", i);
3002  }
3003  }
3004  }
3005  }
3006 }
3007 
3008 pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
3009 {
3010  pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
3011 #ifndef DISABLE_VTK
3012  pcl::MeshQuadricDecimationVTK mqd;
3013  mqd.setTargetReductionFactor(factor);
3014  mqd.setInputMesh(mesh);
3015  mqd.process (*output);
3016 #else
3017  UWARN("RTAB-Map is not built with VTK module so mesh decimation cannot be used!");
3018  *output = *mesh;
3019 #endif
3020  return output;
3021 }
3022 
3023 }
3024 
3025 }
uIsInteger
bool uIsInteger(const std::string &str, bool checkForSign=true)
Definition: UStl.h:632
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:118
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
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:400
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:3035
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:113
pcl::texture_mapping::Camera::height
double height
Definition: texture_mapping.h:77
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)
Definition: util3d_surface.cpp:1412
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:63
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:656
epsilon
double epsilon
UTimer
Definition: UTimer.h:46
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:984
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:2286
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:112
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
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:2359
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
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)
Definition: util3d_surface.cpp:656
f
f
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
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::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)
Definition: texture_mapping.hpp:1053
pcl::texture_mapping::CameraVector
std::vector< Camera, Eigen::aligned_allocator< Camera > > CameraVector
Definition: texture_mapping.h:95
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:443
rtabmap::maxDepthError
const float maxDepthError
Definition: CameraTango.cpp:42
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=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2399
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:2192
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:320
pcl::TextureMapping
The texture mapping algorithm.
Definition: texture_mapping.h:104
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
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:74
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:72
tango_gl::vertices
static const float vertices[]
Definition: quad.cpp:39
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:642
dst
char * dst
Definition: lz4.h:354
pcl::texture_mapping::Camera::texture_file
std::string texture_file
Definition: texture_mapping.h:79
pcl::TextureMapping::setMinClusterSize
void setMinClusterSize(int size)
Definition: texture_mapping.h:192
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:2488
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:64
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:81
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
UMath.h
Basic mathematics functions.
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
UConversion.h
Some conversion functions.
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
rtabmap::util3d::gcd
int gcd(int a, int b)
Definition: util3d_surface.cpp:1049
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:2377
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:2471
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
rtabmap::util3d::convertPolygonsFromPCL
std::vector< std::vector< unsigned int > > RTABMAP_EXP convertPolygonsFromPCL(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:1201
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
pcl::texture_mapping::Camera::roi
std::vector< double > roi
Definition: texture_mapping.h:80
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
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:2734
rtabmap::util3d::sqr
double sqr(uchar v)
Definition: util3d_surface.cpp:1407
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:583
UASSERT
#define UASSERT(condition)
pcl::texture_mapping::Camera
Structure to store camera pose and focal length.
Definition: texture_mapping.h:67
rtabmap::ProgressState::callback
virtual bool callback(const std::string &msg) const
Definition: ProgressState.h:39
rtabmap::util3d::convertPolygonsToPCL
std::vector< pcl::Vertices > RTABMAP_EXP convertPolygonsToPCL(const std::vector< std::vector< unsigned int > > &polygons)
Definition: util3d_surface.cpp:1223
texture_mapping.h
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
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:116
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:3008
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:268
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
util3d_filtering.h
rtabmap::util2d::brightnessAndContrastAuto
cv::Mat RTABMAP_EXP brightnessAndContrastAuto(const cv::Mat &src, const cv::Mat &mask, float clipLowHistPercent=0, float clipHighHistPercent=0)
Automatic brightness and contrast optimization with optional histogram clipping.
Definition: util2d.cpp:1937
glm::pi
GLM_FUNC_DECL genType pi()
pcl::texture_mapping::Camera::width
double width
Definition: texture_mapping.h:78
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, StereoCameraModel &stereoModel) const
Definition: DBDriver.cpp:677
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::util3d::assembleTextureMesh
pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(const cv::Mat &cloudMat, const std::vector< std::vector< std::vector< unsigned int > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, cv::Mat &textures, bool mergeTextures=false)
Definition: util3d_surface.cpp:1246
tree
tree
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
pcl::TextureMapping::setMaxDepthError
void setMaxDepthError(float maxDepthError)
Definition: texture_mapping.h:180
rtabmap::Memory::getSignatureDataConst
SensorData getSignatureDataConst(int locationId, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: Memory.cpp:3530
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:540
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:153
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:1053
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::util2d::exposureFusion
cv::Mat RTABMAP_EXP exposureFusion(const std::vector< cv::Mat > &images)
Definition: util2d.cpp:2012
pcl::texture_mapping::Camera::depth
cv::Mat depth
Definition: texture_mapping.h:81
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)
Definition: util3d_surface.cpp:2914
rtabmap::util3d::fixTextureMeshForVisualization
void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh &textureMesh)
Definition: util3d_surface.cpp:2093
rtabmap::util3d::computeNormalsComplexity
float RTABMAP_EXP computeNormalsComplexity(const LaserScan &scan, cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
Definition: util3d_surface.cpp:2536
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::util3d::filterInvalidPolygons
std::vector< pcl::Vertices > RTABMAP_EXP filterInvalidPolygons(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:516
UDEBUG
#define UDEBUG(...)
rtabmap::util3d::cleanTextureMesh
void RTABMAP_EXP cleanTextureMesh(pcl::TextureMesh &textureMesh, int minClusterSize)
Definition: util3d_surface.cpp:851
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
rtabmap::util3d::adjustNormalsToViewPoint
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint, bool forceGroundNormalsUp)
Definition: util3d_surface.cpp:2827
rtabmap::ProgressState::isCanceled
bool isCanceled() const
Definition: ProgressState.h:51
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:79
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
pcl::TextureMapping::setMaxAngle
void setMaxAngle(float maxAngle)
Definition: texture_mapping.h:186
rtabmap::util3d::removeNaNNormalsFromPointCloud
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
Definition: util3d_filtering.cpp:763
pcl::TextureMapping::setMaxDistance
void setMaxDistance(float maxDistance)
Definition: texture_mapping.h:174
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:2381
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:320
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:84
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:114
pcl::texture_mapping::Camera::pose
Eigen::Affine3f pose
Definition: texture_mapping.h:71
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2363
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
rtabmap::uncompressImage
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat &bytes)
Definition: Compression.cpp:130
util3d_surface.h
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< unsigned int > > &polygons)
Definition: util3d_surface.cpp:1369

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41