40 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
41 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_
43 #include <pcl/registration/warp_point_rigid.h>
44 #include <pcl/registration/warp_point_rigid_6d.h>
45 #include <pcl/registration/distances.h>
46 #include <unsupported/Eigen/NonLinearOptimization>
50 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
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::TransformationEstimationLM::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::TransformationEstimationLM::estimateRigidTransformation] ");
79 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!\n",
84 int n_unknowns = warp_point_->getDimension ();
89 tmp_src_ = &cloud_src;
90 tmp_tgt_ = &cloud_tgt;
92 OptimizationFunctor functor (
static_cast<int> (cloud_src.
points.size ()),
this);
93 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
95 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
96 int info = lm.minimize (x);
99 PCL_DEBUG (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
100 PCL_DEBUG (
"LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
101 PCL_DEBUG (
"Final solution: [%f", x[0]);
102 for (
int i = 1; i < n_unknowns; ++i)
103 PCL_DEBUG (
" %f", x[i]);
107 warp_point_->setParam (x);
108 transformation_matrix = warp_point_->getTransform ();
115 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void
118 const std::vector<int> &indices_src,
120 Matrix4 &transformation_matrix)
const
122 if (indices_src.size () != cloud_tgt.
points.size ())
124 PCL_ERROR (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.
points.size ());
129 transformation_matrix.setIdentity ();
131 const int nr_correspondences =
static_cast<const int> (cloud_tgt.
points.size ());
132 std::vector<int> indices_tgt;
133 indices_tgt.resize(nr_correspondences);
134 for (
int i = 0; i < nr_correspondences; ++i)
137 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
141 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
144 const std::vector<int> &indices_src,
146 const std::vector<int> &indices_tgt,
147 Matrix4 &transformation_matrix)
const
149 if (indices_src.size () != indices_tgt.size ())
151 PCL_ERROR (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
155 if (indices_src.size () < 4)
157 PCL_ERROR (
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
158 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!",
159 indices_src.size ());
163 int n_unknowns = warp_point_->getDimension ();
165 x.setConstant (n_unknowns, 0);
168 tmp_src_ = &cloud_src;
169 tmp_tgt_ = &cloud_tgt;
170 tmp_idx_src_ = &indices_src;
171 tmp_idx_tgt_ = &indices_tgt;
173 OptimizationFunctorWithIndices functor (
static_cast<int> (indices_src.size ()),
this);
174 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
176 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
177 int info = lm.minimize (x);
180 PCL_DEBUG (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
181 PCL_DEBUG (
"Final solution: [%f", x[0]);
182 for (
int i = 1; i < n_unknowns; ++i)
183 PCL_DEBUG (
" %f", x[i]);
187 warp_point_->setParam (x);
188 transformation_matrix = warp_point_->getTransform ();
192 tmp_idx_src_ = tmp_idx_tgt_ =
nullptr;
196 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
201 Matrix4 &transformation_matrix)
const
203 const int nr_correspondences =
static_cast<int> (correspondences.size ());
204 std::vector<int> indices_src (nr_correspondences);
205 std::vector<int> indices_tgt (nr_correspondences);
206 for (
int i = 0; i < nr_correspondences; ++i)
208 indices_src[i] = correspondences[i].index_query;
209 indices_tgt[i] = correspondences[i].index_match;
212 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
216 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
227 for (
int i = 0; i <
values (); ++i)
229 const PointSource & p_src = src_points.
points[i];
230 const PointTarget & p_tgt = tgt_points.
points[i];
234 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
237 fvec[i] =
estimator_->computeDistance (p_src_warped, p_tgt);
243 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
249 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
250 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
253 estimator_->warp_point_->setParam (x);
256 for (
int i = 0; i < values (); ++i)
258 const PointSource & p_src = src_points.
points[src_indices[i]];
259 const PointTarget & p_tgt = tgt_points.
points[tgt_indices[i]];
263 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
266 fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);