40 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_
41 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_
43 #include <pcl/registration/transformation_estimation.h>
44 #include <pcl/registration/warp_point_rigid.h>
45 #include <pcl/registration/distances.h>
49 namespace registration
58 template <
typename Po
intSource,
typename Po
intTarget,
typename MatScalar =
float>
71 typedef boost::shared_ptr<TransformationEstimationLM<PointSource, PointTarget, MatScalar> >
Ptr;
72 typedef boost::shared_ptr<const TransformationEstimationLM<PointSource, PointTarget, MatScalar> >
ConstPtr;
74 typedef Eigen::Matrix<MatScalar, Eigen::Dynamic, 1>
VectorX;
75 typedef Eigen::Matrix<MatScalar, 4, 1>
Vector4;
117 Matrix4 &transformation_matrix)
const;
128 const std::vector<int> &indices_src,
130 Matrix4 &transformation_matrix)
const;
143 const std::vector<int> &indices_src,
145 const std::vector<int> &indices_tgt,
146 Matrix4 &transformation_matrix)
const;
159 Matrix4 &transformation_matrix)
const;
184 Vector4 s (p_src.x, p_src.y, p_src.z, 0);
185 Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
186 return ((s - t).norm ());
201 Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
202 return ((p_src - t).norm ());
218 boost::shared_ptr<pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar> >
warp_point_;
224 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
233 typedef Eigen::Matrix<_Scalar,InputsAtCompileTime,1>
InputType;
234 typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>
ValueType;
235 typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>
JacobianType;
297 operator () (
const VectorX &x, VectorX &fvec)
const;
343 operator () (
const VectorX &x, VectorX &fvec)
const;
348 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
353 #include <pcl/registration/impl/transformation_estimation_lm.hpp>
virtual MatScalar computeDistance(const PointSource &p_src, const PointTarget &p_tgt) const
Compute the distance between a source point and its corresponding target point.
Eigen::Matrix< MatScalar, 4, 1 > Vector4
TransformationEstimationLM implements Levenberg Marquardt-based estimation of the transformation alig...
OptimizationFunctorWithIndices(const OptimizationFunctorWithIndices &src)
Copy constructor.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Eigen::Matrix< _Scalar, InputsAtCompileTime, 1 > InputType
boost::shared_ptr< const TransformationEstimationLM< PointSource, PointTarget, MatScalar > > ConstPtr
virtual MatScalar computeDistance(const Vector4 &p_src, const PointTarget &p_tgt) const
Compute the distance between a source point and its corresponding target point.
TransformationEstimation represents the base class for methods for transformation estimation based on...
Functor()
Empty Construtor.
OptimizationFunctor & operator=(const OptimizationFunctor &src)
Copy operator.
OptimizationFunctor(const OptimizationFunctor &src)
Copy constructor.
int values() const
Get the number of values.
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
void setWarpFunction(const boost::shared_ptr< WarpPointRigid< PointSource, PointTarget, MatScalar > > &warp_fcn)
Set the function we use to warp points.
TransformationEstimationLM(const TransformationEstimationLM &src)
Copy constructor.
const TransformationEstimationLM< PointSource, PointTarget, MatScalar > * estimator_
Eigen::Matrix< _Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
OptimizationFunctor(int m_data_points, const TransformationEstimationLM *estimator)
Functor constructor.
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid rotation transformation between a source and a target point cloud using LM...
Base functor all the models that need non linear optimization must define their own one and implement...
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
OptimizationFunctorWithIndices & operator=(const OptimizationFunctorWithIndices &src)
Copy operator.
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
OptimizationFunctorWithIndices(int m_data_points, const TransformationEstimationLM *estimator)
Functor constructor.
int operator()(const VectorX &x, VectorX &fvec) const
Fill fvec from x.
int operator()(const VectorX &x, VectorX &fvec) const
Fill fvec from x.
boost::shared_ptr< ::pcl::PointIndices > Ptr
TransformationEstimationLM()
Constructor.
virtual ~Functor()
Destructor.
TransformationEstimation< PointSource, PointTarget, MatScalar >::Matrix4 Matrix4
const TransformationEstimationLM< PointSource, PointTarget, MatScalar > * estimator_
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
virtual ~OptimizationFunctor()
Destructor.
virtual ~TransformationEstimationLM()
Destructor.
boost::shared_ptr< PointCloud< PointSource > > Ptr
Eigen::Matrix< MatScalar, Eigen::Dynamic, 1 > VectorX
boost::shared_ptr< pcl::registration::WarpPointRigid< PointSource, PointTarget, MatScalar > > warp_point_
The parameterized function used to warp the source to the target.
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
boost::shared_ptr< TransformationEstimationLM< PointSource, PointTarget, MatScalar > > Ptr
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
TransformationEstimationLM & operator=(const TransformationEstimationLM &src)
Copy operator.
Functor(int m_data_points)
Constructor.
Eigen::Matrix< _Scalar, ValuesAtCompileTime, 1 > ValueType
virtual ~OptimizationFunctorWithIndices()
Destructor.