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",
87 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
88 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
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]);
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 ());
138 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
139 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
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)
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 ());
182 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
183 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
191 x.setConstant (n_unknowns, 0);
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]);
213 transformation_matrix =
warp_point_->getTransform ();
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;
240 for (
size_t i = 0; i < nr_correspondences; ++i)
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);
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.