40#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
41#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
43#include <pcl/registration/warp_point_rigid_6d.h>
45#include <unsupported/Eigen/NonLinearOptimization>
48template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
58template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
68 PCL_ERROR(
"[pcl::registration::TransformationEstimationLM::"
69 "estimateRigidTransformation] ");
70 PCL_ERROR(
"Number or points in source (%zu) differs than target (%zu)!\n",
77 PCL_ERROR(
"[pcl::registration::TransformationEstimationLM::"
78 "estimateRigidTransformation] ");
79 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
93 OptimizationFunctor functor(
static_cast<int>(
cloud_src.
size()),
this);
94 Eigen::NumericalDiff<OptimizationFunctor>
num_diff(functor);
97 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
MatScalar>
lm(
103 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
104 PCL_DEBUG(
"LM solver finished with exit code %i, having a residual norm of %g. \n",
107 PCL_DEBUG(
"Final solution: [%f", x[0]);
109 PCL_DEBUG(
" %f", x[i]);
113 warp_point_->setParam(x);
121template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
131 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
132 "Number or points in source (%zu) differs than target (%zu)!\n",
147 estimateRigidTransformation(
152template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
163 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
164 "Number or points in source (%lu) differs than target (%lu)!\n",
172 PCL_ERROR(
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
173 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
189 OptimizationFunctorWithIndices functor(
static_cast<int>(
indices_src.
size()),
this);
190 Eigen::NumericalDiff<OptimizationFunctorWithIndices>
num_diff(functor);
193 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>,
196 int info =
lm.minimize(x);
200 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM "
201 "solver finished with exit code %i, having a residual norm of %g. \n",
204 PCL_DEBUG(
"Final solution: [%f", x[0]);
206 PCL_DEBUG(
" %f", x[i]);
210 warp_point_->setParam(x);
215 tmp_idx_src_ = tmp_idx_tgt_ =
nullptr;
219template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
235 estimateRigidTransformation(
240template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
249 estimator_->warp_point_->setParam(x);
253 for (
int i = 0; i < values(); ++i) {
268template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
279 estimator_->warp_point_->setParam(x);
283 for (
int i = 0; i < values(); ++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.