41#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
42#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
44#include <pcl/cloud_iterator.h>
48namespace registration {
50template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
60 "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
61 "Number or points in source (%zu) differs than target (%zu)!\n",
62 static_cast<std::size_t
>(nr_points),
72template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
83 "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
84 "Number or points in source (%zu) differs than target (%zu)!\n",
95template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
107 "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
108 "Number or points in source (%zu) differs than target (%zu)!\n",
119template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
132template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
165template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
172 using Vector6d = Eigen::Matrix<double, 6, 1>;
173 using Matrix6d = Eigen::Matrix<double, 6, 6>;
213 ATA.coeffRef(0) += a * a;
214 ATA.coeffRef(1) += a * b;
215 ATA.coeffRef(2) += a *
c;
216 ATA.coeffRef(3) += a *
nx;
217 ATA.coeffRef(4) += a *
ny;
218 ATA.coeffRef(5) += a *
nz;
219 ATA.coeffRef(7) += b * b;
220 ATA.coeffRef(8) += b *
c;
221 ATA.coeffRef(9) += b *
nx;
222 ATA.coeffRef(10) += b *
ny;
223 ATA.coeffRef(11) += b *
nz;
224 ATA.coeffRef(14) +=
c *
c;
225 ATA.coeffRef(15) +=
c *
nx;
226 ATA.coeffRef(16) +=
c *
ny;
227 ATA.coeffRef(17) +=
c *
nz;
236 ATb.coeffRef(0) += a * d;
237 ATb.coeffRef(1) += b * d;
238 ATb.coeffRef(2) +=
c * d;
239 ATb.coeffRef(3) +=
nx * d;
240 ATb.coeffRef(4) +=
ny * d;
241 ATb.coeffRef(5) +=
nz * d;
247 ATA.coeffRef(6) =
ATA.coeff(1);
248 ATA.coeffRef(12) =
ATA.coeff(2);
249 ATA.coeffRef(13) =
ATA.coeff(8);
250 ATA.coeffRef(18) =
ATA.coeff(3);
251 ATA.coeffRef(19) =
ATA.coeff(9);
252 ATA.coeffRef(20) =
ATA.coeff(15);
253 ATA.coeffRef(24) =
ATA.coeff(4);
254 ATA.coeffRef(25) =
ATA.coeff(10);
255 ATA.coeffRef(26) =
ATA.coeff(16);
256 ATA.coeffRef(27) =
ATA.coeff(22);
257 ATA.coeffRef(30) =
ATA.coeff(5);
258 ATA.coeffRef(31) =
ATA.coeff(11);
259 ATA.coeffRef(32) =
ATA.coeff(17);
260 ATA.coeffRef(33) =
ATA.coeff(23);
261 ATA.coeffRef(34) =
ATA.coeff(29);
264 Vector6d x =
static_cast<Vector6d
>(
ATA.inverse() *
ATb);
267 constructTransformationMatrix(
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.