Main MRPT website > C++ reference for MRPT 1.9.9
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-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 #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::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 // "Closed-form solution of absolute orientation using unit quaternions", BKP
23 // 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
38 // products:
39 // pi1 = pLix'*pRix' pi2 = pLix'*pRiy' pi3 = pLix'*pRiz'
40 // pi4 = pLiy'*pRix' pi5 = pLiy'*pRiy' pi6 = pLiy'*pRiz'
41 // pi7 = pLiz'*pRix' pi8 = pLiz'*pRiy' pi9 = pLiz'*pRiz'
42 //
43 // 4. Compute S components:
44 // Sxx = sum{i}( pi1 ) Sxy = sum{i}( pi2 ) Sxz = sum{i}( pi3 )
45 // Syx = sum{i}( pi4 ) Syy = sum{i}( pi5 ) Syz = sum{i}( pi6 )
46 // Szx = sum{i}( pi7 ) Szy = sum{i}( pi8 ) Szz = sum{i}( pi9 )
47 //
48 // 5. Compute N components:
49 // [ Sxx+Syy+Szz Syz-Szy Szx-Sxz Sxy-Syx ]
50 // [ Syz-Szy Sxx-Syy-Szz Sxy+Syx Szx+Sxz ]
51 // N = [ Szx-Sxz Sxy+Syx -Sxx+Syy-Szz Syz+Szy ]
52 // [ Sxy-Syx Szx+Sxz Syz+Szy -Sxx-Syy+Szz ]
53 //
54 // 6. Rotation represented by the quaternion eigenvector correspondent to the
55 // higher eigenvalue of N
56 //
57 // 7. Scale computation (symmetric expression)
58 // s = sqrt( sum{i}( square(abs(pRi')) / sum{i}( square(abs(pLi')) ) )
59 //
60 // 8. Translation computation (distance between the Right centroid and the
61 // scaled and rotated Left centroid)
62 // t = ct_this-sR(ct_others)
63 
64 /*---------------------------------------------------------------
65  se3_l2 (old "HornMethod()")
66  ---------------------------------------------------------------*/
68  std::vector<mrpt::math::TPoint3D>&
69  points_this, // IN/OUT: It gets modified!
70  std::vector<mrpt::math::TPoint3D>&
71  points_other, // IN/OUT: It gets modified!
72  mrpt::poses::CPose3DQuat& out_transform,
73  double& out_scale, bool forceScaleToUnity)
74 {
76 
77  ASSERT_EQUAL_(points_this.size(), points_other.size());
78  // Compute the centroids
79  TPoint3D ct_others(0, 0, 0), ct_this(0, 0, 0);
80  const size_t nMatches = points_this.size();
81 
82  if (nMatches < 3)
83  return false; // Nothing we can estimate without 3 points!!
84 
85  for (size_t i = 0; i < nMatches; i++)
86  {
87  ct_others += points_other[i];
88  ct_this += points_this[i];
89  }
90 
91  const double F = 1.0 / nMatches;
92  ct_others *= F;
93  ct_this *= F;
94 
95  CMatrixDouble33 S; // Zeroed by default
96 
97  // Substract the centroid and compute the S matrix of cross products
98  for (size_t i = 0; i < nMatches; i++)
99  {
100  points_this[i] -= ct_this;
101  points_other[i] -= ct_others;
102 
103  S.get_unsafe(0, 0) += points_other[i].x * points_this[i].x;
104  S.get_unsafe(0, 1) += points_other[i].x * points_this[i].y;
105  S.get_unsafe(0, 2) += points_other[i].x * points_this[i].z;
106 
107  S.get_unsafe(1, 0) += points_other[i].y * points_this[i].x;
108  S.get_unsafe(1, 1) += points_other[i].y * points_this[i].y;
109  S.get_unsafe(1, 2) += points_other[i].y * points_this[i].z;
110 
111  S.get_unsafe(2, 0) += points_other[i].z * points_this[i].x;
112  S.get_unsafe(2, 1) += points_other[i].z * points_this[i].y;
113  S.get_unsafe(2, 2) += points_other[i].z * points_this[i].z;
114  }
115 
116  // Construct the N matrix
117  CMatrixDouble44 N; // Zeroed by default
118 
119  N.set_unsafe(
120  0, 0, S.get_unsafe(0, 0) + S.get_unsafe(1, 1) + S.get_unsafe(2, 2));
121  N.set_unsafe(0, 1, S.get_unsafe(1, 2) - S.get_unsafe(2, 1));
122  N.set_unsafe(0, 2, S.get_unsafe(2, 0) - S.get_unsafe(0, 2));
123  N.set_unsafe(0, 3, S.get_unsafe(0, 1) - S.get_unsafe(1, 0));
124 
125  N.set_unsafe(1, 0, N.get_unsafe(0, 1));
126  N.set_unsafe(
127  1, 1, S.get_unsafe(0, 0) - S.get_unsafe(1, 1) - S.get_unsafe(2, 2));
128  N.set_unsafe(1, 2, S.get_unsafe(0, 1) + S.get_unsafe(1, 0));
129  N.set_unsafe(1, 3, S.get_unsafe(2, 0) + S.get_unsafe(0, 2));
130 
131  N.set_unsafe(2, 0, N.get_unsafe(0, 2));
132  N.set_unsafe(2, 1, N.get_unsafe(1, 2));
133  N.set_unsafe(
134  2, 2, -S.get_unsafe(0, 0) + S.get_unsafe(1, 1) - S.get_unsafe(2, 2));
135  N.set_unsafe(2, 3, S.get_unsafe(1, 2) + S.get_unsafe(2, 1));
136 
137  N.set_unsafe(3, 0, N.get_unsafe(0, 3));
138  N.set_unsafe(3, 1, N.get_unsafe(1, 3));
139  N.set_unsafe(3, 2, N.get_unsafe(2, 3));
140  N.set_unsafe(
141  3, 3, -S.get_unsafe(0, 0) - S.get_unsafe(1, 1) + S.get_unsafe(2, 2));
142 
143  // q is the quaternion correspondent to the greatest eigenvector of the N
144  // matrix (last column in Z)
145  CMatrixDouble44 Z, D;
146  vector<double> v;
147 
148  N.eigenVectors(Z, D);
149  Z.extractCol(Z.cols() - 1, v);
150 
151  ASSERTDEB_(
152  fabs(
153  sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]) - 1.0) <
154  0.1);
155 
156  // Make q_r > 0
157  if (v[0] < 0)
158  {
159  v[0] *= -1;
160  v[1] *= -1;
161  v[2] *= -1;
162  v[3] *= -1;
163  }
164 
165  // out_transform: Create a pose rotation with the quaternion
166  for (unsigned int i = 0; i < 4; i++) // insert the quaternion part
167  out_transform[i + 3] = v[i];
168 
169  // Compute scale
170  double s;
171  if (forceScaleToUnity)
172  {
173  s = 1.0; // Enforce scale to be 1
174  }
175  else
176  {
177  double num = 0.0;
178  double den = 0.0;
179  for (size_t i = 0; i < nMatches; i++)
180  {
181  num += square(points_other[i].x) + square(points_other[i].y) +
182  square(points_other[i].z);
183  den += square(points_this[i].x) + square(points_this[i].y) +
184  square(points_this[i].z);
185  } // end-for
186 
187  // The scale:
188  s = std::sqrt(num / den);
189  }
190 
191  TPoint3D pp;
192  out_transform.composePoint(
193  ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z);
194  pp *= s;
195 
196  out_transform[0] = ct_this.x - pp.x; // X
197  out_transform[1] = ct_this.y - pp.y; // Y
198  out_transform[2] = ct_this.z - pp.z; // Z
199 
200  out_scale = s; // return scale
201  return true;
202 
203  MRPT_END
204 } // end se3_l2_internal()
205 
207  const std::vector<mrpt::math::TPoint3D>& in_points_this,
208  const std::vector<mrpt::math::TPoint3D>& in_points_other,
209  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
210  bool forceScaleToUnity)
211 {
212  // make a copy because we need it anyway to substract the centroid and to
213  // provide a unified interface to TMatchingList API
214  std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
215  std::vector<mrpt::math::TPoint3D> points_other = in_points_other;
216 
217  return se3_l2_internal(
218  points_this, points_other, out_transform, out_scale, forceScaleToUnity);
219 }
220 
222  const mrpt::tfest::TMatchingPairList& corrs,
223  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
224  bool forceScaleToUnity)
225 {
226  // Transform data types:
227  const size_t N = corrs.size();
228  std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
229  for (size_t i = 0; i < N; i++)
230  {
231  points_this[i].x = corrs[i].this_x;
232  points_this[i].y = corrs[i].this_y;
233  points_this[i].z = corrs[i].this_z;
234  points_other[i].x = corrs[i].other_x;
235  points_other[i].y = corrs[i].other_y;
236  points_other[i].z = corrs[i].other_z;
237  }
238  return se3_l2_internal(
239  points_this, points_other, out_transform, out_scale, forceScaleToUnity);
240 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
STL namespace.
GLdouble s
Definition: glext.h:3676
GLuint GLuint num
Definition: glext.h:7278
T square(const T x)
Inline function for the square of a number.
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=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
This base provides a set of functions for maths stuff.
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:67
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
A list of TMatchingPair.
Definition: TMatchingPair.h:83
double x
X,Y,Z coordinates.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
const GLdouble * v
Definition: glext.h:3678
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.
Definition: exceptions.h:205
#define MRPT_END
Definition: exceptions.h:266
bool se3_l2(const mrpt::tfest::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:221
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
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.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019