MRPT  2.0.5
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-2020, 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(std::abs(pRi')) / sum{i}(
60 // square(std::abs(pLi')) ) )
61 //
62 // 8. Translation computation (distance between the Right centroid and the
63 // scaled and rotated Left centroid)
64 // t = ct_this-sR(ct_others)
65 
66 /*---------------------------------------------------------------
67  se3_l2 (old "HornMethod()")
68  ---------------------------------------------------------------*/
70  std::vector<mrpt::math::TPoint3D>&
71  points_this, // IN/OUT: It gets modified!
72  std::vector<mrpt::math::TPoint3D>&
73  points_other, // IN/OUT: It gets modified!
74  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
75  bool forceScaleToUnity)
76 {
78 
79  ASSERT_EQUAL_(points_this.size(), points_other.size());
80  // Compute the centroids
81  TPoint3D ct_others(0, 0, 0), ct_this(0, 0, 0);
82  const size_t nMatches = points_this.size();
83 
84  if (nMatches < 3)
85  return false; // Nothing we can estimate without 3 points!!
86 
87  for (size_t i = 0; i < nMatches; i++)
88  {
89  ct_others += points_other[i];
90  ct_this += points_this[i];
91  }
92 
93  const double F = 1.0 / nMatches;
94  ct_others *= F;
95  ct_this *= F;
96 
97  CMatrixDouble33 S; // Zeroed by default
98 
99  // Substract the centroid and compute the S matrix of cross products
100  for (size_t i = 0; i < nMatches; i++)
101  {
102  points_this[i] -= ct_this;
103  points_other[i] -= ct_others;
104 
105  S(0, 0) += points_other[i].x * points_this[i].x;
106  S(0, 1) += points_other[i].x * points_this[i].y;
107  S(0, 2) += points_other[i].x * points_this[i].z;
108 
109  S(1, 0) += points_other[i].y * points_this[i].x;
110  S(1, 1) += points_other[i].y * points_this[i].y;
111  S(1, 2) += points_other[i].y * points_this[i].z;
112 
113  S(2, 0) += points_other[i].z * points_this[i].x;
114  S(2, 1) += points_other[i].z * points_this[i].y;
115  S(2, 2) += points_other[i].z * points_this[i].z;
116  }
117 
118  // Construct the N matrix
119  CMatrixDouble44 N; // Zeroed by default
120 
121  N(0, 0) = S(0, 0) + S(1, 1) + S(2, 2);
122  N(0, 1) = S(1, 2) - S(2, 1);
123  N(0, 2) = S(2, 0) - S(0, 2);
124  N(0, 3) = S(0, 1) - S(1, 0);
125 
126  N(1, 0) = N(0, 1);
127  N(1, 1) = S(0, 0) - S(1, 1) - S(2, 2);
128  N(1, 2) = S(0, 1) + S(1, 0);
129  N(1, 3) = S(2, 0) + S(0, 2);
130 
131  N(2, 0) = N(0, 2);
132  N(2, 1) = N(1, 2);
133  N(2, 2) = -S(0, 0) + S(1, 1) - S(2, 2);
134  N(2, 3) = S(1, 2) + S(2, 1);
135 
136  N(3, 0) = N(0, 3);
137  N(3, 1) = N(1, 3);
138  N(3, 2) = N(2, 3);
139  N(3, 3) = -S(0, 0) - S(1, 1) + S(2, 2);
140 
141  // q is the quaternion correspondent to the greatest eigenvector of the N
142  // matrix (last column in Z)
143  CMatrixDouble44 Z;
144  vector<double> eigvals;
145  N.eig_symmetric(Z, eigvals, true /*sorted*/);
146 
147  auto v = CVectorFixedDouble<4>(Z.col(3).eval());
148 
149  ASSERTDEB_(
150  fabs(
151  sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]) - 1.0) <
152  0.1);
153 
154  // Make q_r > 0
155  if (v[0] < 0)
156  {
157  v[0] *= -1;
158  v[1] *= -1;
159  v[2] *= -1;
160  v[3] *= -1;
161  }
162 
163  // out_transform: Create a pose rotation with the quaternion
164  for (unsigned int i = 0; i < 4; i++) // insert the quaternion part
165  out_transform[i + 3] = v[i];
166 
167  // Compute scale
168  double s;
169  if (forceScaleToUnity)
170  {
171  s = 1.0; // Enforce scale to be 1
172  }
173  else
174  {
175  double num = 0.0;
176  double den = 0.0;
177  for (size_t i = 0; i < nMatches; i++)
178  {
179  num += square(points_other[i].x) + square(points_other[i].y) +
180  square(points_other[i].z);
181  den += square(points_this[i].x) + square(points_this[i].y) +
182  square(points_this[i].z);
183  } // end-for
184 
185  // The scale:
186  s = std::sqrt(num / den);
187  }
188 
189  TPoint3D pp;
190  out_transform.composePoint(
191  ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z);
192  pp *= s;
193 
194  out_transform[0] = ct_this.x - pp.x; // X
195  out_transform[1] = ct_this.y - pp.y; // Y
196  out_transform[2] = ct_this.z - pp.z; // Z
197 
198  out_scale = s; // return scale
199  return true;
200 
201  MRPT_END
202 } // end se3_l2_internal()
203 
205  const std::vector<mrpt::math::TPoint3D>& in_points_this,
206  const std::vector<mrpt::math::TPoint3D>& in_points_other,
207  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
208  bool forceScaleToUnity)
209 {
210  // make a copy because we need it anyway to substract the centroid and to
211  // provide a unified interface to TMatchingList API
212  std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
213  std::vector<mrpt::math::TPoint3D> points_other = in_points_other;
214 
215  return se3_l2_internal(
216  points_this, points_other, out_transform, out_scale, forceScaleToUnity);
217 }
218 
220  const mrpt::tfest::TMatchingPairList& corrs,
221  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
222  bool forceScaleToUnity)
223 {
224  // Transform data types:
225  const size_t N = corrs.size();
226  std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
227  for (size_t i = 0; i < N; i++)
228  {
229  points_this[i].x = corrs[i].this_x;
230  points_this[i].y = corrs[i].this_y;
231  points_this[i].z = corrs[i].this_z;
232  points_other[i].x = corrs[i].other_x;
233  points_other[i].y = corrs[i].other_y;
234  points_other[i].z = corrs[i].other_z;
235  }
236  return se3_l2_internal(
237  points_this, points_other, out_transform, out_scale, forceScaleToUnity);
238 }
#define MRPT_START
Definition: exceptions.h:241
auto col(int colIdx)
Definition: MatrixBase.h:89
STL namespace.
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
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 .
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:69
#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...
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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:219
Functions for estimating the optimal transformation between two frames of references given measuremen...



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020