MRPT  1.9.9
se3_l2.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "tfest-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/CPose3DQuat.h>
14 #include <mrpt/tfest/se3.h>
15 #include <Eigen/Dense>
16 
17 using namespace mrpt;
18 using namespace mrpt::tfest;
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
24 // Horn, Journal of the Optical Society of America, 1987.
25 // Algorithm:
26 // 0. Preliminary
27 // pLi = { pLix, pLiy, pLiz }
28 // pRi = { pRix, pRiy, pRiz }
29 // -------------------------------------------------------
30 // 1. Find the centroids of the two sets of measurements:
31 // ct_others = (1/n)*sum{i}( pLi ) ct_others = { cLx, cLy, cLz }
32 // ct_this = (1/n)*sum{i}( pRi ) ct_this = { cRx, cRy, cRz }
33 //
34 // 2. Substract centroids from the point coordinates:
35 // pLi' = pLi - ct_others pLi' = { pLix', pLiy', pLiz' }
36 // pRi' = pRi - ct_this pRi' = { pRix', pRiy', pRiz' }
37 //
38 // 3. For each pair of coordinates (correspondences) compute the nine possible
39 // products:
40 // pi1 = pLix'*pRix' pi2 = pLix'*pRiy' pi3 = pLix'*pRiz'
41 // pi4 = pLiy'*pRix' pi5 = pLiy'*pRiy' pi6 = pLiy'*pRiz'
42 // pi7 = pLiz'*pRix' pi8 = pLiz'*pRiy' pi9 = pLiz'*pRiz'
43 //
44 // 4. Compute S components:
45 // Sxx = sum{i}( pi1 ) Sxy = sum{i}( pi2 ) Sxz = sum{i}( pi3 )
46 // Syx = sum{i}( pi4 ) Syy = sum{i}( pi5 ) Syz = sum{i}( pi6 )
47 // Szx = sum{i}( pi7 ) Szy = sum{i}( pi8 ) Szz = sum{i}( pi9 )
48 //
49 // 5. Compute N components:
50 // [ Sxx+Syy+Szz Syz-Szy Szx-Sxz Sxy-Syx ]
51 // [ Syz-Szy Sxx-Syy-Szz Sxy+Syx Szx+Sxz ]
52 // N = [ Szx-Sxz Sxy+Syx -Sxx+Syy-Szz Syz+Szy ]
53 // [ Sxy-Syx Szx+Sxz Syz+Szy -Sxx-Syy+Szz ]
54 //
55 // 6. Rotation represented by the quaternion eigenvector correspondent to the
56 // higher eigenvalue of N
57 //
58 // 7. Scale computation (symmetric expression)
59 // s = sqrt( sum{i}( square(abs(pRi')) / sum{i}( square(abs(pLi')) ) )
60 //
61 // 8. Translation computation (distance between the Right centroid and the
62 // scaled and rotated Left centroid)
63 // t = ct_this-sR(ct_others)
64 
65 /*---------------------------------------------------------------
66  se3_l2 (old "HornMethod()")
67  ---------------------------------------------------------------*/
69  std::vector<mrpt::math::TPoint3D>&
70  points_this, // IN/OUT: It gets modified!
71  std::vector<mrpt::math::TPoint3D>&
72  points_other, // IN/OUT: It gets modified!
73  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
74  bool forceScaleToUnity)
75 {
77 
78  ASSERT_EQUAL_(points_this.size(), points_other.size());
79  // Compute the centroids
80  TPoint3D ct_others(0, 0, 0), ct_this(0, 0, 0);
81  const size_t nMatches = points_this.size();
82 
83  if (nMatches < 3)
84  return false; // Nothing we can estimate without 3 points!!
85 
86  for (size_t i = 0; i < nMatches; i++)
87  {
88  ct_others += points_other[i];
89  ct_this += points_this[i];
90  }
91 
92  const double F = 1.0 / nMatches;
93  ct_others *= F;
94  ct_this *= F;
95 
96  CMatrixDouble33 S; // Zeroed by default
97 
98  // Substract the centroid and compute the S matrix of cross products
99  for (size_t i = 0; i < nMatches; i++)
100  {
101  points_this[i] -= ct_this;
102  points_other[i] -= ct_others;
103 
104  S(0, 0) += points_other[i].x * points_this[i].x;
105  S(0, 1) += points_other[i].x * points_this[i].y;
106  S(0, 2) += points_other[i].x * points_this[i].z;
107 
108  S(1, 0) += points_other[i].y * points_this[i].x;
109  S(1, 1) += points_other[i].y * points_this[i].y;
110  S(1, 2) += points_other[i].y * points_this[i].z;
111 
112  S(2, 0) += points_other[i].z * points_this[i].x;
113  S(2, 1) += points_other[i].z * points_this[i].y;
114  S(2, 2) += points_other[i].z * points_this[i].z;
115  }
116 
117  // Construct the N matrix
118  CMatrixDouble44 N; // Zeroed by default
119 
120  N(0, 0) = S(0, 0) + S(1, 1) + S(2, 2);
121  N(0, 1) = S(1, 2) - S(2, 1);
122  N(0, 2) = S(2, 0) - S(0, 2);
123  N(0, 3) = S(0, 1) - S(1, 0);
124 
125  N(1, 0) = N(0, 1);
126  N(1, 1) = S(0, 0) - S(1, 1) - S(2, 2);
127  N(1, 2) = S(0, 1) + S(1, 0);
128  N(1, 3) = S(2, 0) + S(0, 2);
129 
130  N(2, 0) = N(0, 2);
131  N(2, 1) = N(1, 2);
132  N(2, 2) = -S(0, 0) + S(1, 1) - S(2, 2);
133  N(2, 3) = S(1, 2) + S(2, 1);
134 
135  N(3, 0) = N(0, 3);
136  N(3, 1) = N(1, 3);
137  N(3, 2) = N(2, 3);
138  N(3, 3) = -S(0, 0) - S(1, 1) + S(2, 2);
139 
140  // q is the quaternion correspondent to the greatest eigenvector of the N
141  // matrix (last column in Z)
142  CMatrixDouble44 Z;
143  vector<double> eigvals;
144  N.eig_symmetric(Z, eigvals, true /*sorted*/);
145 
146  auto v = CVectorFixedDouble<4>(Z.col(3).eval());
147 
148  ASSERTDEB_(
149  fabs(
150  sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]) - 1.0) <
151  0.1);
152 
153  // Make q_r > 0
154  if (v[0] < 0)
155  {
156  v[0] *= -1;
157  v[1] *= -1;
158  v[2] *= -1;
159  v[3] *= -1;
160  }
161 
162  // out_transform: Create a pose rotation with the quaternion
163  for (unsigned int i = 0; i < 4; i++) // insert the quaternion part
164  out_transform[i + 3] = v[i];
165 
166  // Compute scale
167  double s;
168  if (forceScaleToUnity)
169  {
170  s = 1.0; // Enforce scale to be 1
171  }
172  else
173  {
174  double num = 0.0;
175  double den = 0.0;
176  for (size_t i = 0; i < nMatches; i++)
177  {
178  num += square(points_other[i].x) + square(points_other[i].y) +
179  square(points_other[i].z);
180  den += square(points_this[i].x) + square(points_this[i].y) +
181  square(points_this[i].z);
182  } // end-for
183 
184  // The scale:
185  s = std::sqrt(num / den);
186  }
187 
188  TPoint3D pp;
189  out_transform.composePoint(
190  ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z);
191  pp *= s;
192 
193  out_transform[0] = ct_this.x - pp.x; // X
194  out_transform[1] = ct_this.y - pp.y; // Y
195  out_transform[2] = ct_this.z - pp.z; // Z
196 
197  out_scale = s; // return scale
198  return true;
199 
200  MRPT_END
201 } // end se3_l2_internal()
202 
204  const std::vector<mrpt::math::TPoint3D>& in_points_this,
205  const std::vector<mrpt::math::TPoint3D>& in_points_other,
206  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
207  bool forceScaleToUnity)
208 {
209  // make a copy because we need it anyway to substract the centroid and to
210  // provide a unified interface to TMatchingList API
211  std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
212  std::vector<mrpt::math::TPoint3D> points_other = in_points_other;
213 
214  return se3_l2_internal(
215  points_this, points_other, out_transform, out_scale, forceScaleToUnity);
216 }
217 
219  const mrpt::tfest::TMatchingPairList& corrs,
220  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
221  bool forceScaleToUnity)
222 {
223  // Transform data types:
224  const size_t N = corrs.size();
225  std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
226  for (size_t i = 0; i < N; i++)
227  {
228  points_this[i].x = corrs[i].this_x;
229  points_this[i].y = corrs[i].this_y;
230  points_this[i].z = corrs[i].this_z;
231  points_other[i].x = corrs[i].other_x;
232  points_other[i].y = corrs[i].other_y;
233  points_other[i].z = corrs[i].other_z;
234  }
235  return se3_l2_internal(
236  points_this, points_other, out_transform, out_scale, forceScaleToUnity);
237 }
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
auto col(int colIdx)
Definition: MatrixBase.h:89
STL namespace.
GLdouble s
Definition: glext.h:3682
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
GLuint GLuint num
Definition: glext.h:7397
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
T square(const T x)
Inline function for the square of a number.
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:68
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
A list of TMatchingPair.
Definition: TMatchingPair.h:70
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
const GLdouble * v
Definition: glext.h:3684
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:190
#define MRPT_END
Definition: exceptions.h:245
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:218
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
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: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019