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",
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]);
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)
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 ());
165 x.setConstant (n_unknowns, 0);
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]);
188 transformation_matrix =
warp_point_->getTransform ();
196 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void 201 Matrix4 &transformation_matrix)
const 203 const int nr_correspondences =
static_cast<const 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;
216 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int 224 estimator_->warp_point_->setParam (x);
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);
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
WarpPointRigid3D enables 6D (3D rotation + 3D translation) transformations for points.