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/distances.h>
43 #include <pcl/registration/warp_point_rigid.h>
44 #include <pcl/registration/warp_point_rigid_6d.h>
46 #include <unsupported/Eigen/NonLinearOptimization>
49 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
59 , use_correspondence_weights_(true){};
62 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
68 Matrix4& transformation_matrix)
const
72 if (cloud_src.
size() != cloud_tgt.
size()) {
73 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
74 "estimateRigidTransformation] ");
75 PCL_ERROR(
"Number or points in source (%zu) differs than target (%zu)!\n",
76 static_cast<std::size_t
>(cloud_src.
size()),
77 static_cast<std::size_t
>(cloud_tgt.
size()));
80 if (cloud_src.
size() < 4)
82 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
83 "estimateRigidTransformation] ");
84 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
86 static_cast<std::size_t
>(cloud_src.
size()));
90 if (correspondence_weights_.size() != cloud_src.
size()) {
91 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
92 "estimateRigidTransformation] ");
93 PCL_ERROR(
"Number of weights (%zu) differs than number of points (%zu)!\n",
94 correspondence_weights_.size(),
95 static_cast<std::size_t
>(cloud_src.
size()));
99 int n_unknowns = warp_point_->getDimension();
100 VectorX x(n_unknowns);
104 tmp_src_ = &cloud_src;
105 tmp_tgt_ = &cloud_tgt;
107 OptimizationFunctor functor(
static_cast<int>(cloud_src.
size()),
this);
108 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
109 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm(
111 int info = lm.minimize(x);
114 PCL_DEBUG(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
115 "estimateRigidTransformation]");
116 PCL_DEBUG(
"LM solver finished with exit code %i, having a residual norm of %g. \n",
119 PCL_DEBUG(
"Final solution: [%f", x[0]);
120 for (
int i = 1; i < n_unknowns; ++i)
121 PCL_DEBUG(
" %f", x[i]);
125 warp_point_->setParam(x);
126 transformation_matrix = warp_point_->getTransform();
133 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
140 Matrix4& transformation_matrix)
const
142 if (indices_src.size() != cloud_tgt.
size()) {
143 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
144 "estimateRigidTransformation] Number or points in source (%zu) differs "
145 "than target (%zu)!\n",
147 static_cast<std::size_t
>(cloud_tgt.
size()));
151 if (correspondence_weights_.size() != indices_src.size()) {
152 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
153 "estimateRigidTransformation] ");
154 PCL_ERROR(
"Number of weights (%zu) differs than number of points (%zu)!\n",
155 correspondence_weights_.size(),
161 transformation_matrix.setIdentity();
163 const auto nr_correspondences = cloud_tgt.
size();
165 indices_tgt.resize(nr_correspondences);
166 for (std::size_t i = 0; i < nr_correspondences; ++i)
169 estimateRigidTransformation(
170 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
174 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
182 Matrix4& transformation_matrix)
const
184 if (indices_src.size() != indices_tgt.size()) {
185 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
186 "estimateRigidTransformation] Number or points in source (%lu) differs "
187 "than target (%lu)!\n",
193 if (indices_src.size() < 4)
195 PCL_ERROR(
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
196 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
202 if (correspondence_weights_.size() != indices_src.size()) {
203 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
204 "estimateRigidTransformation] ");
205 PCL_ERROR(
"Number of weights (%lu) differs than number of points (%lu)!\n",
206 correspondence_weights_.size(),
211 int n_unknowns = warp_point_->getDimension();
213 x.setConstant(n_unknowns, 0);
216 tmp_src_ = &cloud_src;
217 tmp_tgt_ = &cloud_tgt;
218 tmp_idx_src_ = &indices_src;
219 tmp_idx_tgt_ = &indices_tgt;
222 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff(functor);
223 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
226 int info = lm.minimize(x);
229 PCL_DEBUG(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
230 "estimateRigidTransformation] LM solver finished with exit code %i, having "
231 "a residual norm of %g. \n",
234 PCL_DEBUG(
"Final solution: [%f", x[0]);
235 for (
int i = 1; i < n_unknowns; ++i)
236 PCL_DEBUG(
" %f", x[i]);
240 warp_point_->setParam(x);
241 transformation_matrix = warp_point_->getTransform();
245 tmp_idx_src_ = tmp_idx_tgt_ =
nullptr;
249 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
256 Matrix4& transformation_matrix)
const
258 const int nr_correspondences =
static_cast<int>(correspondences.size());
261 for (
int i = 0; i < nr_correspondences; ++i) {
262 indices_src[i] = correspondences[i].index_query;
263 indices_tgt[i] = correspondences[i].index_match;
266 if (use_correspondence_weights_) {
267 correspondence_weights_.resize(nr_correspondences);
268 for (std::size_t i = 0; i < nr_correspondences; ++i)
269 correspondence_weights_[i] = correspondences[i].weight;
272 estimateRigidTransformation(
273 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
277 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
282 MatScalar>::OptimizationFunctor::operator()(
const VectorX& x,
VectorX& fvec)
const
288 estimator_->warp_point_->setParam(x);
292 for (
int i = 0; i < values(); ++i) {
293 const PointSource& p_src = src_points[i];
294 const PointTarget& p_tgt = tgt_points[i];
298 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
301 fvec[i] = estimator_->correspondence_weights_[i] *
302 estimator_->computeDistance(p_src_warped, p_tgt);
308 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
313 MatScalar>::OptimizationFunctorWithIndices::operator()(
const VectorX& x,
318 const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
319 const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
322 estimator_->warp_point_->setParam(x);
326 for (
int i = 0; i < values(); ++i) {
327 const PointSource& p_src = src_points[src_indices[i]];
328 const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
332 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
335 fvec[i] = estimator_->correspondence_weights_[i] *
336 estimator_->computeDistance(p_src_warped, p_tgt);
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.