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 , use_correspondence_weights_ (true)
61 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void
65 Matrix4 &transformation_matrix)
const
69 if (cloud_src.
points.size () != cloud_tgt.
points.size ())
71 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
72 PCL_ERROR (
"Number or points in source (%lu) differs than target (%lu)!\n",
76 if (cloud_src.
points.size () < 4)
78 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
79 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!\n",
84 if (correspondence_weights_.size () != cloud_src.
size ())
86 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
87 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
88 correspondence_weights_.size (), cloud_src.
points.size ());
92 int n_unknowns = warp_point_->getDimension ();
97 tmp_src_ = &cloud_src;
98 tmp_tgt_ = &cloud_tgt;
101 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
102 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
103 int info = lm.minimize (x);
106 PCL_DEBUG (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]");
107 PCL_DEBUG (
"LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
108 PCL_DEBUG (
"Final solution: [%f", x[0]);
109 for (
int i = 1; i < n_unknowns; ++i)
110 PCL_DEBUG (
" %f", x[i]);
114 warp_point_->setParam (x);
115 transformation_matrix = warp_point_->getTransform ();
122 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void
125 const std::vector<int> &indices_src,
127 Matrix4 &transformation_matrix)
const
129 if (indices_src.size () != cloud_tgt.
points.size ())
131 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
135 if (correspondence_weights_.size () != indices_src.size ())
137 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
138 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
139 correspondence_weights_.size (), indices_src.size ());
145 transformation_matrix.setIdentity ();
147 const int nr_correspondences =
static_cast<const int> (cloud_tgt.
points.size ());
148 std::vector<int> indices_tgt;
149 indices_tgt.resize(nr_correspondences);
150 for (
int i = 0; i < nr_correspondences; ++i)
153 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
157 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
160 const std::vector<int> &indices_src,
162 const std::vector<int> &indices_tgt,
163 Matrix4 &transformation_matrix)
const
165 if (indices_src.size () != indices_tgt.size ())
167 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
171 if (indices_src.size () < 4)
173 PCL_ERROR (
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
174 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!",
175 indices_src.size ());
179 if (correspondence_weights_.size () != indices_src.size ())
181 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
182 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
183 correspondence_weights_.size (), indices_src.size ());
188 int n_unknowns = warp_point_->getDimension ();
190 x.setConstant (n_unknowns, 0);
193 tmp_src_ = &cloud_src;
194 tmp_tgt_ = &cloud_tgt;
195 tmp_idx_src_ = &indices_src;
196 tmp_idx_tgt_ = &indices_tgt;
199 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
200 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
201 int info = lm.minimize (x);
204 PCL_DEBUG (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
205 PCL_DEBUG (
"Final solution: [%f", x[0]);
206 for (
int i = 1; i < n_unknowns; ++i)
207 PCL_DEBUG (
" %f", x[i]);
211 warp_point_->setParam (x);
212 transformation_matrix = warp_point_->getTransform ();
216 tmp_idx_src_ = tmp_idx_tgt_ = NULL;
220 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
225 Matrix4 &transformation_matrix)
const
227 const int nr_correspondences =
static_cast<int> (correspondences.size ());
228 std::vector<int> indices_src (nr_correspondences);
229 std::vector<int> indices_tgt (nr_correspondences);
230 for (
int i = 0; i < nr_correspondences; ++i)
232 indices_src[i] = correspondences[i].index_query;
233 indices_tgt[i] = correspondences[i].index_match;
236 if (use_correspondence_weights_)
238 correspondence_weights_.resize (nr_correspondences);
239 for (std::size_t i = 0; i < nr_correspondences; ++i)
240 correspondence_weights_[i] = correspondences[i].weight;
243 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
247 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
255 estimator_->warp_point_->setParam (x);
258 for (
int i = 0; i < values (); ++i)
260 const PointSource & p_src = src_points.
points[i];
261 const PointTarget & p_tgt = tgt_points.
points[i];
265 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
268 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
274 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
280 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
281 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
284 estimator_->warp_point_->setParam (x);
287 for (
int i = 0; i < values (); ++i)
289 const PointSource & p_src = src_points.
points[src_indices[i]];
290 const PointTarget & p_tgt = tgt_points.
points[tgt_indices[i]];
294 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
297 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences