39 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_
40 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_
42 #include <pcl/registration/warp_point_rigid.h>
43 #include <pcl/registration/warp_point_rigid_6d.h>
44 #include <pcl/registration/distances.h>
45 #include <unsupported/Eigen/NonLinearOptimization>
49 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
56 , correspondence_weights_ ()
57 , use_correspondence_weights_ (true)
62 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void
66 Matrix4 &transformation_matrix)
const
70 if (cloud_src.
points.size () != cloud_tgt.
points.size ())
72 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
73 PCL_ERROR (
"Number or points in source (%lu) differs than target (%lu)!\n",
77 if (cloud_src.
points.size () < 4)
79 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
80 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!\n",
85 if (correspondence_weights_.size () != cloud_src.
size ())
87 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
88 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
89 correspondence_weights_.size (), cloud_src.
points.size ());
93 int n_unknowns = warp_point_->getDimension ();
98 tmp_src_ = &cloud_src;
99 tmp_tgt_ = &cloud_tgt;
102 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
103 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
104 int info = lm.minimize (x);
107 PCL_DEBUG (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]");
108 PCL_DEBUG (
"LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
109 PCL_DEBUG (
"Final solution: [%f", x[0]);
110 for (
int i = 1; i < n_unknowns; ++i)
111 PCL_DEBUG (
" %f", x[i]);
115 warp_point_->setParam (x);
116 transformation_matrix = warp_point_->getTransform ();
123 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void
126 const std::vector<int> &indices_src,
128 Matrix4 &transformation_matrix)
const
130 if (indices_src.size () != cloud_tgt.
points.size ())
132 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
136 if (correspondence_weights_.size () != indices_src.size ())
138 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
139 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
140 correspondence_weights_.size (), indices_src.size ());
146 transformation_matrix.setIdentity ();
148 const int nr_correspondences =
static_cast<const int> (cloud_tgt.
points.size ());
149 std::vector<int> indices_tgt;
150 indices_tgt.resize(nr_correspondences);
151 for (
int i = 0; i < nr_correspondences; ++i)
154 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
158 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
161 const std::vector<int> &indices_src,
163 const std::vector<int> &indices_tgt,
164 Matrix4 &transformation_matrix)
const
166 if (indices_src.size () != indices_tgt.size ())
168 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
172 if (indices_src.size () < 4)
174 PCL_ERROR (
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
175 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!",
176 indices_src.size ());
180 if (correspondence_weights_.size () != indices_src.size ())
182 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
183 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
184 correspondence_weights_.size (), indices_src.size ());
189 int n_unknowns = warp_point_->getDimension ();
191 x.setConstant (n_unknowns, 0);
194 tmp_src_ = &cloud_src;
195 tmp_tgt_ = &cloud_tgt;
196 tmp_idx_src_ = &indices_src;
197 tmp_idx_tgt_ = &indices_tgt;
200 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
201 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
202 int info = lm.minimize (x);
205 PCL_DEBUG (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
206 PCL_DEBUG (
"Final solution: [%f", x[0]);
207 for (
int i = 1; i < n_unknowns; ++i)
208 PCL_DEBUG (
" %f", x[i]);
212 warp_point_->setParam (x);
213 transformation_matrix = warp_point_->getTransform ();
217 tmp_idx_src_ = tmp_idx_tgt_ = NULL;
221 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
226 Matrix4 &transformation_matrix)
const
228 const int nr_correspondences =
static_cast<const int> (correspondences.size ());
229 std::vector<int> indices_src (nr_correspondences);
230 std::vector<int> indices_tgt (nr_correspondences);
231 for (
int i = 0; i < nr_correspondences; ++i)
233 indices_src[i] = correspondences[i].index_query;
234 indices_tgt[i] = correspondences[i].index_match;
237 if (use_correspondence_weights_)
239 correspondence_weights_.resize (nr_correspondences);
240 for (
size_t i = 0; i < nr_correspondences; ++i)
241 correspondence_weights_[i] = correspondences[i].weight;
244 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
248 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
256 estimator_->warp_point_->setParam (x);
259 for (
int i = 0; i < values (); ++i)
261 const PointSource & p_src = src_points.
points[i];
262 const PointTarget & p_tgt = tgt_points.
points[i];
266 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
269 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
275 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
281 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
282 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
285 estimator_->warp_point_->setParam (x);
288 for (
int i = 0; i < values (); ++i)
290 const PointSource & p_src = src_points.
points[src_indices[i]];
291 const PointTarget & p_tgt = tgt_points.
points[tgt_indices[i]];
295 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
298 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);