37 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
38 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
41 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
45 Matrix4 &transformation_matrix)
const
47 size_t nr_points = cloud_src.
points.size ();
48 if (cloud_tgt.
points.size () != nr_points)
50 PCL_ERROR (
"[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.
points.size ());
56 estimateRigidTransformation (source_it, target_it, transformation_matrix);
60 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
63 const std::vector<int> &indices_src,
65 Matrix4 &transformation_matrix)
const
67 if (indices_src.size () != cloud_tgt.
points.size ())
69 PCL_ERROR (
"[pcl::Transformation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.
points.size ());
75 estimateRigidTransformation (source_it, target_it, transformation_matrix);
80 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
83 const std::vector<int> &indices_src,
85 const std::vector<int> &indices_tgt,
86 Matrix4 &transformation_matrix)
const
88 if (indices_src.size () != indices_tgt.size ())
90 PCL_ERROR (
"[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
96 estimateRigidTransformation (source_it, target_it, transformation_matrix);
100 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
105 Matrix4 &transformation_matrix)
const
109 estimateRigidTransformation (source_it, target_it, transformation_matrix);
113 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
117 Matrix4 &transformation_matrix)
const
121 Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
128 centroid_src[2] = 0.0f;
129 centroid_tgt[2] = 0.0f;
131 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
135 getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
139 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
141 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
142 const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
143 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
144 const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt,
145 Matrix4 &transformation_matrix)
const
147 transformation_matrix.setIdentity ();
150 Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
152 float angle = atan2 ((H (0, 1) - H (1, 0)), (H(0, 0) + H (1, 1)));
154 Eigen::Matrix<Scalar, 3, 3> R (Eigen::Matrix<Scalar, 3, 3>::Identity ());
155 R (0, 0) = R (1, 1) = cos (angle);
156 R (0, 1) = -sin (angle);
157 R (1, 0) = sin (angle);
160 transformation_matrix.topLeftCorner (3, 3).matrix () = R;
161 const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3).matrix ());
162 transformation_matrix.block (0, 3, 3, 1).matrix () = centroid_tgt.head (3) - Rc;
165 #endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Iterator class for point clouds with or without given indices.
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid transformation between a source and a target point cloud in 2D.
void getTransformationFromCorrelation(const Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &cloud_src_demean, const Eigen::Matrix< Scalar, 4, 1 > ¢roid_src, const Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &cloud_tgt_demean, const Eigen::Matrix< Scalar, 4, 1 > ¢roid_tgt, Matrix4 &transformation_matrix) const
Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt'.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Eigen::Matrix< Scalar, 4, 4 > Matrix4
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.