Main MRPT website > C++ reference for MRPT 1.9.9
Plane.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a
13  * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
14  */
15 
16 #include "pbmap-precomp.h" // Precompiled headers
17 
18 #if MRPT_HAS_PCL
19 
20 #include <mrpt/pbmap/Plane.h>
21 #include <mrpt/pbmap/Miscellaneous.h>
22 #include <pcl/common/time.h>
23 #include <pcl/filters/voxel_grid.h>
24 #include <cassert>
25 
26 using namespace mrpt::pbmap;
27 using namespace mrpt::utils;
28 using namespace std;
29 
30 IMPLEMENTS_SERIALIZABLE(Plane, CSerializable, mrpt::pbmap)
31 
32 ///*---------------------------------------------------------------
33 // writeToStream
34 // ---------------------------------------------------------------*/
35 // void Plane::writeToStream(mrpt::utils::CStream &out, int *out_Version) const
36 //{
37 // if (out_Version)
38 // *out_Version = 0;
39 // else
40 // {
41 // cout << "write plane \n";
42 // // The data
43 // out << static_cast<uint32_t>(numObservations);//out <<
44 // uint32_t(numObservations);
45 // out << areaVoxels;
46 // out << areaHull;
47 // out << elongation;
48 // out << v3normal(0) << v3normal(1) << v3normal(2);
49 // out << v3center(0) << v3center(1) << v3center(2);
50 // out << v3PpalDir(0) << v3PpalDir(1) << v3PpalDir(2);
51 // out << v3colorNrgb(0) << v3colorNrgb(1) << v3colorNrgb(2);
52 // out << v3colorNrgbDev(0) << v3colorNrgbDev(1) << v3colorNrgbDev(2);
53 //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal(0),3);
54 //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center(0),3);
55 //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir(0),3);
56 //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb(0),3);
57 ////
58 /// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev(0),3);
59 //
60 // out << (uint32_t)neighborPlanes.size();
61 // for (std::map<unsigned,unsigned>::const_iterator
62 // it=neighborPlanes.begin(); it != neighborPlanes.end(); it++)
63 // out << static_cast<uint32_t>(it->first) <<
64 // static_cast<uint32_t>(it->second);
65 //
66 // out << (uint32_t)polygonContourPtr->size();
67 // for (uint32_t i=0; i < polygonContourPtr->size(); i++)
68 // out << polygonContourPtr->points[i].x << polygonContourPtr->points[i].y
69 // << polygonContourPtr->points[i].z;
70 //
71 // out << bFullExtent;
72 // out << bFromStructure;
73 // }
74 //
75 //}
76 //
77 ///*---------------------------------------------------------------
78 // readFromStream
79 // ---------------------------------------------------------------*/
80 // void Plane::readFromStream(mrpt::utils::CStream &in, int version)
81 //{
82 // switch(version)
83 // {
84 // case 0:
85 // {
86 // cout << "Read plane\n";
87 //
88 // // The data
89 // uint32_t n;
90 // in >> n;
91 // numObservations = (unsigned)n;
92 // in >> areaVoxels;
93 // in >> areaHull;
94 // in >> elongation;
95 // in >> v3normal(0) >> v3normal(1) >> v3normal(2);
96 // in >> v3center(0) >> v3center(1) >> v3center(2);
97 // in >> v3PpalDir(0) >> v3PpalDir(1) >> v3PpalDir(2);
98 // in >> v3colorNrgb(0) >> v3colorNrgb(1) >> v3colorNrgb(2);
99 // in >> v3colorNrgbDev(0) >> v3colorNrgbDev(1) >> v3colorNrgbDev(2);
100 ////
101 /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal[0],sizeof(v3normal[0])*3);
102 ////
103 /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center[0],sizeof(v3center[0])*3);
104 ////
105 /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir[0],sizeof(v3PpalDir[0])*3);
106 ////
107 /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb[0],sizeof(v3colorNrgb[0])*3);
108 ////
109 /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev[0],sizeof(v3colorNrgbDev[0])*3);
110 //
111 // in >> n;
112 // neighborPlanes.clear();
113 // for (uint32_t i=0; i < n; i++)
114 // {
115 // uint32_t neighbor, commonObs;
116 // in >> neighbor >> commonObs;
117 // neighborPlanes[neighbor] = commonObs;
118 // }
119 //
120 // in >> n;
121 // polygonContourPtr->resize(n);
122 // for (unsigned i=0; i < n; i++)
123 // in >> polygonContourPtr->points[i].x >> polygonContourPtr->points[i].y
124 // >> polygonContourPtr->points[i].z;
125 //
126 // in >> bFullExtent;
127 // in >> bFromStructure;
128 //
129 // } break;
130 // default:
131 // MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
132 // };
133 //}
134 
135 /*---------------------------------------------------------------
136  writeToStream
137  ---------------------------------------------------------------*/
138 void Plane::writeToStream(mrpt::utils::CStream& out, int* out_Version) const
139 {
140  // cout << "Write plane. Version " << *out_Version << endl;
141  if (out_Version) *out_Version = 0;
142  // else //if(*out_Version == 0)
143  // {
144  // // The data
145  // out << static_cast<uint32_t>(numObservations);//out <<
146  // uint32_t(numObservations);
147  // out << areaVoxels;
148  // out << areaHull;
149  // out << elongation;
150  // out << v3normal(0) << v3normal(1) << v3normal(2);
151  // out << v3center(0) << v3center(1) << v3center(2);
152  // out << v3PpalDir(0) << v3PpalDir(1) << v3PpalDir(2);
153  // out << v3colorNrgb(0) << v3colorNrgb(1) << v3colorNrgb(2);
154  // out << v3colorNrgbDev(0) << v3colorNrgbDev(1) << v3colorNrgbDev(2);
155  //
156  //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal(0),3);
157  //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center(0),3);
158  //// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir(0),3);
159  ////
160  /// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb(0),3);
161  ////
162  /// out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev(0),3);
163  //
164  // out << (uint32_t)neighborPlanes.size();
165  // for (std::map<unsigned,unsigned>::const_iterator
166  // it=neighborPlanes.begin(); it != neighborPlanes.end(); it++)
167  // out << static_cast<uint32_t>(it->first) <<
168  // static_cast<uint32_t>(it->second);
169  //
170  // out << (uint32_t)polygonContourPtr->size();
171  // for (uint32_t i=0; i < polygonContourPtr->size(); i++)
172  // out << polygonContourPtr->points[i].x <<
173  // polygonContourPtr->points[i].y << polygonContourPtr->points[i].z;
174  //
175  // out << bFullExtent;
176  // out << bFromStructure;
177  ////cout << "Write plane\n";
178  // }
179  // else if(*out_Version == 1)
180  else
181  {
182  // The data
183  // out << static_cast<uint32_t>(numObservations);//out <<
184  // uint32_t(numObservations);
185  // out << areaVoxels;
186  out << areaHull;
187  out << elongation;
188  out << curvature;
189  out << v3normal(0) << v3normal(1) << v3normal(2);
190  out << v3center(0) << v3center(1) << v3center(2);
191  out << v3PpalDir(0) << v3PpalDir(1) << v3PpalDir(2);
192  out << v3colorNrgb(0) << v3colorNrgb(1) << v3colorNrgb(2);
193  //// out << v3colorNrgbDev(0) << v3colorNrgbDev(1) <<
194  /// v3colorNrgbDev(2);
195  out << dominantIntensity;
196  out << bDominantColor;
197 
198  out << hist_H;
199  // cout << "color " << hist_H.size() << endl;
200  // for (size_t i=0; i < 74; i++){//cout << hist_H[i] << " ";
201  // out << hist_H[i];}
202 
203  out << inliers;
204 
205  out << label;
206  out << label_object;
207  out << label_context;
208 
209  // out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal(0),3);
210  // out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center(0),3);
211  // out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir(0),3);
212  // out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb(0),3);
213  // out.WriteBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev(0),3);
214 
215  // cout << "write neighbors " << neighborPlanes.size() << endl;
216 
217  // out << (uint32_t)neighborPlanes.size();
218  // for (std::map<unsigned,unsigned>::const_iterator
219  // it=neighborPlanes.begin(); it != neighborPlanes.end(); it++)
220  // out << static_cast<uint32_t>(it->first) <<
221  // static_cast<uint32_t>(it->second);
222 
223  out << (uint32_t)polygonContourPtr->size();
224  for (uint32_t i = 0; i < polygonContourPtr->size(); i++)
225  out << polygonContourPtr->points[i].x
226  << polygonContourPtr->points[i].y
227  << polygonContourPtr->points[i].z;
228 
229  // out << bFullExtent;
230  // out << bFromStructure;
231  }
232 }
233 
234 /*---------------------------------------------------------------
235  readFromStream
236  ---------------------------------------------------------------*/
237 void Plane::readFromStream(mrpt::utils::CStream& in, int version)
238 {
239  switch (version)
240  {
241  case 0:
242  // {
243  // // The data
244  // uint32_t n;
245  // in >> n;
246  // numObservations = (unsigned)n;
247  // in >> areaVoxels;
248  // in >> areaHull;
249  // in >> elongation;
250  // in >> v3normal(0) >> v3normal(1) >> v3normal(2);
251  // in >> v3center(0) >> v3center(1) >> v3center(2);
252  // in >> v3PpalDir(0) >> v3PpalDir(1) >> v3PpalDir(2);
253  // in >> v3colorNrgb(0) >> v3colorNrgb(1) >>
254  // v3colorNrgb(2);
255  // in >> v3colorNrgbDev(0) >> v3colorNrgbDev(1) >>
256  // v3colorNrgbDev(2);
257  ////
258  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal[0],sizeof(v3normal[0])*3);
259  ////
260  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center[0],sizeof(v3center[0])*3);
261  ////
262  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir[0],sizeof(v3PpalDir[0])*3);
263  ////
264  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb[0],sizeof(v3colorNrgb[0])*3);
265  ////
266  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev[0],sizeof(v3colorNrgbDev[0])*3);
267  //
268  // in >> n;
269  // neighborPlanes.clear();
270  // for (uint32_t i=0; i < n; i++)
271  // {
272  // uint32_t neighbor, commonObs;
273  // in >> neighbor >> commonObs;
274  // neighborPlanes[neighbor] = commonObs;
275  // }
276  //
277  // in >> n;
278  // polygonContourPtr->resize(n);
279  // for (unsigned i=0; i < n; i++)
280  // in >> polygonContourPtr->points[i].x >>
281  // polygonContourPtr->points[i].y >>
282  // polygonContourPtr->points[i].z;
283  //
284  // in >> bFullExtent;
285  // in >> bFromStructure;
286  //
287  // } break;
288  //// case 1:
289  {
290  // // The data
291  uint32_t n;
292  // in >> n;
293  // numObservations = (unsigned)n;
294  // in >> areaVoxels;
295  in >> areaHull;
296  in >> elongation;
297  in >> curvature;
298  in >> v3normal(0) >> v3normal(1) >> v3normal(2);
299  in >> v3center(0) >> v3center(1) >> v3center(2);
300  in >> v3PpalDir(0) >> v3PpalDir(1) >> v3PpalDir(2);
301  d = -v3normal.dot(v3center);
302  in >> v3colorNrgb(0) >> v3colorNrgb(1) >> v3colorNrgb(2);
303  //// in >> v3colorNrgbDev(0) >> v3colorNrgbDev(1) >>
304  /// v3colorNrgbDev(2);
305  in >> dominantIntensity;
306  in >> bDominantColor;
307 
308  in >> hist_H;
309  // hist_H.resize(74);
310  // for (size_t i=0; i < 74; i++)
311  // in >> hist_H[i];
312 
313  in >> inliers;
314 
315  in >> label;
316  in >> label_object;
317  in >> label_context;
318  ////cout << "Read Nrgb color \n";// << v3colorNrgb.transpose()
319  ////
320  ////cout << "Read color histogram\n";
321  ////
322  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3normal[0],sizeof(v3normal[0])*3);
323  ////
324  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3center[0],sizeof(v3center[0])*3);
325  ////
326  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3PpalDir[0],sizeof(v3PpalDir[0])*3);
327  ////
328  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgb[0],sizeof(v3colorNrgb[0])*3);
329  ////
330  /// in.ReadBufferFixEndianness<Eigen::Vector3f::Scalar>(&v3colorNrgbDev[0],sizeof(v3colorNrgbDev[0])*3);
331  //
332  // in >> n;
333  // neighborPlanes.clear();
334  // for (uint32_t i=0; i < n; i++)
335  // {
336  // uint32_t neighbor, commonObs;
337  // in >> neighbor >> commonObs;
338  // neighborPlanes[neighbor] = commonObs;
339  // }
340 
341  in >> n;
342  // cout << "neighbors " << n << endl;
343  polygonContourPtr->resize(n);
344  for (unsigned i = 0; i < n; i++)
345  in >> polygonContourPtr->points[i].x >>
346  polygonContourPtr->points[i].y >>
347  polygonContourPtr->points[i].z;
348 
349  // in >> bFullExtent;
350  // in >> bFromStructure;
351  }
352  break;
353  default:
354  MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
355  };
356 }
357 
358 /*!
359  * Force the plane inliers to lay on the plane
360  */
361 void Plane::forcePtsLayOnPlane()
362 {
363  // The plane equation has the form Ax + By + Cz + D = 0, where the vector
364  // N=(A,B,C) is the normal and the constant D can be calculated as D =
365  // -N*(PlanePoint) = -N*PlaneCenter
366  const double D = -(v3normal.dot(v3center));
367  for (unsigned i = 0; i < planePointCloudPtr->size(); i++)
368  {
369  double dist = v3normal[0] * planePointCloudPtr->points[i].x +
370  v3normal[1] * planePointCloudPtr->points[i].y +
371  v3normal[2] * planePointCloudPtr->points[i].z + D;
372  planePointCloudPtr->points[i].x -= v3normal[0] * dist;
373  planePointCloudPtr->points[i].y -= v3normal[1] * dist;
374  planePointCloudPtr->points[i].z -= v3normal[2] * dist;
375  }
376  // Do the same with the points defining the convex hull
377  for (unsigned i = 0; i < polygonContourPtr->size(); i++)
378  {
379  double dist = v3normal[0] * polygonContourPtr->points[i].x +
380  v3normal[1] * polygonContourPtr->points[i].y +
381  v3normal[2] * polygonContourPtr->points[i].z + D;
382  polygonContourPtr->points[i].x -= v3normal[0] * dist;
383  polygonContourPtr->points[i].y -= v3normal[1] * dist;
384  polygonContourPtr->points[i].z -= v3normal[2] * dist;
385  }
386 }
387 
388 /** \brief Compute the area of a 2D planar polygon patch
389  */
390 float Plane::compute2DPolygonalArea()
391 {
392  int k0, k1, k2;
393 
394  // Find axis with largest normal component and project onto perpendicular
395  // plane
396  k0 = (fabs(v3normal[0]) > fabs(v3normal[1])) ? 0 : 1;
397  k0 = (fabs(v3normal[k0]) > fabs(v3normal[2])) ? k0 : 2;
398  k1 = (k0 + 1) % 3;
399  k2 = (k0 + 2) % 3;
400 
401  // cos(theta), where theta is the angle between the polygon and the
402  // projected plane
403  float ct = fabs(v3normal[k0]);
404  float AreaX2 = 0.0;
405  float p_i[3], p_j[3];
406 
407  for (unsigned int i = 0; i < polygonContourPtr->points.size(); i++)
408  {
409  p_i[0] = polygonContourPtr->points[i].x;
410  p_i[1] = polygonContourPtr->points[i].y;
411  p_i[2] = polygonContourPtr->points[i].z;
412  int j = (i + 1) % polygonContourPtr->points.size();
413  p_j[0] = polygonContourPtr->points[j].x;
414  p_j[1] = polygonContourPtr->points[j].y;
415  p_j[2] = polygonContourPtr->points[j].z;
416 
417  AreaX2 += p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
418  }
419  AreaX2 = fabs(AreaX2) / (2 * ct);
420 
421  return AreaX2;
422 }
423 
424 /** \brief Compute the patch's convex-hull area and mass center
425  */
426 void Plane::computeMassCenterAndArea()
427 {
428  int k0, k1, k2;
429 
430  // Find axis with largest normal component and project onto perpendicular
431  // plane
432  k0 = (fabs(v3normal[0]) > fabs(v3normal[1])) ? 0 : 1;
433  k0 = (fabs(v3normal[k0]) > fabs(v3normal[2])) ? k0 : 2;
434  k1 = (k0 + 1) % 3;
435  k2 = (k0 + 2) % 3;
436 
437  // cos(theta), where theta is the angle between the polygon and the
438  // projected plane
439  float ct = fabs(v3normal[k0]);
440  float AreaX2 = 0.0;
441  Eigen::Vector3f massCenter = Eigen::Vector3f::Zero();
442  float p_i[3], p_j[3];
443 
444  for (unsigned int i = 0; i < polygonContourPtr->points.size(); i++)
445  {
446  p_i[0] = polygonContourPtr->points[i].x;
447  p_i[1] = polygonContourPtr->points[i].y;
448  p_i[2] = polygonContourPtr->points[i].z;
449  int j = (i + 1) % polygonContourPtr->points.size();
450  p_j[0] = polygonContourPtr->points[j].x;
451  p_j[1] = polygonContourPtr->points[j].y;
452  p_j[2] = polygonContourPtr->points[j].z;
453  double cross_segment = p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
454 
455  AreaX2 += cross_segment;
456  massCenter[k1] += (p_i[k1] + p_j[k1]) * cross_segment;
457  massCenter[k2] += (p_i[k2] + p_j[k2]) * cross_segment;
458  }
459  areaHull = fabs(AreaX2) / (2 * ct);
460 
461  massCenter[k1] /= (3 * AreaX2);
462  massCenter[k2] /= (3 * AreaX2);
463  massCenter[k0] = (v3normal.dot(v3center) - v3normal[k1] * massCenter[k1] -
464  v3normal[k2] * massCenter[k2]) /
465  v3normal[k0];
466 
467  v3center = massCenter;
468 
469  d = -(v3normal.dot(v3center));
470 }
471 
472 void Plane::calcElongationAndPpalDir()
473 {
474  pcl::PCA<PointT> pca;
475  pca.setInputCloud(planePointCloudPtr);
476  Eigen::VectorXf eigenVal = pca.getEigenValues();
477  // if( eigenVal[0] > 2 * eigenVal[1] )
478  {
479  elongation = sqrt(eigenVal[0] / eigenVal[1]);
480  Eigen::MatrixXf eigenVect = pca.getEigenVectors();
481  // v3PpalDir = makeVector(eigenVect(0,0), eigenVect(1,0),
482  // eigenVect(2,0));
483  v3PpalDir[0] = eigenVect(0, 0);
484  v3PpalDir[1] = eigenVect(1, 0);
485  v3PpalDir[2] = eigenVect(2, 0);
486  }
487 }
488 
489 // The point cloud of the plane must be filled before using this function
490 void Plane::getPlaneNrgb()
491 {
492  // cout << "Plane::getPlaneNrgb() " << planePointCloudPtr->size() << endl;
493  assert(planePointCloudPtr->size() > 0);
494 
495  r.resize(planePointCloudPtr->size());
496  g.resize(planePointCloudPtr->size());
497  b.resize(planePointCloudPtr->size());
498  intensity.resize(planePointCloudPtr->size());
499 
500  size_t countPix = 0;
501  for (size_t i = 0; i < planePointCloudPtr->size(); i++)
502  {
503  float sumRGB = (float)planePointCloudPtr->points[i].r +
504  planePointCloudPtr->points[i].g +
505  planePointCloudPtr->points[i].b;
506  intensity[i] = sumRGB;
507  if (sumRGB != 0)
508  {
509  r[countPix] = planePointCloudPtr->points[i].r / sumRGB;
510  g[countPix] = planePointCloudPtr->points[i].g / sumRGB;
511  b[countPix] = planePointCloudPtr->points[i].b / sumRGB;
512  ++countPix;
513  }
514  }
515  r.resize(countPix);
516  g.resize(countPix);
517  b.resize(countPix);
518  intensity.resize(countPix);
519 }
520 
521 void Plane::getPlaneC1C2C3()
522 {
523  c1.resize(planePointCloudPtr->size());
524  c2.resize(planePointCloudPtr->size());
525  c3.resize(planePointCloudPtr->size());
526 
527  for (unsigned i = 0; i < planePointCloudPtr->size(); i++)
528  {
529  c1[i] = atan2(
530  (double)planePointCloudPtr->points[i].r,
531  (double)max(
532  planePointCloudPtr->points[i].g,
533  planePointCloudPtr->points[i].b));
534  c2[i] = atan2(
535  (double)planePointCloudPtr->points[i].g,
536  (double)max(
537  planePointCloudPtr->points[i].r,
538  planePointCloudPtr->points[i].b));
539  c3[i] = atan2(
540  (double)planePointCloudPtr->points[i].b,
541  (double)max(
542  planePointCloudPtr->points[i].r,
543  planePointCloudPtr->points[i].g));
544  }
545 }
546 
547 void Plane::calcPlaneHistH()
548 {
549  // cout << "Plane::calcPlaneHistH()\n";
550  float fR, fG, fB;
551  float fH, fS, fV;
552  const float FLOAT_TO_BYTE = 255.0f;
553  const float BYTE_TO_FLOAT = 1.0f / FLOAT_TO_BYTE;
554 
555  vector<int> H(74, 0);
556 
557  for (unsigned i = 0; i < planePointCloudPtr->size(); i++)
558  {
559  // Convert from 8-bit integers to floats.
560  fR = planePointCloudPtr->points[i].r * BYTE_TO_FLOAT;
561  fG = planePointCloudPtr->points[i].g * BYTE_TO_FLOAT;
562  fB = planePointCloudPtr->points[i].b * BYTE_TO_FLOAT;
563  // Convert from RGB to HSV, using float ranges 0.0 to 1.0.
564  float fDelta;
565  float fMin, fMax;
566  int iMax;
567  // Get the min and max, but use integer comparisons for slight speedup.
568  if (planePointCloudPtr->points[i].b < planePointCloudPtr->points[i].g)
569  {
570  if (planePointCloudPtr->points[i].b <
571  planePointCloudPtr->points[i].r)
572  {
573  fMin = fB;
574  if (planePointCloudPtr->points[i].r >
575  planePointCloudPtr->points[i].g)
576  {
577  iMax = planePointCloudPtr->points[i].r;
578  fMax = fR;
579  }
580  else
581  {
582  iMax = planePointCloudPtr->points[i].g;
583  fMax = fG;
584  }
585  }
586  else
587  {
588  fMin = fR;
589  fMax = fG;
590  iMax = planePointCloudPtr->points[i].g;
591  }
592  }
593  else
594  {
595  if (planePointCloudPtr->points[i].g <
596  planePointCloudPtr->points[i].r)
597  {
598  fMin = fG;
599  if (planePointCloudPtr->points[i].b >
600  planePointCloudPtr->points[i].r)
601  {
602  fMax = fB;
603  iMax = planePointCloudPtr->points[i].b;
604  }
605  else
606  {
607  fMax = fR;
608  iMax = planePointCloudPtr->points[i].r;
609  }
610  }
611  else
612  {
613  fMin = fR;
614  fMax = fB;
615  iMax = planePointCloudPtr->points[i].b;
616  }
617  }
618  fDelta = fMax - fMin;
619  fV = fMax; // Value (Brightness).
620  if (fV < 0.01)
621  {
622  // fH = 72; // Histogram has 72 bins for Hue values, and 2 bins
623  // more for black and white
624  H[72]++;
625  continue;
626  }
627  else
628  // if (iMax != 0)
629  { // Make sure its not pure black.
630  fS = fDelta / fMax; // Saturation.
631  if (fS < 0.2)
632  {
633  // fH = 73; // Histogram has 72 bins for Hue values, and
634  // 2 bins more for black and white
635  H[73]++;
636  continue;
637  }
638 
639  // float ANGLE_TO_UNIT = 1.0f / (6.0f * fDelta); // Make the
640  // Hues between 0.0 to 1.0 instead of 6.0
641  float ANGLE_TO_UNIT = 12.0f / fDelta; // 12 bins
642  if (iMax == planePointCloudPtr->points[i].r)
643  { // between yellow and magenta.
644  fH = (fG - fB) * ANGLE_TO_UNIT;
645  }
646  else if (iMax == planePointCloudPtr->points[i].g)
647  { // between cyan and yellow.
648  fH = (2.0f / 6.0f) + (fB - fR) * ANGLE_TO_UNIT;
649  }
650  else
651  { // between magenta and cyan.
652  fH = (4.0f / 6.0f) + (fR - fG) * ANGLE_TO_UNIT;
653  }
654  // Wrap outlier Hues around the circle.
655  if (fH < 0.0f) fH += 72.0f;
656  if (fH >= 72.0f) fH -= 72.0f;
657  }
658  // cout << "H " << fH << endl;
659  H[int(fH)]++;
660  }
661  // cout << "FInish histogram calculation\n";
662  // Normalize histogram
663  float numPixels = 0;
664  for (unsigned i = 0; i < H.size(); i++) numPixels += H[i];
665  hist_H.resize(74);
666  for (unsigned i = 0; i < H.size(); i++) hist_H[i] = H[i] / numPixels;
667 
668  // cout << "Got H histogram" << hist_H.size() << "\n";
669 }
670 
671 float concentrationThreshold = 0.5;
672 
673 void Plane::calcMainColor2()
674 {
675  assert(planePointCloudPtr->size() > 0);
676 
677  // double getColorStart, getColorEnd;
678  // getColorStart = pcl::getTime();
679  std::vector<Eigen::Vector4f> color(planePointCloudPtr->size());
680 
681  size_t countPix = 0;
682  size_t stepColor = std::max(
683  planePointCloudPtr->size() / 2000,
684  static_cast<size_t>(
685  1)); // Limit the main color calculation to 2000 pixels
686  for (size_t i = 0; i < planePointCloudPtr->size(); i += stepColor)
687  {
688  float sumRGB = (float)planePointCloudPtr->points[i].r +
689  planePointCloudPtr->points[i].g +
690  planePointCloudPtr->points[i].b;
691  if (sumRGB != 0)
692  {
693  color[countPix][0] = planePointCloudPtr->points[i].r / sumRGB;
694  color[countPix][1] = planePointCloudPtr->points[i].g / sumRGB;
695  color[countPix][2] = planePointCloudPtr->points[i].b / sumRGB;
696  color[countPix][3] = sumRGB;
697  // intensity[countPix] = sumRGB;
698  // cout << "color " << color[countPix][0] << " " <<
699  // color[countPix][1] << " " << color[countPix][2] << " " <<
700  // color[countPix][3] << endl;
701  ++countPix;
702  }
703  }
704  color.resize(countPix);
705  intensity.resize(countPix);
706 
707  float concentration, histStdDev;
708  // Eigen::Vector4f dominantColor;
709  Eigen::Vector4f dominantColor =
710  getMultiDimMeanShift_color(color, concentration, histStdDev);
711  v3colorNrgb = dominantColor.head(3);
712  dominantIntensity = dominantColor(3);
713 
714  if (concentration > concentrationThreshold)
715  bDominantColor = true;
716  else
717  bDominantColor = false;
718  // getColorEnd = pcl::getTime ();
719  // cout << "Nrgb2 in " << (getColorEnd - getColorStart)*1000000 << " us\n";
720  // cout << "colorNrgb " << v3colorNrgb[0] << " " << v3colorNrgb[1] << " " <<
721  // v3colorNrgb[2] << " " << v3colorNrgb[3] << endl;
722 
723  // getColorStart = pcl::getTime ();
724  //
725  // // c1c2c3
726  // getPlaneC1C2C3();
727  // v3colorC1C2C3[0] = getHistogramMeanShift(c1, (float)1.5708,
728  // v3colorNrgbDev[0]);
729  // v3colorC1C2C3[1] = getHistogramMeanShift(c2, (float)1.5708,
730  // v3colorNrgbDev[1]);
731  // v3colorC1C2C3[2] = getHistogramMeanShift(c3, (float)1.5708,
732  // v3colorNrgbDev[2]);
733  // getColorEnd = pcl::getTime ();
734  // cout << "c1c2c3 in " << (getColorEnd - getColorStart)*1000000 << " us\n";
735  //
736  // getColorStart = pcl::getTime ();
737  //
738  // // H histogram
739  // calcPlaneHistH();
740  // getColorEnd = pcl::getTime ();
741  // cout << "HistH in " << (getColorEnd - getColorStart)*1000000 << " us\n";
742 }
743 
744 void Plane::calcMainColor()
745 {
746  // Normalized rgb
747  // double getColorStart ,getColorEnd;
748  // getColorStart = pcl::getTime ();
749 
750  getPlaneNrgb();
751  // cout << "r size " << r.size() << " " << v3colorNrgbDev(0) << endl;
752  v3colorNrgb(0) = getHistogramMeanShift(r, 1.0, v3colorNrgbDev(0));
753  v3colorNrgb(1) = getHistogramMeanShift(g, 1.0, v3colorNrgbDev(1));
754  v3colorNrgb(2) = getHistogramMeanShift(b, 1.0, v3colorNrgbDev(2));
755  //// dominantIntensity = getHistogramMeanShift(intensity, 767.0,
756  /// v3colorNrgbDev(2));
757  // assert(v3colorNrgb(0) != 0 && v3colorNrgb(1) != 0 && v3colorNrgb(2) !=
758  // 0);
759  //
760  dominantIntensity = 0;
761  int countFringe05 = 0;
762  for (unsigned i = 0; i < r.size(); i++)
763  if (fabs(r[i] - v3colorNrgb(0)) < 0.05 &&
764  fabs(g[i] - v3colorNrgb(1)) < 0.05 &&
765  fabs(b[i] - v3colorNrgb(2)) < 0.05)
766  {
767  dominantIntensity += intensity[i];
768  ++countFringe05;
769  }
770  assert(countFringe05 > 0);
771  dominantIntensity /= countFringe05;
772  float concentration05 = static_cast<float>(countFringe05) / r.size();
773 
774  // getColorEnd = pcl::getTime ();
775  // cout << "Nrgb in " << (getColorEnd - getColorStart)*1000000 << " us\n";
776  // cout << "Concentration " << concentration1 << " " << concentration2 << "
777  // " << concentration3 << endl;
778  // We are storing the concentration vale in v3colorNrgbDev to heck if there
779  // exists a dominant color
780  // if(concentration1 > concentrationThreshold && concentration2 >
781  // concentrationThreshold && concentration3 > concentrationThreshold)
782  if (concentration05 > 0.5)
783  bDominantColor = true;
784  else
785  bDominantColor = false;
786 
787  // getColorStart = pcl::getTime ();
788 
789  // // c1c2c3
790  // getPlaneC1C2C3();
791  // v3colorC1C2C3[0] = getHistogramMeanShift(c1, (float)1.5708,
792  // v3colorNrgbDev[0]);
793  // v3colorC1C2C3[1] = getHistogramMeanShift(c2, (float)1.5708,
794  // v3colorNrgbDev[1]);
795  // v3colorC1C2C3[2] = getHistogramMeanShift(c3, (float)1.5708,
796  // v3colorNrgbDev[2]);
797  ////getColorEnd = pcl::getTime ();
798  ////cout << "c1c2c3 in " << (getColorEnd - getColorStart)*1000000 << "
799  /// us\n";
800  //
801  ////getColorStart = pcl::getTime ();
802  // H histogram
803  calcPlaneHistH();
804  ////cout << "Update color fin" << endl;
805  // getColorEnd = pcl::getTime ();
806  // cout << "HistH in " << (getColorEnd - getColorStart)*1000000 << " us\n";
807 }
808 
809 /**!
810  * mPointHull serves to calculate the convex hull of a set of points in 2D,
811  * which are defined by its position (x,y)
812  * and an identity id
813 */
814 struct mPointHull
815 {
816  float x, y;
817  size_t id;
818 
819  bool operator<(const mPointHull& p) const
820  {
821  return x < p.x || (x == p.x && y < p.y);
822  }
823 };
824 
825 float cross(const mPointHull& O, const mPointHull& A, const mPointHull& B)
826 {
827  return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
828 }
829 
830 ///**!
831 // * Calculate the plane's convex hull with the monotone chain algorithm.
832 //*/
833 // void Plane::calcConvexHull(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
834 // &pointCloud)
835 //{
836 // // Find axis with largest normal component and project onto perpendicular
837 // plane
838 // int k0;//, k1, k2;
839 // k0 = (fabs (v3normal(0) ) > fabs(v3normal[1])) ? 0 : 1;
840 // k0 = (fabs (v3normal(k0) ) > fabs(v3normal(2))) ? k0 : 2;
841 //
842 // std::vector<mPointHull> P;//(pointCloud->size() );
843 // P.resize(pointCloud->size() );
844 // if(k0 == 0)
845 // for(size_t i=0; i < pointCloud->size(); i++)
846 // {
847 // P[i].x = pointCloud->points[i].y;
848 // P[i].y = pointCloud->points[i].z;
849 // P[i].id = i;
850 // }
851 // else if(k0 == 1)
852 // for(size_t i=0; i < pointCloud->size(); i++)
853 // {
854 // P[i].x = pointCloud->points[i].x;
855 // P[i].y = pointCloud->points[i].z;
856 // P[i].id = i;
857 // }
858 // else // (k0 == 2)
859 // for(size_t i=0; i < pointCloud->size(); i++)
860 // {
861 // P[i].x = pointCloud->points[i].x;
862 // P[i].y = pointCloud->points[i].y;
863 // P[i].id = i;
864 // }
865 //
866 // int n = P.size(), k = 0;
867 // std::vector<mPointHull> H(2*n);
868 //
869 // // Sort points lexicographically
870 // std::sort(P.begin(), P.end());
871 //
872 // // Build lower hull
873 // for (int i = 0; i < n; i++)
874 // {
875 // while (k >= 2 && cross(H[k-2], H[k-1], P[i]) <= 0)
876 // k--;
877 // H[k++] = P[i];
878 // if(k > 0)
879 // assert(H[k-1].id != H[k-2].id);
880 // }
881 //
882 // // Build upper hull
883 // for (int i = n-2, t = k+1; i >= 0; i--)
884 // {
885 // while (k >= t && cross(H[k-2], H[k-1], P[i]) <= 0)
886 // k--;
887 // H[k++] = P[i];
888 // }
889 //
890 // // Fill convexHull vector
891 // size_t hull_noRep = k-1; // Neglect the last_point = first_point
892 // H.resize(k);
893 // polygonContourPtr->resize(hull_noRep);
894 //
895 // for(size_t i=0; i < hull_noRep; i++)
896 // {
897 // polygonContourPtr->points[i] = pointCloud->points[ H[i].id ];
898 // }
899 //
900 //}
901 
902 void Plane::calcConvexHull(
903  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloud,
904  std::vector<size_t>& indices)
905 {
906  // Find axis with largest normal component and project onto perpendicular
907  // plane
908  int k0; //, k1, k2;
909  k0 = (fabs(v3normal(0)) > fabs(v3normal[1])) ? 0 : 1;
910  k0 = (fabs(v3normal(k0)) > fabs(v3normal(2))) ? k0 : 2;
911 
912  std::vector<mPointHull> P; //(pointCloud->size() );
913  P.resize(pointCloud->size());
914  if (k0 == 0)
915  for (size_t i = 0; i < pointCloud->size(); i++)
916  {
917  P[i].x = pointCloud->points[i].y;
918  P[i].y = pointCloud->points[i].z;
919  P[i].id = i;
920  }
921  else if (k0 == 1)
922  for (size_t i = 0; i < pointCloud->size(); i++)
923  {
924  P[i].x = pointCloud->points[i].x;
925  P[i].y = pointCloud->points[i].z;
926  P[i].id = i;
927  }
928  else // (k0 == 2)
929  for (size_t i = 0; i < pointCloud->size(); i++)
930  {
931  P[i].x = pointCloud->points[i].x;
932  P[i].y = pointCloud->points[i].y;
933  P[i].id = i;
934  }
935 
936  int n = P.size(), k = 0;
937  std::vector<mPointHull> H(2 * n);
938 
939  // Sort points lexicographically
940  std::sort(P.begin(), P.end());
941 
942  // Build lower hull
943  for (int i = 0; i < n; i++)
944  {
945  while (k >= 2 && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
946  H[k++] = P[i];
947  if (k > 0) assert(H[k - 1].id != H[k - 2].id);
948  }
949 
950  // Build upper hull
951  for (int i = n - 2, t = k + 1; i >= 0; i--)
952  {
953  while (k >= t && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
954  H[k++] = P[i];
955  }
956 
957  // Fill convexHull vector (polygonContourPtr)
958  size_t hull_noRep = k - 1; // Neglect the last_point = first_point
959  H.resize(k);
960  polygonContourPtr->resize(hull_noRep);
961  indices.resize(hull_noRep);
962 
963  for (size_t i = 0; i < hull_noRep; i++)
964  {
965  polygonContourPtr->points[i] = pointCloud->points[H[i].id];
966  indices[i] = H[i].id;
967  }
968 }
969 
970 void Plane::calcConvexHullandParams(
971  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloud,
972  std::vector<size_t>& indices)
973 {
974  // Find axis with largest normal component and project onto perpendicular
975  // plane
976  int k0, k1, k2;
977  k0 = (fabs(v3normal(0)) > fabs(v3normal[1])) ? 0 : 1;
978  k0 = (fabs(v3normal(k0)) > fabs(v3normal(2))) ? k0 : 2;
979  k1 = (k0 + 1) % 3;
980  k2 = (k0 + 2) % 3;
981 
982  std::vector<mPointHull> P; //(pointCloud->size() );
983  P.resize(pointCloud->size());
984  if (k0 == 0)
985  for (size_t i = 0; i < pointCloud->size(); i++)
986  {
987  P[i].x = pointCloud->points[i].y;
988  P[i].y = pointCloud->points[i].z;
989  P[i].id = i;
990  }
991  else if (k0 == 1)
992  for (size_t i = 0; i < pointCloud->size(); i++)
993  {
994  P[i].x = pointCloud->points[i].z;
995  P[i].y = pointCloud->points[i].x;
996  P[i].id = i;
997  }
998  else // (k0 == 2)
999  for (size_t i = 0; i < pointCloud->size(); i++)
1000  {
1001  P[i].x = pointCloud->points[i].x;
1002  P[i].y = pointCloud->points[i].y;
1003  P[i].id = i;
1004  }
1005 
1006  int n = P.size(), k = 0;
1007  std::vector<mPointHull> H(2 * n);
1008 
1009  // Sort points lexicographically
1010  std::sort(P.begin(), P.end());
1011 
1012  // Build lower hull
1013  for (int i = 0; i < n; i++)
1014  {
1015  while (k >= 2 && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
1016  H[k++] = P[i];
1017  if (k > 0) assert(H[k - 1].id != H[k - 2].id);
1018  }
1019 
1020  // Build upper hull
1021  for (int i = n - 2, t = k + 1; i >= 0; i--)
1022  {
1023  while (k >= t && cross(H[k - 2], H[k - 1], P[i]) <= 0) k--;
1024  H[k++] = P[i];
1025  }
1026 
1027  // Fill convexHull vector (polygonContourPtr)
1028  size_t hull_noRep = k - 1; // Neglect the last_point = first_point
1029  H.resize(k);
1030  polygonContourPtr->resize(hull_noRep);
1031  indices.resize(hull_noRep);
1032 
1033  for (size_t i = 0; i < hull_noRep; i++)
1034  {
1035  polygonContourPtr->points[i] = pointCloud->points[H[i].id];
1036  indices[i] = H[i].id;
1037  }
1038 
1039  // Calc area and mass center
1040  float ct = fabs(v3normal[k0]);
1041  float AreaX2 = 0.0;
1042  Eigen::Vector3f massCenter = Eigen::Vector3f::Zero();
1043  for (size_t i = 0, j = 1; i < hull_noRep; i++, j++)
1044  {
1045  float cross_segment = H[i].x * H[j].y - H[i].y * H[j].x;
1046 
1047  AreaX2 += cross_segment;
1048  massCenter[k1] += (H[i].x + H[j].x) * cross_segment;
1049  massCenter[k2] += (H[i].y + H[j].y) * cross_segment;
1050  }
1051  areaHull = fabs(AreaX2) / (2 * ct);
1052 
1053  v3center[k1] /= (3 * AreaX2);
1054  v3center[k2] /= (3 * AreaX2);
1055  v3center[k0] =
1056  (-d - v3normal[k1] * massCenter[k1] - v3normal[k2] * massCenter[k2]) /
1057  v3normal[k0];
1058 
1059  d = -v3normal.dot(v3center);
1060 
1061  // Compute elongation and ppal direction
1062  Eigen::Matrix2f covariances = Eigen::Matrix2f::Zero();
1063  Eigen::Vector2f p1, p2;
1064  p2[0] = H[0].x - massCenter[k1];
1065  p2[1] = H[0].y - massCenter[k2];
1066  // float perimeter = 0.0;
1067  for (size_t i = 1; i < hull_noRep; i++)
1068  {
1069  p1 = p2;
1070  p2[0] = H[i].x - massCenter[k1];
1071  p2[1] = H[i].y - massCenter[k2];
1072  float lenght_side = (p1 - p2).norm();
1073  // perimeter += lenght_side;
1074  covariances +=
1075  lenght_side * (p1 * p1.transpose() + p2 * p2.transpose());
1076  }
1077  // covariances /= perimeter;
1078 
1079  // Compute eigen vectors and values
1080  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> evd(covariances);
1081  // Organize eigenvectors and eigenvalues in ascendent order
1082  for (int i = 0; i < 2; ++i)
1083  {
1084  std::cout << "Eig " << evd.eigenvalues()[i] << " v "
1085  << evd.eigenvectors().col(i).transpose() << "\n";
1086  }
1087  if (evd.eigenvalues()[0] > 2 * evd.eigenvalues()[1])
1088  {
1089  elongation = sqrt(evd.eigenvalues()[0] / evd.eigenvalues()[1]);
1090  v3PpalDir[k1] = evd.eigenvectors()(0, 0);
1091  v3PpalDir[k2] = evd.eigenvectors()(1, 0);
1092  v3PpalDir[k0] =
1093  (-d - v3normal[k1] * v3PpalDir[k1] - v3normal[k2] * v3PpalDir[k2]) /
1094  v3normal[k0];
1095  v3PpalDir /= v3PpalDir.norm();
1096  }
1097 }
1098 
1099 /*!Returns true when the closest distance between the patches "this" and the
1100  * input "plane_nearby" is under distThreshold. This function is approximated*/
1101 bool Plane::isPlaneNearby(Plane& plane_nearby, const float distThreshold)
1102 {
1103  float distThres2 = distThreshold * distThreshold;
1104 
1105  // First we check distances between centroids and vertex to accelerate this
1106  // check
1107  if ((v3center - plane_nearby.v3center).squaredNorm() < distThres2)
1108  return true;
1109 
1110  for (unsigned i = 1; i < polygonContourPtr->size(); i++)
1111  if ((getVector3fromPointXYZ(polygonContourPtr->points[i]) -
1112  plane_nearby.v3center)
1113  .squaredNorm() < distThres2)
1114  return true;
1115 
1116  for (unsigned j = 1; j < plane_nearby.polygonContourPtr->size(); j++)
1117  if ((v3center -
1118  getVector3fromPointXYZ(plane_nearby.polygonContourPtr->points[j]))
1119  .squaredNorm() < distThres2)
1120  return true;
1121 
1122  for (unsigned i = 1; i < polygonContourPtr->size(); i++)
1123  for (unsigned j = 1; j < plane_nearby.polygonContourPtr->size(); j++)
1124  if ((diffPoints(
1125  polygonContourPtr->points[i],
1126  plane_nearby.polygonContourPtr->points[j]))
1127  .squaredNorm() < distThres2)
1128  return true;
1129 
1130  // a) Between an edge and a vertex
1131  // b) Between two edges (imagine two polygons on perpendicular planes)
1132  // a) & b)
1133  for (unsigned i = 1; i < polygonContourPtr->size(); i++)
1134  for (unsigned j = 1; j < plane_nearby.polygonContourPtr->size(); j++)
1135  if (dist3D_Segment_to_Segment2(
1136  Segment(
1137  polygonContourPtr->points[i],
1138  polygonContourPtr->points[i - 1]),
1139  Segment(
1140  plane_nearby.polygonContourPtr->points[j],
1141  plane_nearby.polygonContourPtr->points[j - 1])) <
1142  distThres2)
1143  return true;
1144 
1145  return false;
1146 }
1147 
1148 /*! Returns true if the two input planes represent the same physical surface for
1149 * some given angle and distance thresholds.
1150 * If the planes are the same they are merged in this and the function returns
1151 * true. Otherwise it returns false.*/
1152 bool Plane::isSamePlane(
1153  Plane& plane_nearby, const float& cosAngleThreshold,
1154  const float& parallelDistThres, const float& proxThreshold)
1155 {
1156  // Check that both planes have similar orientation
1157  if (v3normal.dot(plane_nearby.v3normal) < cosAngleThreshold) return false;
1158 
1159  // Check the normal distance of the planes centers using their average
1160  // normal
1161  float dist_normal = v3normal.dot(plane_nearby.v3center - v3center);
1162  if (fabs(dist_normal) > parallelDistThres) // Then merge the planes
1163  return false;
1164 
1165  // Check that the distance between the planes centers is not too big
1166  if (!isPlaneNearby(plane_nearby, proxThreshold)) return false;
1167 
1168  return true;
1169 }
1170 
1171 /*!Check if the the input plane is the same than this plane for some given angle
1172  * and distance thresholds.
1173  * If the planes are the same they are merged in this and the function returns
1174  * true. Otherwise it returns false.*/
1175 bool Plane::isSamePlane(
1176  Eigen::Matrix4f& Rt, Plane& plane_, const float& cosAngleThreshold,
1177  const float& parallelDistThres, const float& proxThreshold)
1178 {
1179  Plane plane = plane_;
1180  plane.v3normal = Rt.block(0, 0, 3, 3) * plane_.v3normal;
1181  plane.v3center =
1182  Rt.block(0, 0, 3, 3) * plane_.v3center + Rt.block(0, 3, 3, 1);
1183  pcl::transformPointCloud(
1184  *plane_.polygonContourPtr, *plane.polygonContourPtr, Rt);
1185 
1186  if (fabs(d - plane.d) > parallelDistThres) return false;
1187 
1188  // Check that both planes have similar orientation
1189  if (v3normal.dot(plane.v3normal) < cosAngleThreshold) return false;
1190 
1191  // // Check the normal distance of the planes centers using their average
1192  // normal
1193  // float dist_normal = v3normal.dot(plane.v3center - v3center);
1194  //// if(fabs(dist_normal) > parallelDistThres ) // Avoid matching different
1195  /// parallel planes
1196  //// return false;
1197  // float thres_max_dist = max(parallelDistThres,
1198  // parallelDistThres*2*norm(plane.v3center - v3center));
1199  // if(fabs(dist_normal) > thres_max_dist ) // Avoid matching different
1200  // parallel planes
1201  // return false;
1202 
1203  // Once we know that the planes are almost coincident (parallelism and
1204  // position)
1205  // we check that the distance between the planes is not too big
1206  return isPlaneNearby(plane, proxThreshold);
1207 }
1208 
1209 bool Plane::hasSimilarDominantColor(Plane& plane, const float colorThreshold)
1210 {
1211  if (bDominantColor && plane.bDominantColor &&
1212  (fabs(v3colorNrgb[0] - plane.v3colorNrgb[0]) > colorThreshold ||
1213  fabs(v3colorNrgb[1] - plane.v3colorNrgb[1]) > colorThreshold))
1214  return false;
1215  else
1216  return true;
1217 }
1218 
1219 /*! Merge the input "same_plane_patch" into "this".
1220 * Recalculate center, normal vector, area, inlier points (filtered), convex
1221 * hull, etc.
1222 */
1223 void Plane::mergePlane(Plane& same_plane_patch)
1224 {
1225  // Update normal and center
1226  // assert(areaVoxels > 0 && same_plane_patch.areaVoxels > 0)
1227  // v3normal = (areaVoxels*v3normal +
1228  // same_plane_patch.areaVoxels*same_plane_patch.v3normal);
1229  assert(inliers.size() > 0 && same_plane_patch.inliers.size() > 0);
1230  v3normal =
1231  (inliers.size() * v3normal +
1232  same_plane_patch.inliers.size() * same_plane_patch.v3normal);
1233  v3normal = v3normal / v3normal.norm();
1234 
1235  // Update point inliers
1236  // *polygonContourPtr += *same_plane_patch.polygonContourPtr; // Merge
1237  // polygon points
1238  *planePointCloudPtr +=
1239  *same_plane_patch.planePointCloudPtr; // Add the points of the new
1240  // detection and perform a voxel
1241  // grid
1242 
1243  // Filter the points of the patch with a voxel-grid. This points are used
1244  // only for visualization
1245  static pcl::VoxelGrid<pcl::PointXYZRGBA> merge_grid;
1246  merge_grid.setLeafSize(0.05, 0.05, 0.05);
1247  pcl::PointCloud<pcl::PointXYZRGBA> mergeCloud;
1248  merge_grid.setInputCloud(planePointCloudPtr);
1249  merge_grid.filter(mergeCloud);
1250  planePointCloudPtr->clear();
1251  *planePointCloudPtr = mergeCloud;
1252  // calcMainColor();
1253  // if(configPbMap.use_color)
1254  // calcMainColor();
1255 
1256  inliers.insert(
1257  inliers.end(), same_plane_patch.inliers.begin(),
1258  same_plane_patch.inliers.end());
1259 
1260  *same_plane_patch.polygonContourPtr += *polygonContourPtr;
1261  calcConvexHull(same_plane_patch.polygonContourPtr);
1262  computeMassCenterAndArea();
1263 
1264  d = -v3normal.dot(v3center);
1265 
1266  // Move the points to fulfill the plane equation
1267  forcePtsLayOnPlane();
1268 
1269  // Update area
1270  // double area_recalc = planePointCloudPtr->size() * 0.0025;
1271  // mpPlaneInferInfo->isFullExtent(same_plane_patch, area_recalc);
1272  areaVoxels = planePointCloudPtr->size() * 0.0025;
1273 }
1274 
1275 // Adaptation for RGBD360
1276 void Plane::mergePlane2(Plane& same_plane_patch)
1277 {
1278  // Update normal and center
1279  assert(inliers.size() > 0 && same_plane_patch.inliers.size() > 0);
1280  v3normal =
1281  (inliers.size() * v3normal +
1282  same_plane_patch.inliers.size() * same_plane_patch.v3normal);
1283  v3normal = v3normal / v3normal.norm();
1284 
1285  // Update point inliers
1286  *planePointCloudPtr +=
1287  *same_plane_patch.planePointCloudPtr; // Add the points of the new
1288  // detection and perform a voxel
1289  // grid
1290 
1291  inliers.insert(
1292  inliers.end(), same_plane_patch.inliers.begin(),
1293  same_plane_patch.inliers.end());
1294 
1295  *same_plane_patch.polygonContourPtr += *polygonContourPtr;
1296  calcConvexHull(same_plane_patch.polygonContourPtr);
1297  // calcConvexHullandParams(same_plane_patch.polygonContourPtr);
1298  // std::cout << d << " area " << areaHull << " center " <<
1299  // v3center.transpose() << " elongation " << elongation << " v3PpalDir " <<
1300  // v3PpalDir.transpose() << std::endl;
1301  computeMassCenterAndArea();
1302 
1303  calcElongationAndPpalDir();
1304 
1305  d = -v3normal.dot(v3center);
1306  // std::cout << d << "area " << areaHull << " center " <<
1307  // v3center.transpose() << " elongation " << elongation << " v3PpalDir " <<
1308  // v3PpalDir.transpose() << std::endl;
1309 
1310  calcMainColor2(); // Calculate dominant color
1311 
1312  // Update color histogram
1313  for (size_t i = 0; i < hist_H.size(); i++)
1314  {
1315  hist_H[i] += same_plane_patch.hist_H[i];
1316  hist_H[i] /= 2;
1317  }
1318 }
1319 
1320 void Plane::transform(Eigen::Matrix4f& Rt)
1321 {
1322  // Transform normal and ppal direction
1323  v3normal = Rt.block(0, 0, 3, 3) * v3normal;
1324  v3PpalDir = Rt.block(0, 0, 3, 3) * v3PpalDir;
1325 
1326  // Transform centroid
1327  v3center = Rt.block(0, 0, 3, 3) * v3center + Rt.block(0, 3, 3, 1);
1328 
1329  d = -(v3normal.dot(v3center));
1330 
1331  // Transform convex hull points
1332  pcl::transformPointCloud(*polygonContourPtr, *polygonContourPtr, Rt);
1333 
1334  pcl::transformPointCloud(*planePointCloudPtr, *planePointCloudPtr, Rt);
1335 }
1336 
1337 #endif
mrpt::pbmap::getMultiDimMeanShift_color
Eigen::Vector4f getMultiDimMeanShift_color(std::vector< Eigen::Vector4f > &data, dataType &stdDevHist, dataType &concentration)
Definition: Miscellaneous.h:154
mrpt::pbmap::diffPoints
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:46
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::pbmap::Plane::polygonContourPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:184
mrpt::pbmap::Plane::transform
void transform(Eigen::Matrix4f &Rt)
Definition: Plane.cpp:1320
mrpt::utils::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:44
mrpt::pbmap::Plane
A class used to store a planar feature (Plane for short).
Definition: Plane.h:48
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:169
Miscellaneous.h
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::pbmap::Plane::mergePlane2
void mergePlane2(Plane &plane)
Definition: Plane.cpp:1276
indices
GLuint GLuint GLsizei GLenum const GLvoid * indices
Definition: glext.h:3528
std
STL namespace.
mrpt::pbmap::Plane::mergePlane
void mergePlane(Plane &plane)
Definition: Plane.cpp:1223
mrpt::pbmap::Plane::v3colorNrgb
Eigen::Vector3f v3colorNrgb
! Radiometric description
Definition: Plane.h:161
mrpt::pbmap::Plane::calcConvexHullandParams
void calcConvexHullandParams(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &pointCloud, std::vector< size_t > &indices=DEFAULT_VECTOR)
Definition: Plane.cpp:970
mPointHull::operator<
bool operator<(const mPointHull &p) const
Definition: Plane.cpp:819
mrpt::pbmap::Plane::calcElongationAndPpalDir
void calcElongationAndPpalDir()
Definition: Plane.cpp:472
Plane.h
color
GLuint color
Definition: glext.h:8300
mPointHull
! mPointHull serves to calculate the convex hull of a set of points in 2D, which are defined by its p...
Definition: Plane.cpp:814
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: CColouredPointsMap.h:182
mrpt::pbmap::Plane::calcPlaneHistH
void calcPlaneHistH()
Definition: Plane.cpp:547
mrpt::pbmap::Plane::v3normal
Eigen::Vector3f v3normal
Definition: Plane.h:144
mrpt::pbmap::Plane::planePointCloudPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:188
g
GLubyte g
Definition: glext.h:6279
mrpt::pbmap::getHistogramMeanShift
dataType getHistogramMeanShift(std::vector< dataType > &data, double range, dataType &stdDevHist_out)
Definition: Miscellaneous.h:248
b
GLubyte GLubyte b
Definition: glext.h:6279
mrpt::pbmap::Plane::forcePtsLayOnPlane
void forcePtsLayOnPlane()
Definition: Plane.cpp:361
mPointHull::x
float x
Definition: Plane.cpp:816
cross
float cross(const mPointHull &O, const mPointHull &A, const mPointHull &B)
Definition: Plane.cpp:825
mrpt::pbmap::Plane::isSamePlane
bool isSamePlane(Plane &plane, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold)
Definition: Plane.cpp:1152
mrpt::pbmap::Plane::isPlaneNearby
bool isPlaneNearby(Plane &plane, const float distThreshold)
Definition: Plane.cpp:1101
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
mrpt::pbmap::Plane::getPlaneC1C2C3
void getPlaneC1C2C3()
Definition: Plane.cpp:521
mrpt::pbmap::Plane::calcMainColor
void calcMainColor()
Definition: Plane.cpp:744
mrpt::pbmap::Plane::compute2DPolygonalArea
float compute2DPolygonalArea()
Compute the area of a 2D planar polygon patch - using a given normal.
Definition: Plane.cpp:390
mrpt::pbmap::dist3D_Segment_to_Segment2
float dist3D_Segment_to_Segment2(Segment S1, Segment S2)
Definition: Miscellaneous.cpp:25
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::pbmap::Plane::computeMassCenterAndArea
void computeMassCenterAndArea()
Compute the patch&#39;s convex-hull area and mass center.
Definition: Plane.cpp:426
mrpt::pbmap::Plane::inliers
std::vector< int32_t > inliers
! Convex Hull
Definition: Plane.h:183
in
GLuint in
Definition: glext.h:7274
mrpt::pbmap::Plane::hist_H
std::vector< float > hist_H
Definition: Plane.h:167
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::pbmap::Plane::v3center
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:143
void
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
mrpt::pbmap::Plane::calcMainColor2
void calcMainColor2()
Definition: Plane.cpp:673
concentrationThreshold
float concentrationThreshold
Definition: Plane.cpp:671
mrpt::pbmap::Plane::getPlaneNrgb
void getPlaneNrgb()
Definition: Plane.cpp:490
mPointHull::y
float y
Definition: Plane.cpp:816
x
GLenum GLint x
Definition: glext.h:3538
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::pbmap::Plane::bDominantColor
bool bDominantColor
Definition: Plane.h:163
mrpt::pbmap::Plane::hasSimilarDominantColor
bool hasSimilarDominantColor(Plane &plane, const float colorThreshold)
Definition: Plane.cpp:1209
mrpt::pbmap::Plane::calcConvexHull
void calcConvexHull(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &pointCloud, std::vector< size_t > &indices=DEFAULT_VECTOR)
! Calculate the plane&#39;s convex hull with the monotone chain algorithm.
Definition: Plane.cpp:902
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:134
mrpt::pbmap::Plane::readFromStream
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: Plane.cpp:237
mrpt::pbmap::getVector3fromPointXYZ
Eigen::Vector3f getVector3fromPointXYZ(pointPCL &pt)
Definition: Miscellaneous.h:40
mPointHull::id
size_t id
Definition: Plane.cpp:817


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019