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>
49template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
53 MatScalar>::TransformationEstimationPointToPlaneWeighted()
59, use_correspondence_weights_(
true){};
62template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
73 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
74 "estimateRigidTransformation] ");
75 PCL_ERROR(
"Number or points in source (%zu) differs than target (%zu)!\n",
82 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
83 "estimateRigidTransformation] ");
84 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
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(),
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]);
121 PCL_DEBUG(
" %f", x[i]);
125 warp_point_->setParam(x);
133template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
143 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
144 "estimateRigidTransformation] Number or points in source (%zu) differs "
145 "than target (%zu)!\n",
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(),
169 estimateRigidTransformation(
174template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
185 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::"
186 "estimateRigidTransformation] Number or points in source (%lu) differs "
187 "than target (%lu)!\n",
195 PCL_ERROR(
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
196 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
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(),
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]);
236 PCL_DEBUG(
" %f", x[i]);
240 warp_point_->setParam(x);
245 tmp_idx_src_ = tmp_idx_tgt_ =
NULL;
249template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
266 if (use_correspondence_weights_) {
269 correspondence_weights_[i] = correspondences[i].weight;
272 estimateRigidTransformation(
277template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
288 estimator_->warp_point_->setParam(x);
292 for (
int i = 0; i < values(); ++i) {
301 fvec[i] = estimator_->correspondence_weights_[i] *
308template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
322 estimator_->warp_point_->setParam(x);
326 for (
int i = 0; i < values(); ++i) {
335 fvec[i] = estimator_->correspondence_weights_[i] *
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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.