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>
49 namespace registration
52 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
56 Matrix4 &transformation_matrix)
const
58 const auto nr_points = cloud_src.
size ();
59 if (cloud_tgt.
size () != nr_points)
61 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
62 "estimateRigidTransformation] Number or points in source (%zu) differs "
63 "than target (%zu)!\n",
64 static_cast<std::size_t
>(nr_points),
65 static_cast<std::size_t
>(cloud_tgt.
size()));
69 if (weights_.size () != nr_points)
71 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
77 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
78 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
82 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
85 const std::vector<int> &indices_src,
87 Matrix4 &transformation_matrix)
const
89 const std::size_t nr_points = indices_src.size ();
90 if (cloud_tgt.
size () != nr_points)
92 PCL_ERROR(
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::"
93 "estimateRigidTransformation] Number or points in source (%zu) differs "
94 "than target (%zu)!\n",
96 static_cast<std::size_t
>(cloud_tgt.
size()));
100 if (weights_.size () != nr_points)
102 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
109 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
110 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
114 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
117 const std::vector<int> &indices_src,
119 const std::vector<int> &indices_tgt,
120 Matrix4 &transformation_matrix)
const
122 const std::size_t nr_points = indices_src.size ();
123 if (indices_tgt.size () != nr_points)
125 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
129 if (weights_.size () != nr_points)
131 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
137 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
138 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
142 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
147 Matrix4 &transformation_matrix)
const
151 std::vector<Scalar> weights (correspondences.size ());
152 for (std::size_t i = 0; i < correspondences.size (); ++i)
153 weights[i] = correspondences[i].weight;
154 typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
155 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
159 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
162 const double & tx,
const double & ty,
const double & tz,
163 Matrix4 &transformation_matrix)
const
166 transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
167 transformation_matrix (0, 0) =
static_cast<Scalar
> ( std::cos (gamma) * std::cos (beta));
168 transformation_matrix (0, 1) =
static_cast<Scalar
> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
169 transformation_matrix (0, 2) =
static_cast<Scalar
> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
170 transformation_matrix (1, 0) =
static_cast<Scalar
> ( sin (gamma) * std::cos (beta));
171 transformation_matrix (1, 1) =
static_cast<Scalar
> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
172 transformation_matrix (1, 2) =
static_cast<Scalar
> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
173 transformation_matrix (2, 0) =
static_cast<Scalar
> (-sin (beta));
174 transformation_matrix (2, 1) =
static_cast<Scalar
> ( std::cos (beta) * sin (alpha));
175 transformation_matrix (2, 2) =
static_cast<Scalar
> ( std::cos (beta) * std::cos (alpha));
177 transformation_matrix (0, 3) =
static_cast<Scalar
> (tx);
178 transformation_matrix (1, 3) =
static_cast<Scalar
> (ty);
179 transformation_matrix (2, 3) =
static_cast<Scalar
> (tz);
180 transformation_matrix (3, 3) =
static_cast<Scalar
> (1);
184 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
188 typename std::vector<Scalar>::const_iterator& weights_it,
189 Matrix4 &transformation_matrix)
const
191 using Vector6d = Eigen::Matrix<double, 6, 1>;
192 using Matrix6d = Eigen::Matrix<double, 6, 6>;
201 if (!std::isfinite (source_it->x) ||
202 !std::isfinite (source_it->y) ||
203 !std::isfinite (source_it->z) ||
204 !std::isfinite (target_it->x) ||
205 !std::isfinite (target_it->y) ||
206 !std::isfinite (target_it->z) ||
207 !std::isfinite (target_it->normal_x) ||
208 !std::isfinite (target_it->normal_y) ||
209 !std::isfinite (target_it->normal_z))
217 const float & sx = source_it->x;
218 const float & sy = source_it->y;
219 const float & sz = source_it->z;
220 const float & dx = target_it->x;
221 const float & dy = target_it->y;
222 const float & dz = target_it->z;
223 const float & nx = target_it->normal[0] * (*weights_it);
224 const float & ny = target_it->normal[1] * (*weights_it);
225 const float & nz = target_it->normal[2] * (*weights_it);
227 double a = nz*sy - ny*sz;
228 double b = nx*sz - nz*sx;
229 double c = ny*sx - nx*sy;
238 ATA.coeffRef (0) += a * a;
239 ATA.coeffRef (1) += a * b;
240 ATA.coeffRef (2) += a * c;
241 ATA.coeffRef (3) += a * nx;
242 ATA.coeffRef (4) += a * ny;
243 ATA.coeffRef (5) += a * nz;
244 ATA.coeffRef (7) += b * b;
245 ATA.coeffRef (8) += b * c;
246 ATA.coeffRef (9) += b * nx;
247 ATA.coeffRef (10) += b * ny;
248 ATA.coeffRef (11) += b * nz;
249 ATA.coeffRef (14) += c * c;
250 ATA.coeffRef (15) += c * nx;
251 ATA.coeffRef (16) += c * ny;
252 ATA.coeffRef (17) += c * nz;
253 ATA.coeffRef (21) += nx * nx;
254 ATA.coeffRef (22) += nx * ny;
255 ATA.coeffRef (23) += nx * nz;
256 ATA.coeffRef (28) += ny * ny;
257 ATA.coeffRef (29) += ny * nz;
258 ATA.coeffRef (35) += nz * nz;
260 double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
261 ATb.coeffRef (0) += a * d;
262 ATb.coeffRef (1) += b * d;
263 ATb.coeffRef (2) += c * d;
264 ATb.coeffRef (3) += nx * d;
265 ATb.coeffRef (4) += ny * d;
266 ATb.coeffRef (5) += nz * d;
273 ATA.coeffRef (6) = ATA.coeff (1);
274 ATA.coeffRef (12) = ATA.coeff (2);
275 ATA.coeffRef (13) = ATA.coeff (8);
276 ATA.coeffRef (18) = ATA.coeff (3);
277 ATA.coeffRef (19) = ATA.coeff (9);
278 ATA.coeffRef (20) = ATA.coeff (15);
279 ATA.coeffRef (24) = ATA.coeff (4);
280 ATA.coeffRef (25) = ATA.coeff (10);
281 ATA.coeffRef (26) = ATA.coeff (16);
282 ATA.coeffRef (27) = ATA.coeff (22);
283 ATA.coeffRef (30) = ATA.coeff (5);
284 ATA.coeffRef (31) = ATA.coeff (11);
285 ATA.coeffRef (32) = ATA.coeff (17);
286 ATA.coeffRef (33) = ATA.coeff (23);
287 ATA.coeffRef (34) = ATA.coeff (29);
290 Vector6d x =
static_cast<Vector6d
> (ATA.inverse () * ATb);
293 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
Iterator class for point clouds with or without given indices.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences