15 #include <Eigen/Dense>
70 std::vector<mrpt::math::TPoint3D>&
72 std::vector<mrpt::math::TPoint3D>&
75 bool forceScaleToUnity)
81 TPoint3D ct_others(0, 0, 0), ct_this(0, 0, 0);
82 const size_t nMatches = points_this.size();
87 for (
size_t i = 0; i < nMatches; i++)
89 ct_others += points_other[i];
90 ct_this += points_this[i];
93 const double F = 1.0 / nMatches;
100 for (
size_t i = 0; i < nMatches; i++)
102 points_this[i] -= ct_this;
103 points_other[i] -= ct_others;
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;
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;
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;
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);
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);
133 N(2, 2) = -S(0, 0) + S(1, 1) - S(2, 2);
134 N(2, 3) = S(1, 2) + S(2, 1);
139 N(3, 3) = -S(0, 0) - S(1, 1) + S(2, 2);
144 vector<double> eigvals;
151 sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]) - 1.0) <
164 for (
unsigned int i = 0; i < 4; i++)
165 out_transform[i + 3] = v[i];
169 if (forceScaleToUnity)
177 for (
size_t i = 0; i < nMatches; i++)
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) +
186 s = std::sqrt(num / den);
191 ct_others.x, ct_others.y, ct_others.z, pp.
x, pp.
y, pp.
z);
194 out_transform[0] = ct_this.
x - pp.
x;
195 out_transform[1] = ct_this.
y - pp.
y;
196 out_transform[2] = ct_this.
z - pp.
z;
205 const std::vector<mrpt::math::TPoint3D>& in_points_this,
206 const std::vector<mrpt::math::TPoint3D>& in_points_other,
208 bool forceScaleToUnity)
212 std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
213 std::vector<mrpt::math::TPoint3D> points_other = in_points_other;
216 points_this, points_other, out_transform, out_scale, forceScaleToUnity);
222 bool forceScaleToUnity)
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++)
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;
237 points_this, points_other, out_transform, out_scale, forceScaleToUnity);