template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Matrix4 &transformation_matrix) const
{
ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Matrix4 &transformation_matrix) const
{
ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
std::vector<Scalar> weights (correspondences.size ());
for (size_t i = 0; i < correspondences.size (); ++i)
weights[i] = correspondences[i].weight;
typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
if (indices_src.size () != cloud_tgt.points.size ())
{
PCL_ERROR ("[pcl::Transformation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
return;
}
ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
size_t nr_points = cloud_src.points.size ();
if (cloud_tgt.points.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ());
return;
}
ConstCloudIterator<PointSource> source_it (cloud_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename MatScalar> inline void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Matrix4 &transformation_matrix) const
{
const int nr_correspondences = static_cast<const int> (correspondences.size ());
std::vector<int> indices_src (nr_correspondences);
std::vector<int> indices_tgt (nr_correspondences);
for (int i = 0; i < nr_correspondences; ++i)
{
indices_src[i] = correspondences[i].index_query;
indices_tgt[i] = correspondences[i].index_match;
}
estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
size_t nr_points = cloud_src.points.size ();
if (cloud_tgt.points.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ());
return;
}
if (weights_.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
return;
}
ConstCloudIterator<PointSource> source_it (cloud_src);
ConstCloudIterator<PointTarget> target_it (cloud_tgt);
typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename MatScalar> void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Matrix4 &transformation_matrix) const
{
if (indices_src.size () != cloud_tgt.points.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ());
return;
}
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
const int nr_correspondences = static_cast<const int> (cloud_tgt.points.size ());
std::vector<int> indices_tgt;
indices_tgt.resize(nr_correspondences);
for (int i = 0; i < nr_correspondences; ++i)
indices_tgt[i] = i;
estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
}