MRPT  1.9.9
ConsistencyTest.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 #include <mrpt/math/CMatrixFixed.h>
19 #include <mrpt/math/CVectorFixed.h>
20 #include <mrpt/math/ransac.h>
24 #include <mrpt/poses/CPose3D.h>
25 #include <mrpt/poses/Lie/SE.h>
26 
27 using namespace std;
28 using namespace Eigen;
29 using namespace mrpt::pbmap;
30 using namespace mrpt::math; // CMatrixF*
31 using namespace mrpt;
32 
33 ConsistencyTest::ConsistencyTest(PbMap& PBM_source, PbMap& PBM_target)
34  : PBMSource(PBM_source), PBMTarget(PBM_target)
35 {
36 }
37 
39  std::map<unsigned, unsigned>& matched_planes, Eigen::Matrix4f& rigidTransf)
40 {
41  double sum_depth_errors2 = 0;
42  double sum_areas = 0;
43  // unsigned count_pts = 0;
44  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
45  {
46  sum_depth_errors2 +=
47  (PBMSource.vPlanes[it->first].areaVoxels +
48  PBMTarget.vPlanes[it->second].areaVoxels) *
49  pow(PBMTarget.vPlanes[it->second].v3normal.dot(
50  compose(
51  rigidTransf, PBMSource.vPlanes[it->first].v3center) -
52  PBMTarget.vPlanes[it->second].v3center),
53  2);
54  sum_areas += PBMSource.vPlanes[it->first].areaVoxels +
55  PBMTarget.vPlanes[it->second].areaVoxels;
56  }
57  double avError2 = sum_depth_errors2 / sum_areas;
58  return sqrt(avError2);
59 }
60 
61 // Transformation from Source to Target
62 Eigen::Matrix4f ConsistencyTest::initPose(
63  std::map<unsigned, unsigned>& matched_planes)
64 {
65  // assert(matched_planes.size() >= 3);
66  if (matched_planes.size() < 3)
67  {
68  cout << "Insuficient matched planes " << matched_planes.size() << endl;
69  return Eigen::Matrix4f::Identity();
70  }
71 
72  // Calculate rotation
73  Matrix3f normalCovariances = Matrix3f::Zero();
74  normalCovariances(0, 0) = 1; // Limit rotation on y/z (horizontal) axis
75  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
76  normalCovariances += PBMTarget.vPlanes[it->second].v3normal *
77  PBMSource.vPlanes[it->first].v3normal.transpose();
78 
79  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
80  Matrix3f Rotation = svd.matrixV() * svd.matrixU().transpose();
81 
82  // Check consitioning. 3 non-parallel planes are required in 6DoF, and only
83  // two for planar movement (3DoF)
84  bool bPlanar_cond = false;
85  for (auto it1 = matched_planes.begin();
86  it1 != matched_planes.end() && !bPlanar_cond; it1++)
87  {
88  auto it2 = it1;
89  it2++;
90  for (; it2 != matched_planes.end() && !bPlanar_cond; it2++)
91  {
92  Eigen::Vector3f planar_conditioning =
93  PBMSource.vPlanes[it1->first].v3normal.cross(
94  PBMSource.vPlanes[it2->first].v3normal);
95  if (fabs(planar_conditioning(0)) > 0.33) bPlanar_cond = true;
96  }
97  }
98  // float conditioning =
99  // svd.singularValues().maxCoeff()/svd.singularValues().minCoeff();
100  // if(conditioning > 100) // ^Dof
101  if (!bPlanar_cond) // ^Dof
102  {
103  // cout << " ConsistencyTest::initPose -> Bad conditioning: " <<
104  // conditioning << " -> Returning the identity\n";
105  return Eigen::Matrix4f::Identity();
106  }
107 
108  double det = Rotation.determinant();
109  if (det != 1)
110  {
111  Eigen::Matrix3f aux;
112  aux << 1, 0, 0, 0, 1, 0, 0, 0, det;
113  Rotation = svd.matrixV() * aux * svd.matrixU().transpose();
114  }
115  // if(Rotation.determinant() < 0)
116  // Rotation.row(2) *= -1;
117  // float det = Rotation.determinant();
118  // cout << "Rotation det " << det << endl;
119 
120  // cout << "Rotation\n" << Rotation << endl;
121 
122  // // Evaluate error of each match looking for outliers
123  // float sumError = 0;
124  // for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it !=
125  // matched_planes.end(); it++)
126  // {
127  // float error = (PBMSource.vPlanes[it->first].v3normal .cross (Rotation
128  // * PBMTarget.vPlanes[it->second].v3normal ) ).norm();
129  // sumError += error;
130  // cout << "errorRot " << it->first << " " << it->second << " is " <<
131  // error << endl;
132  // }
133  // cout << "Average rotation error " << sumError / matched_planes.size() <<
134  // endl;
135 
136  // Calculate translation
137  Vector3f translation;
138  Matrix3f hessian = Matrix3f::Zero();
139  Vector3f gradient = Vector3f::Zero();
140  float accum_error2 = 0.0;
141  hessian(0, 0) = 1; // Limit movement on x (vertical) axis
142  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
143  {
144  float trans_error =
145  (PBMSource.vPlanes[it->first].d -
146  PBMTarget.vPlanes[it->second].d); //+n*t
147 
148  accum_error2 += trans_error * trans_error;
149  // hessian += PBMTarget.vPlanes[it->second].v3normal *
150  // PBMTarget.vPlanes[it->second].v3normal.transpose();
151  // gradient += -PBMTarget.vPlanes[it->second].v3normal * trans_error;
152  hessian += PBMSource.vPlanes[it->first].v3normal *
153  PBMSource.vPlanes[it->first].v3normal.transpose();
154  gradient += PBMSource.vPlanes[it->first].v3normal * trans_error;
155  }
156  translation = -hessian.inverse() * gradient;
157  // cout << "Previous average translation error " << sumError /
158  // matched_planes.size() << endl;
159 
160  // // Evaluate error of each match looking for outliers
161  // sumError = 0;
162  // for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it !=
163  // matched_planes.end(); it++)
164  // {
165  //// float trans_error = fabs(-PBMTarget.vPlanes[it->second].d +
166  /// translation.dot(PBMTarget.vPlanes[it->second].v3normal) +
167  /// PBMSource.vPlanes[it->first].d);
168  // float trans_error = fabs((PBMTarget.vPlanes[it->second].d -
169  // translation.dot(PBMSource.vPlanes[it->first].v3normal)) -
170  // PBMSource.vPlanes[it->first].d);
171  // sumError += trans_error;
172  // cout << "errorTrans " << it->first << " " << it->second << " is " <<
173  // trans_error << endl;
174  // }
175  // cout << "Average translation error " << sumError / matched_planes.size()
176  // << endl;
177 
178  // Form SE3 transformation matrix. This matrix maps the model into the
179  // current data reference frame
180  Eigen::Matrix4f rigidTransf;
181  rigidTransf.block<3, 3>(0, 0) = Rotation;
182  rigidTransf.block(0, 3, 3, 1) = translation;
183  rigidTransf.row(3) << 0, 0, 0, 1;
184  return rigidTransf;
185 }
186 
187 // Transformation from Source to Target
189  std::map<unsigned, unsigned>& matched_planes)
190 {
191  if (matched_planes.size() < 3)
192  {
193  cout << "Insuficient matched planes " << matched_planes.size() << endl;
194  return Eigen::Matrix4f::Identity();
195  }
196 
197  // Calculate rotation
198  Matrix3f normalCovariances = Matrix3f::Zero();
199  // normalCovariances(0,0) = 1; // Limit rotation on y/z (horizontal) axis
200  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
201  normalCovariances += (PBMTarget.vPlanes[it->second].areaHull /
202  PBMTarget.vPlanes[it->second].d) *
203  PBMTarget.vPlanes[it->second].v3normal *
204  PBMSource.vPlanes[it->first].v3normal.transpose();
205  // normalCovariances += PBMTarget.vPlanes[it->second].v3normal *
206  // PBMSource.vPlanes[it->first].v3normal.transpose();
207 
208  // Introduce the virtual matching of two vertical planes n=(1,0,0)
209  for (unsigned r = 0; r < 3; r++)
210  for (unsigned c = 0; c < 3; c++)
211  normalCovariances(0, 0) += normalCovariances(r, c);
212 
213  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
214  Matrix3f Rotation = svd.matrixV() * svd.matrixU().transpose();
215 
216  // Check consitioning. 3 non-parallel planes are required in 6DoF, and only
217  // two for planar movement (3DoF)
218  bool bPlanar_cond = false;
219  for (auto it1 = matched_planes.begin();
220  it1 != matched_planes.end() && !bPlanar_cond; it1++)
221  {
222  auto it2 = it1;
223  it2++;
224  for (; it2 != matched_planes.end() && !bPlanar_cond; it2++)
225  {
226  Eigen::Vector3f planar_conditioning =
227  PBMSource.vPlanes[it1->first].v3normal.cross(
228  PBMSource.vPlanes[it2->first].v3normal);
229  if (fabs(planar_conditioning(0)) > 0.33) bPlanar_cond = true;
230  }
231  }
232  // float conditioning =
233  // svd.singularValues().maxCoeff()/svd.singularValues().minCoeff();
234  // if(conditioning > 100) // ^Dof
235  if (!bPlanar_cond) // ^Dof
236  {
237  // cout << " ConsistencyTest::initPose -> Bad conditioning: " <<
238  // conditioning << " -> Returning the identity\n";
239  return Eigen::Matrix4f::Identity();
240  }
241 
242  double det = Rotation.determinant();
243  if (det != 1)
244  {
245  Eigen::Matrix3f aux;
246  aux << 1, 0, 0, 0, 1, 0, 0, 0, det;
247  Rotation = svd.matrixV() * aux * svd.matrixU().transpose();
248  }
249  // cout << "Rotation\n" << Rotation << endl;
250 
251  // // Evaluate error of each match looking for outliers
252  // float sumError = 0;
253  // for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it !=
254  // matched_planes.end(); it++)
255  // {
256  // float error = (PBMSource.vPlanes[it->first].v3normal .cross (Rotation
257  // * PBMTarget.vPlanes[it->second].v3normal ) ).norm();
258  // sumError += error;
259  // cout << "errorRot " << it->first << " " << it->second << " is " <<
260  // error << endl;
261  // }
262  // cout << "Average rotation error " << sumError / matched_planes.size() <<
263  // endl;
264 
265  // Calculate translation
266  Vector3f translation;
267  Matrix3f hessian = Matrix3f::Zero();
268  Vector3f gradient = Vector3f::Zero();
269  // float accum_error2 = 0.0;
270  // hessian(0,0) = 1; // Limit movement on x (vertical) axis
271  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
272  {
273  float trans_error =
274  (PBMSource.vPlanes[it->first].d -
275  PBMTarget.vPlanes[it->second].d); //+n*t
276  // cout << it->first << " area " <<
277  // PBMSource.vPlanes[it->first].areaHull << endl;
278  // accum_error2 += trans_error * trans_error;
279  // hessian += PBMTarget.vPlanes[it->second].v3normal *
280  // PBMTarget.vPlanes[it->second].v3normal.transpose();
281  // gradient += -PBMTarget.vPlanes[it->second].v3normal * trans_error;
282  hessian += (PBMSource.vPlanes[it->first].areaHull /
283  PBMSource.vPlanes[it->first].d) *
284  PBMSource.vPlanes[it->first].v3normal *
285  PBMSource.vPlanes[it->first].v3normal.transpose();
286  gradient += (PBMSource.vPlanes[it->first].areaHull /
287  PBMSource.vPlanes[it->first].d) *
288  PBMSource.vPlanes[it->first].v3normal * trans_error;
289  }
290 
291  // Introduce the virtual matching of two vertical planes n=(1,0,0)
292  for (unsigned r = 0; r < 3; r++)
293  for (unsigned c = 0; c < 3; c++) hessian(0, 0) += hessian(r, c);
294 
295  translation = -hessian.inverse() * gradient;
296  // cout << "Previous average translation error " << sumError /
297  // matched_planes.size() << endl;
298 
299  // // Evaluate error of each match looking for outliers
300  // sumError = 0;
301  // for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it !=
302  // matched_planes.end(); it++)
303  // {
304  //// float trans_error = fabs(-PBMTarget.vPlanes[it->second].d +
305  /// translation.dot(PBMTarget.vPlanes[it->second].v3normal) +
306  /// PBMSource.vPlanes[it->first].d);
307  // float trans_error = fabs((PBMTarget.vPlanes[it->second].d -
308  // translation.dot(PBMSource.vPlanes[it->first].v3normal)) -
309  // PBMSource.vPlanes[it->first].d);
310  // sumError += trans_error;
311  // cout << "errorTrans " << it->first << " " << it->second << " is " <<
312  // trans_error << endl;
313  // }
314  // cout << "Average translation error " << sumError / matched_planes.size()
315  // << endl;
316 
317  // Form SE3 transformation matrix. This matrix maps the model into the
318  // current data reference frame
319  Eigen::Matrix4f rigidTransf;
320  rigidTransf.block<3, 3>(0, 0) = Rotation;
321  rigidTransf.block(0, 3, 3, 1) = translation;
322  rigidTransf.row(3) << 0, 0, 0, 1;
323  return rigidTransf;
324 }
325 
327  std::map<unsigned, unsigned>& matched_planes, Eigen::Matrix4f& rigidTransf,
328  Eigen::Matrix<float, 6, 6>& covarianceM)
329 {
330  if (matched_planes.size() < 3)
331  {
332  cout << "Insuficient matched planes " << matched_planes.size() << endl;
333  return false;
334  }
335 
336  unsigned col = 0;
337  MatrixXf normalVectors(3, matched_planes.size());
338  for (auto it = matched_planes.begin(); it != matched_planes.end();
339  it++, col++)
340  normalVectors.col(col) = PBMTarget.vPlanes[it->first].v3normal;
341  JacobiSVD<MatrixXf> svd_cond(normalVectors, ComputeThinU | ComputeThinV);
342  // cout << "SV " << svd_cond.singularValues().transpose() << endl;
343  if (svd_cond.singularValues()[0] / svd_cond.singularValues()[1] > 10)
344  return false;
345 
346  // Calculate rotation
347  Matrix3f normalCovariances = Matrix3f::Zero();
348  // normalCovariances(0,0) = 1; // Limit rotation on y/z (horizontal) axis
349  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
350  normalCovariances += (PBMSource.vPlanes[it->first].areaHull /
351  PBMSource.vPlanes[it->first].d) *
352  PBMTarget.vPlanes[it->second].v3normal *
353  PBMSource.vPlanes[it->first].v3normal.transpose();
354 
355  // Limit the rotation to the X (vertical) axis by introducing the virtual
356  // matching of two large horizontal planes n=(1,0,0)
357  for (unsigned r = 0; r < 3; r++)
358  for (unsigned c = 0; c < 3; c++)
359  normalCovariances(0, 0) += fabs(normalCovariances(r, c));
360 
361  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
362 
363  Matrix3f Rotation = svd.matrixV() * svd.matrixU().transpose();
364  double det = Rotation.determinant();
365  if (det != 1)
366  {
367  Eigen::Matrix3f aux;
368  aux << 1, 0, 0, 0, 1, 0, 0, 0, det;
369  Rotation = svd.matrixV() * aux * svd.matrixU().transpose();
370  }
371 
372  // Calculate translation
373  Vector3f translation;
374  Matrix3f hessian = Matrix3f::Zero();
375  Vector3f gradient = Vector3f::Zero();
376  // hessian(0,0) = 1; // Limit movement on x (vertical) axis
377  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
378  {
379  float trans_error =
380  (PBMSource.vPlanes[it->first].d -
381  PBMTarget.vPlanes[it->second].d); //+n*t
382  hessian += (PBMSource.vPlanes[it->first].areaHull /
383  PBMSource.vPlanes[it->first].d) *
384  PBMSource.vPlanes[it->first].v3normal *
385  PBMSource.vPlanes[it->first].v3normal.transpose();
386  gradient += (PBMSource.vPlanes[it->first].areaHull /
387  PBMSource.vPlanes[it->first].d) *
388  PBMSource.vPlanes[it->first].v3normal * trans_error;
389  }
390 
391  // Introduce the virtual matching of a vertical plane n=(1,0,0)
392  for (unsigned r = 0; r < 3; r++)
393  for (unsigned c = 0; c < 3; c++) hessian(0, 0) += fabs(hessian(r, c));
394 
395  translation = -hessian.inverse() * gradient;
396 
397  // Form SE3 transformation matrix. This matrix maps the model into the
398  // current data reference frame
399  // Eigen::Matrix4f rigidTransf;
400  rigidTransf.block<3, 3>(0, 0) = Rotation;
401  rigidTransf.block(0, 3, 3, 1) = translation;
402  rigidTransf.row(3) << 0, 0, 0, 1;
403 
404  // Eigen::Matrix<float,6,6> covarianceM = Eigen::Matrix<float,6,6>::Zero();
405  covarianceM.block<3, 3>(0, 0) = hessian; // The first diagonal 3x3 block
406  // corresponds to the translation
407  // part
408  covarianceM.block(3, 3, 3, 3) = normalCovariances; // Rotation block
409 
410  return true;
411 }
412 
414  std::map<unsigned, unsigned>& matched_planes)
415 {
416  // Calculate rotation
417  Matrix3f normalCovariances = Matrix3f::Zero();
418  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
419  normalCovariances += PBMTarget.vPlanes[it->second].v3normal *
420  PBMSource.vPlanes[it->first].v3normal.transpose();
421  normalCovariances(1, 1) += 100; // Rotation "restricted" to the y axis
422 
423  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
424  Matrix3f Rotation = svd.matrixU() * svd.matrixV().transpose();
425 
426  if (Rotation.determinant() < 0)
427  // Rotation.row(2) *= -1;
428  Rotation = -Rotation;
429 
430  // Calculate translation
431  Vector3f translation;
432  Vector3f center_data = Vector3f::Zero(), center_model = Vector3f::Zero();
433  Vector3f centerFull_data = Vector3f::Zero(),
434  centerFull_model = Vector3f::Zero();
435  unsigned numFull = 0, numNonStruct = 0;
436  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
437  {
438  if (PBMSource.vPlanes[it->first].bFromStructure) // The certainty in
439  // center of
440  // structural planes
441  // is too low
442  continue;
443 
444  ++numNonStruct;
445  center_data += PBMSource.vPlanes[it->first].v3center;
446  center_model += PBMTarget.vPlanes[it->second].v3center;
447  if (PBMSource.vPlanes[it->first].bFullExtent)
448  {
449  centerFull_data += PBMSource.vPlanes[it->first].v3center;
450  centerFull_model += PBMTarget.vPlanes[it->second].v3center;
451  ++numFull;
452  }
453  }
454  if (numFull > 0)
455  {
456  translation =
457  (-centerFull_model + Rotation * centerFull_data) / numFull;
458  }
459  else
460  {
461  translation = (-center_model + Rotation * center_data) / numNonStruct;
462  }
463 
464  translation[1] = 0; // Restrict no translation in the y axis
465 
466  // Form SE3 transformation matrix. This matrix maps the model into the
467  // current data reference frame
468  Eigen::Matrix4f rigidTransf;
469  rigidTransf.block<3, 3>(0, 0) = Rotation;
470  rigidTransf.block(0, 3, 3, 1) = translation;
471  rigidTransf.row(3) << 0, 0, 0, 1;
472  return rigidTransf;
473 }
474 
476  std::map<unsigned, unsigned>& matched_planes)
477 {
478  assert(matched_planes.size() >= 3);
479  Eigen::Matrix4f rigidTransf =
480  initPose(matched_planes); // Inverse-Pose which maps from model to data
481 
482  // std::map<unsigned, unsigned> surrounding_planes = matched_planes;
483  // surrounding_planes.insert(...);
484  double alignmentError = calcAlignmentError(matched_planes, rigidTransf);
485 
486 #ifdef _VERBOSE
487  cout << "INITIALIZATION POSE \n" << rigidTransf << endl;
488  cout << "Alignment error " << alignmentError << endl;
489 #endif
490 
491  unsigned nIter = 0;
492  double improveRate = 0;
493  while (nIter < 10 && improveRate < 0.999)
494  {
495  // Find the rigid transformation which minimizes the distance to the
496  // corresponding planes in the model
497  Vector3f ptInModelRef;
498  Eigen::Matrix<float, 6, 1> v6JacDepthPlane;
500  Eigen::Matrix<float, 6, 6> m6Hessian =
502  double depthError;
503  for (auto it = matched_planes.begin(); it != matched_planes.end(); it++)
504  {
505  ptInModelRef =
506  compose(rigidTransf, PBMSource.vPlanes[it->first].v3center);
507  depthError = PBMTarget.vPlanes[it->second].v3normal.dot(
508  ptInModelRef - PBMTarget.vPlanes[it->second].v3center);
509 
510  v6JacDepthPlane.head(3) = PBMTarget.vPlanes[it->second].v3normal;
511  v6JacDepthPlane(3) =
512  -PBMTarget.vPlanes[it->second].v3normal(1) * ptInModelRef(2) +
513  PBMTarget.vPlanes[it->second].v3normal(2) * ptInModelRef(1);
514  v6JacDepthPlane(4) =
515  PBMTarget.vPlanes[it->second].v3normal(0) * ptInModelRef(2) -
516  PBMTarget.vPlanes[it->second].v3normal(2) * ptInModelRef(0);
517  v6JacDepthPlane(5) =
518  -PBMTarget.vPlanes[it->second].v3normal(0) * ptInModelRef(1) +
519  PBMTarget.vPlanes[it->second].v3normal(1) * ptInModelRef(0);
520  m6Hessian += v6JacDepthPlane * v6JacDepthPlane.transpose();
521  v6Error += v6JacDepthPlane * depthError;
522  }
523 
524  Eigen::Matrix<float, 6, 1> updatedSE3 =
525  (m6Hessian.inverse() * v6Error).transpose();
526  const auto _updatedSE3 =
528  mrpt::math::CMatrixDouble44 CMatUpdate;
529  mrpt::poses::Lie::SE<3>::exp(_updatedSE3)
530  .getHomogeneousMatrix(CMatUpdate);
531  Eigen::Matrix4f updatePose;
532  updatePose << CMatUpdate(0, 0), CMatUpdate(0, 1), CMatUpdate(0, 2),
533  CMatUpdate(0, 3), CMatUpdate(1, 0), CMatUpdate(1, 1),
534  CMatUpdate(1, 2), CMatUpdate(1, 3), CMatUpdate(2, 0),
535  CMatUpdate(2, 1), CMatUpdate(2, 2), CMatUpdate(2, 3), 0, 0, 0, 1;
536  Eigen::Matrix4f tempPose = compose(updatePose, rigidTransf);
537  double newError = calcAlignmentError(matched_planes, tempPose);
538 #ifdef _VERBOSE
539  cout << "New alignment error " << newError << endl;
540 #endif
541 
542  if (newError < alignmentError)
543  {
544  improveRate = newError / alignmentError;
545  alignmentError = newError;
546  rigidTransf = tempPose;
547  }
548  else
549  {
550 #ifdef _VERBOSE
551  cout << "Not converging in iteration " << nIter << endl;
552 #endif
553  break;
554  }
555 
556  ++nIter;
557  }
558 
559 #ifdef _VERBOSE
560  cout << "Consistency test converged after " << nIter << endl;
561 #endif
562 
563  return rigidTransf;
564 }
565 
566 // Obtain the rigid transformation from 3 matched planes
568 {
569  assert(matched_planes.rows() == 8 && matched_planes.cols() == 3);
570 
571  // Calculate rotation
572  Matrix3f normalCovariances = Matrix3f::Zero();
573  normalCovariances(0, 0) = 1;
574  for (unsigned i = 0; i < 3; i++)
575  {
576  Vector3f n_i = Vector3f(
577  matched_planes(0, i), matched_planes(1, i), matched_planes(2, i));
578  Vector3f n_ii = Vector3f(
579  matched_planes(4, i), matched_planes(5, i), matched_planes(6, i));
580  normalCovariances += n_i * n_ii.transpose();
581  // normalCovariances += matched_planes.block(i,0,1,3) *
582  // matched_planes.block(i,4,1,3).transpose();
583  }
584 
585  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
586  Matrix3f Rotation = svd.matrixV() * svd.matrixU().transpose();
587 
588  // float conditioning =
589  // svd.singularValues().maxCoeff()/svd.singularValues().minCoeff();
590  // if(conditioning > 100)
591  // {
592  // cout << " ConsistencyTest::initPose -> Bad conditioning: " <<
593  // conditioning << " -> Returning the identity\n";
594  // return Eigen::Matrix4f::Identity();
595  // }
596 
597  double det = Rotation.determinant();
598  if (det != 1)
599  {
600  Eigen::Matrix3f aux;
601  aux << 1, 0, 0, 0, 1, 0, 0, 0, det;
602  Rotation = svd.matrixV() * aux * svd.matrixU().transpose();
603  }
604 
605  // Calculate translation
606  Vector3f translation;
607  Matrix3f hessian = Matrix3f::Zero();
608  Vector3f gradient = Vector3f::Zero();
609  hessian(0, 0) = 1;
610  for (unsigned i = 0; i < 3; i++)
611  {
612  float trans_error =
613  (matched_planes(3, i) - matched_planes(7, i)); //+n*t
614  // hessian += matched_planes.block(i,0,1,3) *
615  // matched_planes.block(i,0,1,3).transpose();
616  // gradient += matched_planes.block(i,0,1,3) * trans_error;
617  Vector3f n_i = Vector3f(
618  matched_planes(0, i), matched_planes(1, i), matched_planes(2, i));
619  hessian += n_i * n_i.transpose();
620  gradient += n_i * trans_error;
621  }
622  translation = -hessian.inverse() * gradient;
623  // cout << "Previous average translation error " << sumError /
624  // matched_planes.size() << endl;
625 
626  // // Form SE3 transformation matrix. This matrix maps the model into the
627  // current data reference frame
628  // Eigen::Matrix4f rigidTransf;
629  // rigidTransf.block(0,0,3,3) = Rotation;
630  // rigidTransf.block(0,3,3,1) = translation;
631  // rigidTransf.row(3) << 0,0,0,1;
632 
633  CMatrixDouble rigidTransf(4, 4);
634  rigidTransf(0, 0) = Rotation(0, 0);
635  rigidTransf(0, 1) = Rotation(0, 1);
636  rigidTransf(0, 2) = Rotation(0, 2);
637  rigidTransf(1, 0) = Rotation(1, 0);
638  rigidTransf(1, 1) = Rotation(1, 1);
639  rigidTransf(1, 2) = Rotation(1, 2);
640  rigidTransf(2, 0) = Rotation(2, 0);
641  rigidTransf(2, 1) = Rotation(2, 1);
642  rigidTransf(2, 2) = Rotation(2, 2);
643  rigidTransf(0, 3) = translation(0);
644  rigidTransf(1, 3) = translation(1);
645  rigidTransf(2, 3) = translation(2);
646  rigidTransf(3, 0) = 0;
647  rigidTransf(3, 1) = 0;
648  rigidTransf(3, 2) = 0;
649  rigidTransf(3, 3) = 1;
650 
651  return rigidTransf;
652 }
653 
654 // Ransac functions to detect outliers in the plane matching
656  const CMatrixDouble& planeCorresp, const std::vector<size_t>& useIndices,
657  vector<CMatrixDouble>& fitModels)
658 // vector< Eigen::Matrix4f > &fitModels )
659 {
660  ASSERT_(useIndices.size() == 3);
661 
662  try
663  {
664  CMatrixDouble corresp(8, 3);
665 
666  // cout << "Size planeCorresp: " << endl;
667  // cout << "useIndices " << useIndices[0] << " " << useIndices[1] << "
668  // " << useIndices[2] << endl;
669  for (unsigned i = 0; i < 3; i++)
670  corresp.col(i) = planeCorresp.col(useIndices[i]);
671 
672  fitModels.resize(1);
673  // Eigen::Matrix4f &M = fitModels[0];
674  CMatrixDouble& M = fitModels[0];
675  M = getAlignment(corresp);
676  }
677  catch (exception&)
678  {
679  fitModels.clear();
680  return;
681  }
682 }
683 
685  const CMatrixDouble& planeCorresp, const vector<CMatrixDouble>& testModels,
686  const double distanceThreshold, unsigned int& out_bestModelIndex,
687  std::vector<size_t>& out_inlierIndices)
688 {
689  ASSERT_(testModels.size() == 1);
690  out_bestModelIndex = 0;
691  const CMatrixDouble& M = testModels[0];
692 
693  Eigen::Matrix3f Rotation;
694  Rotation << M(0, 0), M(0, 1), M(0, 2), M(1, 0), M(1, 1), M(1, 2), M(2, 0),
695  M(2, 1), M(2, 2);
696  Eigen::Vector3f translation;
697  translation << M(0, 3), M(1, 3), M(2, 3);
698 
699  ASSERT_(M.rows() == 4 && M.cols() == 4);
700 
701  const size_t N = planeCorresp.cols();
702  out_inlierIndices.clear();
703  out_inlierIndices.reserve(100);
704  for (size_t i = 0; i < N; i++)
705  {
706  const Eigen::Vector3f n_i = Eigen::Vector3f(
707  planeCorresp(0, i), planeCorresp(1, i), planeCorresp(2, i));
708  const Eigen::Vector3f n_ii =
709  Rotation *
710  Eigen::Vector3f(
711  planeCorresp(4, i), planeCorresp(5, i), planeCorresp(6, i));
712  const float d_error = fabs(
713  (planeCorresp(7, i) - translation.dot(n_i)) - planeCorresp(3, i));
714  const float angle_error = (n_i.cross(n_ii)).norm();
715 
716  if (d_error < distanceThreshold)
717  if (angle_error < distanceThreshold) // Warning: this threshold has
718  // a different dimension
719  out_inlierIndices.push_back(i);
720  }
721 }
722 
723 /** Return "true" if the selected points are a degenerate (invalid) case.
724  */
726  const CMatrixDouble& planeCorresp, const std::vector<size_t>& useIndices)
727 {
728  ASSERT_(useIndices.size() == 3);
729 
730  const Eigen::Vector3f n_1 = Eigen::Vector3f(
731  planeCorresp(0, useIndices[0]), planeCorresp(1, useIndices[0]),
732  planeCorresp(2, useIndices[0]));
733  const Eigen::Vector3f n_2 = Eigen::Vector3f(
734  planeCorresp(0, useIndices[1]), planeCorresp(1, useIndices[1]),
735  planeCorresp(2, useIndices[1]));
736  const Eigen::Vector3f n_3 = Eigen::Vector3f(
737  planeCorresp(0, useIndices[2]), planeCorresp(1, useIndices[2]),
738  planeCorresp(2, useIndices[2]));
739  // cout << "degenerate " << useIndices[0] << " " << useIndices[1] << " " <<
740  // useIndices[2] << " - " << fabs(n_1. dot( n_2. cross(n_3) ) ) << endl;
741 
742  if (fabs(n_1.dot(n_2.cross(n_3))) < 0.9) return true;
743 
744  return false;
745 }
746 
747 // ------------------------------------------------------
748 // TestRANSAC
749 // ------------------------------------------------------
751  std::map<unsigned, unsigned>& matched_planes)
752 {
753  // assert(matched_planes.size() >= 3);
754  // CTicTac tictac;
755 
756  if (matched_planes.size() <= 3)
757  {
758  cout << "Insuficient matched planes " << matched_planes.size() << endl;
759  return Eigen::Matrix4f::Identity();
760  }
761 
762  CMatrixDouble planeCorresp(8, matched_planes.size());
763  unsigned col = 0;
764  for (auto it = matched_planes.begin(); it != matched_planes.end();
765  it++, col++)
766  {
767  planeCorresp(0, col) = PBMSource.vPlanes[it->first].v3normal(0);
768  planeCorresp(1, col) = PBMSource.vPlanes[it->first].v3normal(1);
769  planeCorresp(2, col) = PBMSource.vPlanes[it->first].v3normal(2);
770  planeCorresp(3, col) = PBMSource.vPlanes[it->first].d;
771  planeCorresp(4, col) = PBMTarget.vPlanes[it->second].v3normal(0);
772  planeCorresp(5, col) = PBMTarget.vPlanes[it->second].v3normal(1);
773  planeCorresp(6, col) = PBMTarget.vPlanes[it->second].v3normal(2);
774  planeCorresp(7, col) = PBMTarget.vPlanes[it->second].d;
775  }
776  // cout << "Size " << matched_planes.size() << " " << size(1) << endl;
777 
778  std::vector<size_t> inliers;
779  // Eigen::Matrix4f best_model;
780  CMatrixDouble best_model;
781 
782  math::RANSAC ransac_executer;
783  ransac_executer.execute(
786  3, // Minimum set of points
787  inliers, best_model, 0.99999);
788 
789  // cout << "Computation time: " << tictac.Tac()*1000.0/TIMES << " ms" <<
790  // endl;
791 
792  cout << "Size planeCorresp: " << planeCorresp.cols() << endl;
793  cout << "RANSAC finished: " << inliers.size() << " inliers: " << inliers
794  << " . \nBest model: \n"
795  << best_model << endl;
796  // cout << "Best inliers: " << best_inliers << endl;
797 
798  Eigen::Matrix4f rigidTransf;
799  rigidTransf << best_model(0, 0), best_model(0, 1), best_model(0, 2),
800  best_model(0, 3), best_model(1, 0), best_model(1, 1), best_model(1, 2),
801  best_model(1, 3), best_model(2, 0), best_model(2, 1), best_model(2, 2),
802  best_model(2, 3), 0, 0, 0, 1;
803 
804  // return best_model;
805  return rigidTransf;
806 }
807 
808 // using namespace mrpt;
809 // ////using namespace mrpt::gui;
810 // using namespace mrpt::math;
811 // using namespace mrpt::random;
812 // using namespace std;
813 //
814 // void ransac3Dplane_fit(
815 // const CMatrixDouble &allData,
816 // const std::vector<size_t> &useIndices,
817 // vector< CMatrixDouble > &fitModels )
818 //{
819 // ASSERT_(useIndices.size()==3);
820 //
821 // TPoint3D p1(
822 // allData(0,useIndices[0]),allData(1,useIndices[0]),allData(2,useIndices[0]) );
823 // TPoint3D p2(
824 // allData(0,useIndices[1]),allData(1,useIndices[1]),allData(2,useIndices[1]) );
825 // TPoint3D p3(
826 // allData(0,useIndices[2]),allData(1,useIndices[2]),allData(2,useIndices[2]) );
827 //
828 // try
829 // {
830 // TPlane plane( p1,p2,p3 );
831 // fitModels.resize(1);
832 // CMatrixDouble &M = fitModels[0];
833 //
834 // M.setSize(1,4);
835 // for (size_t i=0;i<4;i++)
836 // M(0,i)=plane.coefs[i];
837 // }
838 // catch(exception &)
839 // {
840 // fitModels.clear();
841 // return;
842 // }
843 //
844 //
845 //
846 //}
847 //
848 // void ransac3Dplane_distance(
849 // const CMatrixDouble &allData,
850 // const vector< CMatrixDouble > & testModels,
851 // const double distanceThreshold,
852 // unsigned int & out_bestModelIndex,
853 // std::vector<size_t> & out_inlierIndices )
854 //{
855 // ASSERT_( testModels.size()==1 )
856 // out_bestModelIndex = 0;
857 // const CMatrixDouble &M = testModels[0];
858 //
859 // ASSERT_( size(M,1)==1 && size(M,2)==4 )
860 //
861 // TPlane plane;
862 // plane.coefs[0] = M(0,0);
863 // plane.coefs[1] = M(0,1);
864 // plane.coefs[2] = M(0,2);
865 // plane.coefs[3] = M(0,3);
866 //
867 // const size_t N = size(allData,2);
868 // out_inlierIndices.clear();
869 // out_inlierIndices.reserve(100);
870 // for (size_t i=0;i<N;i++)
871 // {
872 // const double d = plane.distance( TPoint3D(
873 // allData(0,i),allData(1,i),allData(2,i) ) );
874 // if (d<distanceThreshold)
875 // out_inlierIndices.push_back(i);
876 // }
877 //}
878 //
879 ///** Return "true" if the selected points are a degenerate (invalid) case.
880 // */
881 // bool ransac3Dplane_degenerate(
882 // const CMatrixDouble &allData,
883 // const std::vector<size_t> &useIndices )
884 //{
885 // return false;
886 //}
887 //
888 //
889 //// ------------------------------------------------------
890 //// TestRANSAC
891 //// ------------------------------------------------------
892 // void ConsistencyTest::TestRANSAC()
893 //{
894 // getRandomGenerator().randomize();
895 //
896 // // Generate random points:
897 // // ------------------------------------
898 // const size_t N_plane = 300;
899 // const size_t N_noise = 100;
900 //
901 // const double PLANE_EQ[4]={ 1,-1,1, -2 };
902 //
903 // CMatrixDouble data(3,N_plane+N_noise);
904 // for (size_t i=0;i<N_plane;i++)
905 // {
906 // const double xx = getRandomGenerator().drawUniform(-3,3);
907 // const double yy = getRandomGenerator().drawUniform(-3,3);
908 // const double zz =
909 //-(PLANE_EQ[3]+PLANE_EQ[0]*xx+PLANE_EQ[1]*yy)/PLANE_EQ[2];
910 // data(0,i) = xx;
911 // data(1,i) = yy;
912 // data(2,i) = zz;
913 // }
914 //
915 // for (size_t i=0;i<N_noise;i++)
916 // {
917 // data(0,i+N_plane) = getRandomGenerator().drawUniform(-4,4);
918 // data(1,i+N_plane) = getRandomGenerator().drawUniform(-4,4);
919 // data(2,i+N_plane) = getRandomGenerator().drawUniform(-4,4);
920 // }
921 //
922 //
923 // // Run RANSAC
924 // // ------------------------------------
925 // CMatrixDouble best_model;
926 // std::vector<size_t> best_inliers;
927 // const double DIST_THRESHOLD = 0.2;
928 //
929 //
930 // CTicTac tictac;
931 // const size_t TIMES=100;
932 //
933 // for (size_t iters=0;iters<TIMES;iters++)
934 // math::RANSAC::execute(
935 // data,
936 // ransac3Dplane_fit,
937 // ransac3Dplane_distance,
938 // ransac3Dplane_degenerate,
939 // DIST_THRESHOLD,
940 // 3, // Minimum set of points
941 // best_inliers,
942 // best_model,
943 // iters==0 // Verbose
944 // );
945 //
946 // cout << "Computation time: " << tictac.Tac()*1000.0/TIMES << " ms" << endl;
947 //
948 // ASSERT_(size(best_model,1)==1 && size(best_model,2)==4)
949 //
950 // cout << "RANSAC finished: Best model: " << best_model << endl;
951 //// cout << "Best inliers: " << best_inliers << endl;
952 //
953 // TPlane plane( best_model(0,0),
954 // best_model(0,1),best_model(0,2),best_model(0,3) );
955 //
956 //
957 //}
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Eigen::Matrix4f estimatePose(std::map< unsigned, unsigned > &matched_planes)
auto col(int colIdx)
Definition: MatrixBase.h:89
bool ransac3Dplane_degenerate(const CMatrixDouble &planeCorresp, const std::vector< size_t > &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
Eigen::Matrix4f estimatePoseRANSAC(std::map< unsigned, unsigned > &matched_planes)
STL namespace.
void ransac3Dplane_distance(const CMatrixDouble &planeCorresp, const vector< CMatrixDouble > &testModels, const double distanceThreshold, unsigned int &out_bestModelIndex, std::vector< size_t > &out_inlierIndices)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
Scalar dot(const CVectorDynamic< Scalar > &v) const
dot product of this \cdot v
const GLubyte * c
Definition: glext.h:6406
void ransacPlaneAlignment_fit(const CMatrixDouble &planeCorresp, const std::vector< size_t > &useIndices, vector< CMatrixDouble > &fitModels)
Eigen::Matrix4f initPose2D(std::map< unsigned, unsigned > &matched_planes)
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
CMatrixDouble getAlignment(const CMatrixDouble &matched_planes)
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
GLsizei GLboolean transpose
Definition: glext.h:4150
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double calcAlignmentError(std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf)
! Get diamond of points around the center.
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
Eigen::Matrix4f initPose(std::map< unsigned, unsigned > &matched_planes)
std::map< unsigned, unsigned > matched_planes
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:54
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:28
bool execute(const CMatrixDynamic< NUMTYPE > &data, const TRansacFitFunctor &fit_func, const TRansacDistanceFunctor &dist_func, const TRansacDegenerateFunctor &degen_func, const double distanceThreshold, const unsigned int minimumSizeSamplesToFit, std::vector< size_t > &out_best_inliers, CMatrixDynamic< NUMTYPE > &out_best_model, const double prob_good_sample=0.999, const size_t maxIter=2000) const
An implementation of the RANSAC algorithm for robust fitting of models to data.
Definition: ransac.cpp:24
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:44
CONTAINER::Scalar norm(const CONTAINER &v)
bool estimatePoseWithCovariance(std::map< unsigned, unsigned > &matched_planes, Eigen::Matrix4f &rigidTransf, Eigen::Matrix< float, 6, 6 > &covarianceM)
Eigen::Matrix< dataType, 3, 1 > compose(Eigen::Matrix< dataType, 4, 4 > &pose, Eigen::Matrix< dataType, 3, 1 > &point)
Definition: Miscellaneous.h:55
Eigen::Matrix4f getRTwithModel(std::map< unsigned, unsigned > &matched_planes)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019