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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST