40 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
41 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
42 #include <pcl/cloud_iterator.h>
45 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
49 Matrix4 &transformation_matrix)
const
51 size_t nr_points = cloud_src.
points.size ();
52 if (cloud_tgt.
points.size () != nr_points)
54 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.
points.size ());
60 estimateRigidTransformation (source_it, target_it, transformation_matrix);
64 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
67 const std::vector<int> &indices_src,
69 Matrix4 &transformation_matrix)
const
71 size_t nr_points = indices_src.size ();
72 if (cloud_tgt.
points.size () != nr_points)
74 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.
points.size ());
80 estimateRigidTransformation (source_it, target_it, transformation_matrix);
85 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
88 const std::vector<int> &indices_src,
90 const std::vector<int> &indices_tgt,
91 Matrix4 &transformation_matrix)
const
93 size_t nr_points = indices_src.size ();
94 if (indices_tgt.size () != nr_points)
96 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
102 estimateRigidTransformation (source_it, target_it, transformation_matrix);
106 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
111 Matrix4 &transformation_matrix)
const
115 estimateRigidTransformation (source_it, target_it, transformation_matrix);
119 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
122 const double & tx,
const double & ty,
const double & tz,
123 Matrix4 &transformation_matrix)
const
126 transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
127 transformation_matrix (0, 0) =
static_cast<Scalar
> ( cos (gamma) * cos (beta));
128 transformation_matrix (0, 1) =
static_cast<Scalar
> (-sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha));
129 transformation_matrix (0, 2) =
static_cast<Scalar
> ( sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha));
130 transformation_matrix (1, 0) =
static_cast<Scalar
> ( sin (gamma) * cos (beta));
131 transformation_matrix (1, 1) =
static_cast<Scalar
> ( cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
132 transformation_matrix (1, 2) =
static_cast<Scalar
> (-cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha));
133 transformation_matrix (2, 0) =
static_cast<Scalar
> (-sin (beta));
134 transformation_matrix (2, 1) =
static_cast<Scalar
> ( cos (beta) * sin (alpha));
135 transformation_matrix (2, 2) =
static_cast<Scalar
> ( cos (beta) * cos (alpha));
137 transformation_matrix (0, 3) =
static_cast<Scalar
> (tx);
138 transformation_matrix (1, 3) =
static_cast<Scalar
> (ty);
139 transformation_matrix (2, 3) =
static_cast<Scalar
> (tz);
140 transformation_matrix (3, 3) =
static_cast<Scalar
> (1);
144 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
148 typedef Eigen::Matrix<double, 6, 1> Vector6d;
149 typedef Eigen::Matrix<double, 6, 6> Matrix6d;
159 if (!pcl_isfinite (source_it->x) ||
160 !pcl_isfinite (source_it->y) ||
161 !pcl_isfinite (source_it->z) ||
162 !pcl_isfinite (source_it->normal_x) ||
163 !pcl_isfinite (source_it->normal_y) ||
164 !pcl_isfinite (source_it->normal_z) ||
165 !pcl_isfinite (target_it->x) ||
166 !pcl_isfinite (target_it->y) ||
167 !pcl_isfinite (target_it->z) ||
168 !pcl_isfinite (target_it->normal_x) ||
169 !pcl_isfinite (target_it->normal_y) ||
170 !pcl_isfinite (target_it->normal_z))
177 const float & sx = source_it->x;
178 const float & sy = source_it->y;
179 const float & sz = source_it->z;
180 const float & dx = target_it->x;
181 const float & dy = target_it->y;
182 const float & dz = target_it->z;
183 const float & nx = target_it->normal[0];
184 const float & ny = target_it->normal[1];
185 const float & nz = target_it->normal[2];
187 double a = nz*sy - ny*sz;
188 double b = nx*sz - nz*sx;
189 double c = ny*sx - nx*sy;
198 ATA.coeffRef (0) += a * a;
199 ATA.coeffRef (1) += a * b;
200 ATA.coeffRef (2) += a * c;
201 ATA.coeffRef (3) += a * nx;
202 ATA.coeffRef (4) += a * ny;
203 ATA.coeffRef (5) += a * nz;
204 ATA.coeffRef (7) += b * b;
205 ATA.coeffRef (8) += b * c;
206 ATA.coeffRef (9) += b * nx;
207 ATA.coeffRef (10) += b * ny;
208 ATA.coeffRef (11) += b * nz;
209 ATA.coeffRef (14) += c * c;
210 ATA.coeffRef (15) += c * nx;
211 ATA.coeffRef (16) += c * ny;
212 ATA.coeffRef (17) += c * nz;
213 ATA.coeffRef (21) += nx * nx;
214 ATA.coeffRef (22) += nx * ny;
215 ATA.coeffRef (23) += nx * nz;
216 ATA.coeffRef (28) += ny * ny;
217 ATA.coeffRef (29) += ny * nz;
218 ATA.coeffRef (35) += nz * nz;
220 double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
221 ATb.coeffRef (0) += a * d;
222 ATb.coeffRef (1) += b * d;
223 ATb.coeffRef (2) += c * d;
224 ATb.coeffRef (3) += nx * d;
225 ATb.coeffRef (4) += ny * d;
226 ATb.coeffRef (5) += nz * d;
231 ATA.coeffRef (6) = ATA.coeff (1);
232 ATA.coeffRef (12) = ATA.coeff (2);
233 ATA.coeffRef (13) = ATA.coeff (8);
234 ATA.coeffRef (18) = ATA.coeff (3);
235 ATA.coeffRef (19) = ATA.coeff (9);
236 ATA.coeffRef (20) = ATA.coeff (15);
237 ATA.coeffRef (24) = ATA.coeff (4);
238 ATA.coeffRef (25) = ATA.coeff (10);
239 ATA.coeffRef (26) = ATA.coeff (16);
240 ATA.coeffRef (27) = ATA.coeff (22);
241 ATA.coeffRef (30) = ATA.coeff (5);
242 ATA.coeffRef (31) = ATA.coeff (11);
243 ATA.coeffRef (32) = ATA.coeff (17);
244 ATA.coeffRef (33) = ATA.coeff (23);
245 ATA.coeffRef (34) = ATA.coeff (29);
248 Vector6d x =
static_cast<Vector6d
> (ATA.inverse () * ATb);
251 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< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences