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>
48 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
58 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
63 Matrix4& transformation_matrix)
const
67 if (cloud_src.
size() != cloud_tgt.
size()) {
68 PCL_ERROR(
"[pcl::registration::TransformationEstimationLM::"
69 "estimateRigidTransformation] ");
70 PCL_ERROR(
"Number or points in source (%zu) differs than target (%zu)!\n",
71 static_cast<std::size_t
>(cloud_src.
size()),
72 static_cast<std::size_t
>(cloud_tgt.
size()));
75 if (cloud_src.
size() < 4)
77 PCL_ERROR(
"[pcl::registration::TransformationEstimationLM::"
78 "estimateRigidTransformation] ");
79 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
81 static_cast<std::size_t
>(cloud_src.
size()));
85 int n_unknowns = warp_point_->getDimension();
86 VectorX x(n_unknowns);
90 tmp_src_ = &cloud_src;
91 tmp_tgt_ = &cloud_tgt;
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(
99 int info = lm.minimize(x);
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]);
108 for (
int i = 1; i < n_unknowns; ++i)
109 PCL_DEBUG(
" %f", x[i]);
113 warp_point_->setParam(x);
114 transformation_matrix = warp_point_->getTransform();
121 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
127 Matrix4& transformation_matrix)
const
129 if (indices_src.size() != cloud_tgt.
size()) {
131 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
132 "Number or points in source (%zu) differs than target (%zu)!\n",
134 static_cast<std::size_t
>(cloud_tgt.
size()));
139 transformation_matrix.setIdentity();
141 const auto nr_correspondences = cloud_tgt.
size();
143 indices_tgt.resize(nr_correspondences);
144 for (std::size_t i = 0; i < nr_correspondences; ++i)
147 estimateRigidTransformation(
148 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
152 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
159 Matrix4& transformation_matrix)
const
161 if (indices_src.size() != indices_tgt.size()) {
163 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
164 "Number or points in source (%lu) differs than target (%lu)!\n",
170 if (indices_src.size() < 4)
172 PCL_ERROR(
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
173 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
179 int n_unknowns = warp_point_->getDimension();
181 x.setConstant(n_unknowns, 0);
184 tmp_src_ = &cloud_src;
185 tmp_tgt_ = &cloud_tgt;
186 tmp_idx_src_ = &indices_src;
187 tmp_idx_tgt_ = &indices_tgt;
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]);
205 for (
int i = 1; i < n_unknowns; ++i)
206 PCL_DEBUG(
" %f", x[i]);
210 warp_point_->setParam(x);
211 transformation_matrix = warp_point_->getTransform();
215 tmp_idx_src_ = tmp_idx_tgt_ =
nullptr;
219 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
225 Matrix4& transformation_matrix)
const
227 const auto nr_correspondences = correspondences.size();
230 for (std::size_t i = 0; i < nr_correspondences; ++i) {
231 indices_src[i] = correspondences[i].index_query;
232 indices_tgt[i] = correspondences[i].index_match;
235 estimateRigidTransformation(
236 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
240 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
249 estimator_->warp_point_->setParam(x);
253 for (
int i = 0; i < values(); ++i) {
254 const PointSource& p_src = src_points[i];
255 const PointTarget& p_tgt = tgt_points[i];
259 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
262 fvec[i] = estimator_->computeDistance(p_src_warped, p_tgt);
268 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
275 const pcl::Indices& src_indices = *estimator_->tmp_idx_src_;
276 const pcl::Indices& tgt_indices = *estimator_->tmp_idx_tgt_;
279 estimator_->warp_point_->setParam(x);
283 for (
int i = 0; i < values(); ++i) {
284 const PointSource& p_src = src_points[src_indices[i]];
285 const PointTarget& p_tgt = tgt_points[tgt_indices[i]];
289 estimator_->warp_point_->warpPoint(p_src, p_src_warped);
292 fvec[i] = 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.