Main MRPT website > C++ reference for MRPT 1.5.7
se3_l2.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 #include "tfest-precomp.h" // Precompiled headers
11 
12 #include <mrpt/tfest/se3.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::tfest;
18 using namespace mrpt::utils;
19 using namespace mrpt::poses;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 // "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987.
24 // Algorithm:
25 // 0. Preliminary
26 // pLi = { pLix, pLiy, pLiz }
27 // pRi = { pRix, pRiy, pRiz }
28 // -------------------------------------------------------
29 // 1. Find the centroids of the two sets of measurements:
30 // ct_others = (1/n)*sum{i}( pLi ) ct_others = { cLx, cLy, cLz }
31 // ct_this = (1/n)*sum{i}( pRi ) ct_this = { cRx, cRy, cRz }
32 //
33 // 2. Substract centroids from the point coordinates:
34 // pLi' = pLi - ct_others pLi' = { pLix', pLiy', pLiz' }
35 // pRi' = pRi - ct_this pRi' = { pRix', pRiy', pRiz' }
36 //
37 // 3. For each pair of coordinates (correspondences) compute the nine possible products:
38 // pi1 = pLix'*pRix' pi2 = pLix'*pRiy' pi3 = pLix'*pRiz'
39 // pi4 = pLiy'*pRix' pi5 = pLiy'*pRiy' pi6 = pLiy'*pRiz'
40 // pi7 = pLiz'*pRix' pi8 = pLiz'*pRiy' pi9 = pLiz'*pRiz'
41 //
42 // 4. Compute S components:
43 // Sxx = sum{i}( pi1 ) Sxy = sum{i}( pi2 ) Sxz = sum{i}( pi3 )
44 // Syx = sum{i}( pi4 ) Syy = sum{i}( pi5 ) Syz = sum{i}( pi6 )
45 // Szx = sum{i}( pi7 ) Szy = sum{i}( pi8 ) Szz = sum{i}( pi9 )
46 //
47 // 5. Compute N components:
48 // [ Sxx+Syy+Szz Syz-Szy Szx-Sxz Sxy-Syx ]
49 // [ Syz-Szy Sxx-Syy-Szz Sxy+Syx Szx+Sxz ]
50 // N = [ Szx-Sxz Sxy+Syx -Sxx+Syy-Szz Syz+Szy ]
51 // [ Sxy-Syx Szx+Sxz Syz+Szy -Sxx-Syy+Szz ]
52 //
53 // 6. Rotation represented by the quaternion eigenvector correspondent to the higher eigenvalue of N
54 //
55 // 7. Scale computation (symmetric expression)
56 // s = sqrt( sum{i}( square(abs(pRi')) / sum{i}( square(abs(pLi')) ) )
57 //
58 // 8. Translation computation (distance between the Right centroid and the scaled and rotated Left centroid)
59 // t = ct_this-sR(ct_others)
60 
61 /*---------------------------------------------------------------
62  se3_l2 (old "HornMethod()")
63  ---------------------------------------------------------------*/
65  std::vector<mrpt::math::TPoint3D> & points_this, // IN/OUT: It gets modified!
66  std::vector<mrpt::math::TPoint3D> & points_other, // IN/OUT: It gets modified!
67  mrpt::poses::CPose3DQuat & out_transform,
68  double & out_scale,
69  bool forceScaleToUnity)
70 {
72 
73  ASSERT_EQUAL_(points_this.size(),points_other.size())
74 
75  // Compute the centroids
76  TPoint3D ct_others(0,0,0), ct_this(0,0,0);
77  const size_t nMatches = points_this.size();
78 
79  if (nMatches<3)
80  return false; // Nothing we can estimate without 3 points!!
81 
82  for(size_t i = 0; i < nMatches; i++ )
83  {
84  ct_others += points_other[i];
85  ct_this += points_this [i];
86  }
87 
88  const double F = 1.0/nMatches;
89  ct_others *= F;
90  ct_this *= F;
91 
92  CMatrixDouble33 S; // Zeroed by default
93 
94  // Substract the centroid and compute the S matrix of cross products
95  for(size_t i = 0; i < nMatches; i++ )
96  {
97  points_this[i] -= ct_this;
98  points_other[i] -= ct_others;
99 
100  S.get_unsafe(0,0) += points_other[i].x * points_this[i].x;
101  S.get_unsafe(0,1) += points_other[i].x * points_this[i].y;
102  S.get_unsafe(0,2) += points_other[i].x * points_this[i].z;
103 
104  S.get_unsafe(1,0) += points_other[i].y * points_this[i].x;
105  S.get_unsafe(1,1) += points_other[i].y * points_this[i].y;
106  S.get_unsafe(1,2) += points_other[i].y * points_this[i].z;
107 
108  S.get_unsafe(2,0) += points_other[i].z * points_this[i].x;
109  S.get_unsafe(2,1) += points_other[i].z * points_this[i].y;
110  S.get_unsafe(2,2) += points_other[i].z * points_this[i].z;
111  }
112 
113  // Construct the N matrix
114  CMatrixDouble44 N; // Zeroed by default
115 
116  N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) );
117  N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) );
118  N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) );
119  N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) );
120 
121  N.set_unsafe( 1,0,N.get_unsafe(0,1) );
122  N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) );
123  N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) );
124  N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) );
125 
126  N.set_unsafe( 2,0,N.get_unsafe(0,2) );
127  N.set_unsafe( 2,1,N.get_unsafe(1,2) );
128  N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) );
129  N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) );
130 
131  N.set_unsafe( 3,0,N.get_unsafe(0,3) );
132  N.set_unsafe( 3,1,N.get_unsafe(1,3) );
133  N.set_unsafe( 3,2,N.get_unsafe(2,3) );
134  N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) );
135 
136  // q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z)
137  CMatrixDouble44 Z, D;
138  vector<double> v;
139 
140  N.eigenVectors( Z, D );
141  Z.extractCol( Z.getColCount()-1, v );
142 
143  ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 );
144 
145  // Make q_r > 0
146  if( v[0] < 0 ){ v[0] *= -1; v[1] *= -1; v[2] *= -1; v[3] *= -1; }
147 
148  // out_transform: Create a pose rotation with the quaternion
149  for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part
150  out_transform[i+3] = v[i];
151 
152  // Compute scale
153  double s;
154  if( forceScaleToUnity )
155  {
156  s = 1.0; // Enforce scale to be 1
157  }
158  else
159  {
160  double num = 0.0;
161  double den = 0.0;
162  for(size_t i = 0; i < nMatches; i++ )
163  {
164  num += square( points_other[i].x ) + square( points_other[i].y ) + square( points_other[i].z );
165  den += square( points_this[i].x ) + square( points_this[i].y ) + square( points_this[i].z );
166  } // end-for
167 
168  // The scale:
169  s = std::sqrt( num/den );
170  }
171 
172  TPoint3D pp;
173  out_transform.composePoint( ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z );
174  pp*=s;
175 
176  out_transform[0] = ct_this.x - pp.x; // X
177  out_transform[1] = ct_this.y - pp.y; // Y
178  out_transform[2] = ct_this.z - pp.z; // Z
179 
180  out_scale=s; // return scale
181  return true;
182 
183  MRPT_END
184 } // end se3_l2_internal()
185 
186 
188  const std::vector<mrpt::math::TPoint3D> & in_points_this,
189  const std::vector<mrpt::math::TPoint3D> & in_points_other,
190  mrpt::poses::CPose3DQuat & out_transform,
191  double & out_scale,
192  bool forceScaleToUnity)
193 {
194  // make a copy because we need it anyway to substract the centroid and to provide a unified interface to TMatchingList API
195  std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
196  std::vector<mrpt::math::TPoint3D> points_other= in_points_other;
197 
198  return se3_l2_internal(points_this,points_other,out_transform,out_scale,forceScaleToUnity);
199 }
200 
202  const mrpt::utils::TMatchingPairList & corrs,
203  mrpt::poses::CPose3DQuat & out_transform,
204  double & out_scale,
205  bool forceScaleToUnity)
206 {
207  // Transform data types:
208  const size_t N = corrs.size();
209  std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
210  for (size_t i=0;i<N;i++)
211  {
212  points_this[i].x = corrs[i].this_x;
213  points_this[i].y = corrs[i].this_y;
214  points_this[i].z = corrs[i].this_z;
215  points_other[i].x = corrs[i].other_x;
216  points_other[i].y = corrs[i].other_y;
217  points_other[i].z = corrs[i].other_z;
218  }
219  return se3_l2_internal(points_this,points_other,out_transform,out_scale,forceScaleToUnity);
220 }
221 
#define ASSERT_EQUAL_(__A, __B)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble z
Definition: glext.h:3734
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point G such as .
STL namespace.
double z
X,Y,Z coordinates.
GLdouble s
Definition: glext.h:3602
GLuint GLuint num
Definition: glext.h:6303
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
bool se3_l2_internal(std::vector< mrpt::math::TPoint3D > &points_this, std::vector< mrpt::math::TPoint3D > &points_other, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity)
Definition: se3_l2.cpp:64
A list of TMatchingPair.
Definition: TMatchingPair.h:78
bool TFEST_IMPEXP se3_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2.cpp:201
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define MRPT_START
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
GLenum GLint GLint y
Definition: glext.h:3516
GLenum GLint x
Definition: glext.h:3516
Lightweight 3D point.
Functions for estimating the optimal transformation between two frames of references given measuremen...



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