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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019