40#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
41#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
43#include <pcl/cloud_iterator.h>
47namespace registration {
49template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
58 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
59 "estimateRigidTransformation] Number or points in source (%zu) differs "
60 "than target (%zu)!\n",
61 static_cast<std::size_t
>(nr_points),
66 if (weights_.size() != nr_points) {
67 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
68 "estimateRigidTransformation] Number or weights from the number of "
69 "correspondences! Use setWeights () to set them.\n");
75 typename std::vector<Scalar>::const_iterator
weights_it = weights_.begin();
79template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
89 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
90 "estimateRigidTransformation] Number or points in source (%zu) differs "
91 "than target (%zu)!\n",
97 if (weights_.size() != nr_points) {
98 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
99 "estimateRigidTransformation] Number or weights from the number of "
100 "correspondences! Use setWeights () to set them.\n");
106 typename std::vector<Scalar>::const_iterator
weights_it = weights_.begin();
110template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
121 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
122 "estimateRigidTransformation] Number or points in source (%lu) differs "
123 "than target (%lu)!\n",
129 if (weights_.size() != nr_points) {
130 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
131 "estimateRigidTransformation] Number or weights from the number of "
132 "correspondences! Use setWeights () to set them.\n");
138 typename std::vector<Scalar>::const_iterator
weights_it = weights_.begin();
142template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
152 std::vector<Scalar> weights(correspondences.size());
153 for (std::size_t i = 0; i < correspondences.size(); ++i)
154 weights[i] = correspondences[i].weight;
155 typename std::vector<Scalar>::const_iterator
weights_it = weights.begin();
159template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
192template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
198 typename std::vector<Scalar>::const_iterator&
weights_it,
201 using Vector6d = Eigen::Matrix<double, 6, 1>;
202 using Matrix6d = Eigen::Matrix<double, 6, 6>;
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);
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;
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(
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.