39 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
40 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
41 #include <pcl/cloud_iterator.h>
44 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
48 Matrix4 &transformation_matrix)
const
50 size_t nr_points = cloud_src.
points.size ();
51 if (cloud_tgt.
points.size () != nr_points)
53 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.
points.size ());
57 if (weights_.size () != nr_points)
59 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
65 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
66 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
70 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
73 const std::vector<int> &indices_src,
75 Matrix4 &transformation_matrix)
const
77 size_t nr_points = indices_src.size ();
78 if (cloud_tgt.
points.size () != nr_points)
80 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
84 if (weights_.size () != nr_points)
86 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
93 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
94 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
99 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
102 const std::vector<int> &indices_src,
104 const std::vector<int> &indices_tgt,
105 Matrix4 &transformation_matrix)
const
107 size_t nr_points = indices_src.size ();
108 if (indices_tgt.size () != nr_points)
110 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
114 if (weights_.size () != nr_points)
116 PCL_ERROR (
"[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
122 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
123 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
127 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
132 Matrix4 &transformation_matrix)
const
136 std::vector<Scalar> weights (correspondences.size ());
137 for (
size_t i = 0; i < correspondences.size (); ++i)
138 weights[i] = correspondences[i].weight;
139 typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
140 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
144 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
147 const double & tx,
const double & ty,
const double & tz,
148 Matrix4 &transformation_matrix)
const
151 transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
152 transformation_matrix (0, 0) =
static_cast<Scalar
> ( cos (gamma) * cos (beta));
153 transformation_matrix (0, 1) =
static_cast<Scalar
> (-sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha));
154 transformation_matrix (0, 2) =
static_cast<Scalar
> ( sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha));
155 transformation_matrix (1, 0) =
static_cast<Scalar
> ( sin (gamma) * cos (beta));
156 transformation_matrix (1, 1) =
static_cast<Scalar
> ( cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
157 transformation_matrix (1, 2) =
static_cast<Scalar
> (-cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha));
158 transformation_matrix (2, 0) =
static_cast<Scalar
> (-sin (beta));
159 transformation_matrix (2, 1) =
static_cast<Scalar
> ( cos (beta) * sin (alpha));
160 transformation_matrix (2, 2) =
static_cast<Scalar
> ( cos (beta) * cos (alpha));
162 transformation_matrix (0, 3) =
static_cast<Scalar
> (tx);
163 transformation_matrix (1, 3) =
static_cast<Scalar
> (ty);
164 transformation_matrix (2, 3) =
static_cast<Scalar
> (tz);
165 transformation_matrix (3, 3) =
static_cast<Scalar
> (1);
169 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
173 typename std::vector<Scalar>::const_iterator& weights_it,
174 Matrix4 &transformation_matrix)
const
176 typedef Eigen::Matrix<double, 6, 1> Vector6d;
177 typedef Eigen::Matrix<double, 6, 6> Matrix6d;
186 if (!pcl_isfinite (source_it->x) ||
187 !pcl_isfinite (source_it->y) ||
188 !pcl_isfinite (source_it->z) ||
189 !pcl_isfinite (target_it->x) ||
190 !pcl_isfinite (target_it->y) ||
191 !pcl_isfinite (target_it->z) ||
192 !pcl_isfinite (target_it->normal_x) ||
193 !pcl_isfinite (target_it->normal_y) ||
194 !pcl_isfinite (target_it->normal_z))
202 const float & sx = source_it->x;
203 const float & sy = source_it->y;
204 const float & sz = source_it->z;
205 const float & dx = target_it->x;
206 const float & dy = target_it->y;
207 const float & dz = target_it->z;
208 const float & nx = target_it->normal[0] * (*weights_it);
209 const float & ny = target_it->normal[1] * (*weights_it);
210 const float & nz = target_it->normal[2] * (*weights_it);
212 double a = nz*sy - ny*sz;
213 double b = nx*sz - nz*sx;
214 double c = ny*sx - nx*sy;
223 ATA.coeffRef (0) += a * a;
224 ATA.coeffRef (1) += a * b;
225 ATA.coeffRef (2) += a * c;
226 ATA.coeffRef (3) += a * nx;
227 ATA.coeffRef (4) += a * ny;
228 ATA.coeffRef (5) += a * nz;
229 ATA.coeffRef (7) += b * b;
230 ATA.coeffRef (8) += b * c;
231 ATA.coeffRef (9) += b * nx;
232 ATA.coeffRef (10) += b * ny;
233 ATA.coeffRef (11) += b * nz;
234 ATA.coeffRef (14) += c * c;
235 ATA.coeffRef (15) += c * nx;
236 ATA.coeffRef (16) += c * ny;
237 ATA.coeffRef (17) += c * nz;
238 ATA.coeffRef (21) += nx * nx;
239 ATA.coeffRef (22) += nx * ny;
240 ATA.coeffRef (23) += nx * nz;
241 ATA.coeffRef (28) += ny * ny;
242 ATA.coeffRef (29) += ny * nz;
243 ATA.coeffRef (35) += nz * nz;
245 double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
246 ATb.coeffRef (0) += a * d;
247 ATb.coeffRef (1) += b * d;
248 ATb.coeffRef (2) += c * d;
249 ATb.coeffRef (3) += nx * d;
250 ATb.coeffRef (4) += ny * d;
251 ATb.coeffRef (5) += nz * d;
258 ATA.coeffRef (6) = ATA.coeff (1);
259 ATA.coeffRef (12) = ATA.coeff (2);
260 ATA.coeffRef (13) = ATA.coeff (8);
261 ATA.coeffRef (18) = ATA.coeff (3);
262 ATA.coeffRef (19) = ATA.coeff (9);
263 ATA.coeffRef (20) = ATA.coeff (15);
264 ATA.coeffRef (24) = ATA.coeff (4);
265 ATA.coeffRef (25) = ATA.coeff (10);
266 ATA.coeffRef (26) = ATA.coeff (16);
267 ATA.coeffRef (27) = ATA.coeff (22);
268 ATA.coeffRef (30) = ATA.coeff (5);
269 ATA.coeffRef (31) = ATA.coeff (11);
270 ATA.coeffRef (32) = ATA.coeff (17);
271 ATA.coeffRef (33) = ATA.coeff (23);
272 ATA.coeffRef (34) = ATA.coeff (29);
275 Vector6d x =
static_cast<Vector6d
> (ATA.inverse () * ATb);
278 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);