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



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