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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019