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.
size () != cloud_tgt.
size ())
71 PCL_ERROR (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
72 PCL_ERROR(
"Number or points in source (%zu) differs than target (%zu)!\n",
73 static_cast<std::size_t
>(cloud_src.
size()),
74 static_cast<std::size_t
>(cloud_tgt.
size()));
77 if (cloud_src.
size () < 4)
79 PCL_ERROR (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
80 PCL_ERROR(
"Need at least 4 points to estimate a transform! Source and target have "
82 static_cast<std::size_t
>(cloud_src.
size()));
86 int n_unknowns = warp_point_->getDimension ();
87 VectorX x (n_unknowns);
91 tmp_src_ = &cloud_src;
92 tmp_tgt_ = &cloud_tgt;
94 OptimizationFunctor functor (
static_cast<int> (cloud_src.
size ()),
this);
95 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
97 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
98 int info = lm.minimize (x);
101 PCL_DEBUG (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
102 PCL_DEBUG (
"LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
103 PCL_DEBUG (
"Final solution: [%f", x[0]);
104 for (
int i = 1; i < n_unknowns; ++i)
105 PCL_DEBUG (
" %f", x[i]);
109 warp_point_->setParam (x);
110 transformation_matrix = warp_point_->getTransform ();
117 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void
120 const std::vector<int> &indices_src,
122 Matrix4 &transformation_matrix)
const
124 if (indices_src.size () != cloud_tgt.
size ())
127 "[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "
128 "Number or points in source (%zu) differs than target (%zu)!\n",
130 static_cast<std::size_t
>(cloud_tgt.
size()));
135 transformation_matrix.setIdentity ();
137 const auto nr_correspondences = cloud_tgt.
size ();
138 std::vector<int> indices_tgt;
139 indices_tgt.resize(nr_correspondences);
140 for (std::size_t i = 0; i < nr_correspondences; ++i)
143 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
147 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
150 const std::vector<int> &indices_src,
152 const std::vector<int> &indices_tgt,
153 Matrix4 &transformation_matrix)
const
155 if (indices_src.size () != indices_tgt.size ())
157 PCL_ERROR (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
161 if (indices_src.size () < 4)
163 PCL_ERROR (
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
164 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!",
165 indices_src.size ());
169 int n_unknowns = warp_point_->getDimension ();
170 VectorX x (n_unknowns);
171 x.setConstant (n_unknowns, 0);
174 tmp_src_ = &cloud_src;
175 tmp_tgt_ = &cloud_tgt;
176 tmp_idx_src_ = &indices_src;
177 tmp_idx_tgt_ = &indices_tgt;
179 OptimizationFunctorWithIndices functor (
static_cast<int> (indices_src.size ()),
this);
180 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
182 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
183 int info = lm.minimize (x);
186 PCL_DEBUG (
"[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
187 PCL_DEBUG (
"Final solution: [%f", x[0]);
188 for (
int i = 1; i < n_unknowns; ++i)
189 PCL_DEBUG (
" %f", x[i]);
193 warp_point_->setParam (x);
194 transformation_matrix = warp_point_->getTransform ();
198 tmp_idx_src_ = tmp_idx_tgt_ =
nullptr;
202 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void
207 Matrix4 &transformation_matrix)
const
209 const auto nr_correspondences = correspondences.size ();
210 std::vector<int> indices_src (nr_correspondences);
211 std::vector<int> indices_tgt (nr_correspondences);
212 for (std::size_t i = 0; i < nr_correspondences; ++i)
214 indices_src[i] = correspondences[i].index_query;
215 indices_tgt[i] = correspondences[i].index_match;
218 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
222 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
230 estimator_->warp_point_->setParam (x);
233 for (
int i = 0; i < values (); ++i)
235 const PointSource & p_src = src_points[i];
236 const PointTarget & p_tgt = tgt_points[i];
240 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
243 fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
249 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int
255 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
256 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
259 estimator_->warp_point_->setParam (x);
262 for (
int i = 0; i < values (); ++i)
264 const PointSource & p_src = src_points[src_indices[i]];
265 const PointTarget & p_tgt = tgt_points[tgt_indices[i]];
269 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
272 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