39 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ 40 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ 42 #include <pcl/registration/warp_point_rigid.h> 43 #include <pcl/registration/warp_point_rigid_6d.h> 44 #include <pcl/registration/distances.h> 45 #include <unsupported/Eigen/NonLinearOptimization> 49 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
56 , use_correspondence_weights_ (true)
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::TransformationEstimationPointToPlaneWeighted::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::TransformationEstimationPointToPlaneWeighted::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 if (correspondence_weights_.size () != cloud_src.
size ())
88 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
89 PCL_ERROR(
"Number of weights (%zu) differs than number of points (%zu)!\n",
90 correspondence_weights_.size(),
91 static_cast<std::size_t
>(cloud_src.
size()));
95 int n_unknowns = warp_point_->getDimension ();
100 tmp_src_ = &cloud_src;
101 tmp_tgt_ = &cloud_tgt;
104 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
105 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff);
106 int info = lm.minimize (x);
109 PCL_DEBUG (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]");
110 PCL_DEBUG (
"LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
111 PCL_DEBUG (
"Final solution: [%f", x[0]);
112 for (
int i = 1; i < n_unknowns; ++i)
113 PCL_DEBUG (
" %f", x[i]);
117 warp_point_->setParam (x);
118 transformation_matrix = warp_point_->getTransform ();
125 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
void 128 const std::vector<int> &indices_src,
130 Matrix4 &transformation_matrix)
const 132 if (indices_src.size () != cloud_tgt.
size ())
134 PCL_ERROR(
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::" 135 "estimateRigidTransformation] Number or points in source (%zu) differs " 136 "than target (%zu)!\n",
138 static_cast<std::size_t
>(cloud_tgt.
size()));
142 if (correspondence_weights_.size () != indices_src.size ())
144 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
145 PCL_ERROR(
"Number of weights (%zu) differs than number of points (%zu)!\n",
146 correspondence_weights_.size(),
153 transformation_matrix.setIdentity ();
155 const auto nr_correspondences = cloud_tgt.
size ();
156 std::vector<int> indices_tgt;
157 indices_tgt.resize(nr_correspondences);
158 for (std::size_t i = 0; i < nr_correspondences; ++i)
161 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
165 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void 168 const std::vector<int> &indices_src,
170 const std::vector<int> &indices_tgt,
171 Matrix4 &transformation_matrix)
const 173 if (indices_src.size () != indices_tgt.size ())
175 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
179 if (indices_src.size () < 4)
181 PCL_ERROR (
"[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
182 PCL_ERROR (
"Need at least 4 points to estimate a transform! Source and target have %lu points!",
183 indices_src.size ());
187 if (correspondence_weights_.size () != indices_src.size ())
189 PCL_ERROR (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] ");
190 PCL_ERROR (
"Number of weights (%lu) differs than number of points (%lu)!\n",
191 correspondence_weights_.size (), indices_src.size ());
196 int n_unknowns = warp_point_->getDimension ();
198 x.setConstant (n_unknowns, 0);
201 tmp_src_ = &cloud_src;
202 tmp_tgt_ = &cloud_tgt;
203 tmp_idx_src_ = &indices_src;
204 tmp_idx_tgt_ = &indices_tgt;
207 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
208 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff);
209 int info = lm.minimize (x);
212 PCL_DEBUG (
"[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
213 PCL_DEBUG (
"Final solution: [%f", x[0]);
214 for (
int i = 1; i < n_unknowns; ++i)
215 PCL_DEBUG (
" %f", x[i]);
219 warp_point_->setParam (x);
220 transformation_matrix = warp_point_->getTransform ();
224 tmp_idx_src_ = tmp_idx_tgt_ = NULL;
228 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
inline void 233 Matrix4 &transformation_matrix)
const 235 const int nr_correspondences =
static_cast<int> (correspondences.size ());
236 std::vector<int> indices_src (nr_correspondences);
237 std::vector<int> indices_tgt (nr_correspondences);
238 for (
int i = 0; i < nr_correspondences; ++i)
240 indices_src[i] = correspondences[i].index_query;
241 indices_tgt[i] = correspondences[i].index_match;
244 if (use_correspondence_weights_)
246 correspondence_weights_.resize (nr_correspondences);
247 for (std::size_t i = 0; i < nr_correspondences; ++i)
248 correspondence_weights_[i] = correspondences[i].weight;
251 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
255 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int 263 estimator_->warp_point_->setParam (x);
266 for (
int i = 0; i < values (); ++i)
268 const PointSource & p_src = src_points[i];
269 const PointTarget & p_tgt = tgt_points[i];
273 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
276 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt);
282 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar>
int 288 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
289 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
292 estimator_->warp_point_->setParam (x);
295 for (
int i = 0; i < values (); ++i)
297 const PointSource & p_src = src_points[src_indices[i]];
298 const PointTarget & p_tgt = tgt_points[tgt_indices[i]];
302 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
305 fvec[i] = estimator_->correspondence_weights_[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