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>
50 namespace registration
53 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
57 Matrix4 &transformation_matrix)
const
59 const auto nr_points = cloud_src.
size ();
60 if (cloud_tgt.
size () != nr_points)
63 "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
64 "Number or points in source (%zu) differs than target (%zu)!\n",
65 static_cast<std::size_t
>(nr_points),
66 static_cast<std::size_t
>(cloud_tgt.
size()));
72 estimateRigidTransformation (source_it, target_it, transformation_matrix);
76 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
79 const std::vector<int> &indices_src,
81 Matrix4 &transformation_matrix)
const
83 const auto nr_points = indices_src.size ();
84 if (cloud_tgt.
size () != nr_points)
87 "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
88 "Number or points in source (%zu) differs than target (%zu)!\n",
90 static_cast<std::size_t
>(cloud_tgt.
size()));
96 estimateRigidTransformation (source_it, target_it, transformation_matrix);
100 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
103 const std::vector<int> &indices_src,
105 const std::vector<int> &indices_tgt,
106 Matrix4 &transformation_matrix)
const
108 const auto nr_points = indices_src.size ();
109 if (indices_tgt.size () != nr_points)
112 "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
113 "Number or points in source (%zu) differs than target (%zu)!\n",
121 estimateRigidTransformation (source_it, target_it, transformation_matrix);
125 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
130 Matrix4 &transformation_matrix)
const
134 estimateRigidTransformation (source_it, target_it, transformation_matrix);
138 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
141 const double & tx,
const double & ty,
const double & tz,
142 Matrix4 &transformation_matrix)
const
145 transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
146 transformation_matrix (0, 0) =
static_cast<Scalar
> ( std::cos (gamma) * std::cos (beta));
147 transformation_matrix (0, 1) =
static_cast<Scalar
> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
148 transformation_matrix (0, 2) =
static_cast<Scalar
> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
149 transformation_matrix (1, 0) =
static_cast<Scalar
> ( sin (gamma) * std::cos (beta));
150 transformation_matrix (1, 1) =
static_cast<Scalar
> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
151 transformation_matrix (1, 2) =
static_cast<Scalar
> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
152 transformation_matrix (2, 0) =
static_cast<Scalar
> (-sin (beta));
153 transformation_matrix (2, 1) =
static_cast<Scalar
> ( std::cos (beta) * sin (alpha));
154 transformation_matrix (2, 2) =
static_cast<Scalar
> ( std::cos (beta) * std::cos (alpha));
156 transformation_matrix (0, 3) =
static_cast<Scalar
> (tx);
157 transformation_matrix (1, 3) =
static_cast<Scalar
> (ty);
158 transformation_matrix (2, 3) =
static_cast<Scalar
> (tz);
159 transformation_matrix (3, 3) =
static_cast<Scalar
> (1);
163 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
167 using Vector6d = Eigen::Matrix<double, 6, 1>;
168 using Matrix6d = Eigen::Matrix<double, 6, 6>;
178 if (!std::isfinite (source_it->x) ||
179 !std::isfinite (source_it->y) ||
180 !std::isfinite (source_it->z) ||
181 !std::isfinite (target_it->x) ||
182 !std::isfinite (target_it->y) ||
183 !std::isfinite (target_it->z) ||
184 !std::isfinite (target_it->normal_x) ||
185 !std::isfinite (target_it->normal_y) ||
186 !std::isfinite (target_it->normal_z))
193 const float & sx = source_it->x;
194 const float & sy = source_it->y;
195 const float & sz = source_it->z;
196 const float & dx = target_it->x;
197 const float & dy = target_it->y;
198 const float & dz = target_it->z;
199 const float & nx = target_it->normal[0];
200 const float & ny = target_it->normal[1];
201 const float & nz = target_it->normal[2];
203 double a = nz*sy - ny*sz;
204 double b = nx*sz - nz*sx;
205 double c = ny*sx - nx*sy;
214 ATA.coeffRef (0) += a * a;
215 ATA.coeffRef (1) += a * b;
216 ATA.coeffRef (2) += a * c;
217 ATA.coeffRef (3) += a * nx;
218 ATA.coeffRef (4) += a * ny;
219 ATA.coeffRef (5) += a * nz;
220 ATA.coeffRef (7) += b * b;
221 ATA.coeffRef (8) += b * c;
222 ATA.coeffRef (9) += b * nx;
223 ATA.coeffRef (10) += b * ny;
224 ATA.coeffRef (11) += b * nz;
225 ATA.coeffRef (14) += c * c;
226 ATA.coeffRef (15) += c * nx;
227 ATA.coeffRef (16) += c * ny;
228 ATA.coeffRef (17) += c * nz;
229 ATA.coeffRef (21) += nx * nx;
230 ATA.coeffRef (22) += nx * ny;
231 ATA.coeffRef (23) += nx * nz;
232 ATA.coeffRef (28) += ny * ny;
233 ATA.coeffRef (29) += ny * nz;
234 ATA.coeffRef (35) += nz * nz;
236 double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
237 ATb.coeffRef (0) += a * d;
238 ATb.coeffRef (1) += b * d;
239 ATb.coeffRef (2) += c * d;
240 ATb.coeffRef (3) += nx * d;
241 ATb.coeffRef (4) += ny * d;
242 ATb.coeffRef (5) += nz * d;
248 ATA.coeffRef (6) = ATA.coeff (1);
249 ATA.coeffRef (12) = ATA.coeff (2);
250 ATA.coeffRef (13) = ATA.coeff (8);
251 ATA.coeffRef (18) = ATA.coeff (3);
252 ATA.coeffRef (19) = ATA.coeff (9);
253 ATA.coeffRef (20) = ATA.coeff (15);
254 ATA.coeffRef (24) = ATA.coeff (4);
255 ATA.coeffRef (25) = ATA.coeff (10);
256 ATA.coeffRef (26) = ATA.coeff (16);
257 ATA.coeffRef (27) = ATA.coeff (22);
258 ATA.coeffRef (30) = ATA.coeff (5);
259 ATA.coeffRef (31) = ATA.coeff (11);
260 ATA.coeffRef (32) = ATA.coeff (17);
261 ATA.coeffRef (33) = ATA.coeff (23);
262 ATA.coeffRef (34) = ATA.coeff (29);
265 Vector6d x =
static_cast<Vector6d
> (ATA.inverse () * ATb);
268 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