168 Matrix4& transformation_matrix)
const
171 transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero();
172 transformation_matrix(0, 0) =
static_cast<Scalar
>(std::cos(gamma) * std::cos(beta));
173 transformation_matrix(0, 1) =
static_cast<Scalar
>(
174 -sin(gamma) * std::cos(alpha) + std::cos(gamma) * sin(beta) * sin(alpha));
175 transformation_matrix(0, 2) =
static_cast<Scalar
>(
176 sin(gamma) * sin(alpha) + std::cos(gamma) * sin(beta) * std::cos(alpha));
177 transformation_matrix(1, 0) =
static_cast<Scalar
>(sin(gamma) * std::cos(beta));
178 transformation_matrix(1, 1) =
static_cast<Scalar
>(
179 std::cos(gamma) * std::cos(alpha) + sin(gamma) * sin(beta) * sin(alpha));
180 transformation_matrix(1, 2) =
static_cast<Scalar
>(
181 -std::cos(gamma) * sin(alpha) + sin(gamma) * sin(beta) * std::cos(alpha));
182 transformation_matrix(2, 0) =
static_cast<Scalar
>(-sin(beta));
183 transformation_matrix(2, 1) =
static_cast<Scalar
>(std::cos(beta) * sin(alpha));
184 transformation_matrix(2, 2) =
static_cast<Scalar
>(std::cos(beta) * std::cos(alpha));
186 transformation_matrix(0, 3) =
static_cast<Scalar
>(tx);
187 transformation_matrix(1, 3) =
static_cast<Scalar
>(ty);
188 transformation_matrix(2, 3) =
static_cast<Scalar
>(tz);
189 transformation_matrix(3, 3) =
static_cast<Scalar
>(1);
198 typename std::vector<Scalar>::const_iterator& weights_it,
199 Matrix4& transformation_matrix)
const
201 using Vector6d = Eigen::Matrix<double, 6, 1>;
202 using Matrix6d = Eigen::Matrix<double, 6, 6>;
210 if (!std::isfinite(source_it->x) || !std::isfinite(source_it->y) ||
211 !std::isfinite(source_it->z) || !std::isfinite(target_it->x) ||
212 !std::isfinite(target_it->y) || !std::isfinite(target_it->z) ||
213 !std::isfinite(target_it->normal_x) || !std::isfinite(target_it->normal_y) ||
214 !std::isfinite(target_it->normal_z)) {
221 const float& sx = source_it->x;
222 const float& sy = source_it->y;
223 const float& sz = source_it->z;
224 const float& dx = target_it->x;
225 const float& dy = target_it->y;
226 const float& dz = target_it->z;
227 const float& nx = target_it->normal[0] * (*weights_it);
228 const float& ny = target_it->normal[1] * (*weights_it);
229 const float& nz = target_it->normal[2] * (*weights_it);
231 double a = nz * sy - ny * sz;
232 double b = nx * sz - nz * sx;
233 double c = ny * sx - nx * sy;
242 ATA.coeffRef(0) += a * a;
243 ATA.coeffRef(1) += a * b;
244 ATA.coeffRef(2) += a * c;
245 ATA.coeffRef(3) += a * nx;
246 ATA.coeffRef(4) += a * ny;
247 ATA.coeffRef(5) += a * nz;
248 ATA.coeffRef(7) += b * b;
249 ATA.coeffRef(8) += b * c;
250 ATA.coeffRef(9) += b * nx;
251 ATA.coeffRef(10) += b * ny;
252 ATA.coeffRef(11) += b * nz;
253 ATA.coeffRef(14) += c * c;
254 ATA.coeffRef(15) += c * nx;
255 ATA.coeffRef(16) += c * ny;
256 ATA.coeffRef(17) += c * nz;
257 ATA.coeffRef(21) += nx * nx;
258 ATA.coeffRef(22) += nx * ny;
259 ATA.coeffRef(23) += nx * nz;
260 ATA.coeffRef(28) += ny * ny;
261 ATA.coeffRef(29) += ny * nz;
262 ATA.coeffRef(35) += nz * nz;
264 double d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
265 ATb.coeffRef(0) += a * d;
266 ATb.coeffRef(1) += b * d;
267 ATb.coeffRef(2) += c * d;
268 ATb.coeffRef(3) += nx * d;
269 ATb.coeffRef(4) += ny * d;
270 ATb.coeffRef(5) += nz * d;
277 ATA.coeffRef(6) = ATA.coeff(1);
278 ATA.coeffRef(12) = ATA.coeff(2);
279 ATA.coeffRef(13) = ATA.coeff(8);
280 ATA.coeffRef(18) = ATA.coeff(3);
281 ATA.coeffRef(19) = ATA.coeff(9);
282 ATA.coeffRef(20) = ATA.coeff(15);
283 ATA.coeffRef(24) = ATA.coeff(4);
284 ATA.coeffRef(25) = ATA.coeff(10);
285 ATA.coeffRef(26) = ATA.coeff(16);
286 ATA.coeffRef(27) = ATA.coeff(22);
287 ATA.coeffRef(30) = ATA.coeff(5);
288 ATA.coeffRef(31) = ATA.coeff(11);
289 ATA.coeffRef(32) = ATA.coeff(17);
290 ATA.coeffRef(33) = ATA.coeff(23);
291 ATA.coeffRef(34) = ATA.coeff(29);
294 Vector6d x =
static_cast<Vector6d
>(ATA.inverse() * ATb);
297 constructTransformationMatrix(
298 x(0), x(1), x(2), x(3), x(4), x(5), transformation_matrix);