template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
// Convert to Eigen format
const int npts = static_cast <int> (source_it.size ());
Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
for (int i = 0; i < npts; ++i)
{
cloud_src (0, i) = source_it->x;
cloud_src (1, i) = source_it->y;
cloud_src (2, i) = source_it->z;
++source_it;
cloud_tgt (0, i) = target_it->x;
cloud_tgt (1, i) = target_it->y;
cloud_tgt (2, i) = target_it->z;
++target_it;
}
if (use_umeyama_)
{
// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
}
else
{
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
}
template <typename PointT, typename Scalar> void
pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
const Eigen::Matrix<Scalar, 4, 1> ¢roid,
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
int npts)
{
// Calculate the number of points if not given
if (npts == 0)
{
while (cloud_iterator.isValid ())
{
++npts;
++cloud_iterator;
}
cloud_iterator.reset ();
}
cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
int i = 0;
while (cloud_iterator.isValid ())
{
cloud_out (0, i) = cloud_iterator->x - centroid[0];
cloud_out (1, i) = cloud_iterator->y - centroid[1];
cloud_out (2, i) = cloud_iterator->z - centroid[2];
++i;
++cloud_iterator;
}
}
template <typename PointT, typename Scalar> void
pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
const Eigen::Matrix<Scalar, 4, 1> ¢roid,
pcl::PointCloud<PointT> &cloud_out,
int npts)
{
// Calculate the number of points if not given
if (npts == 0)
{
while (cloud_iterator.isValid ())
{
++npts;
++cloud_iterator;
}
cloud_iterator.reset ();
}
int i = 0;
cloud_out.resize (npts);
// Subtract the centroid from cloud_in
while (cloud_iterator.isValid ())
{
cloud_out[i].x = cloud_iterator->x - centroid[0];
cloud_out[i].y = cloud_iterator->y - centroid[1];
cloud_out[i].z = cloud_iterator->z - centroid[2];
++i;
++cloud_iterator;
}
cloud_out.width = cloud_out.size ();
cloud_out.height = 1;
}
TEST (PCL, compute3DCentroidCloudIterator)
{
pcl::PointIndices pindices;
std::vector<int> indices;
PointXYZ point;
PointCloud<PointXYZ> cloud;
Eigen::Vector4f centroid_f;
for (point.x = -1; point.x < 2; point.x += 2)
{
for (point.y = -1; point.y < 2; point.y += 2)
{
for (point.z = -1; point.z < 2; point.z += 2)
{
cloud.push_back (point);
}
}
}
cloud.is_dense = true;
indices.resize (4); // only positive y values
indices [0] = 2;
indices [1] = 3;
indices [2] = 6;
indices [3] = 7;
// Test finite data
{
ConstCloudIterator<PointXYZ> it (cloud, indices);
EXPECT_EQ (compute3DCentroid (it, centroid_f), 4);
EXPECT_EQ (centroid_f[0], 0.0f);
EXPECT_EQ (centroid_f[1], 1.0f);
EXPECT_EQ (centroid_f[2], 0.0f);
EXPECT_EQ (centroid_f[3], 1.0f);
Eigen::Vector4d centroid_d;
it.reset ();
EXPECT_EQ (compute3DCentroid (it, centroid_d), 4);
EXPECT_EQ (centroid_d[0], 0.0);
EXPECT_EQ (centroid_d[1], 1.0);
EXPECT_EQ (centroid_d[2], 0.0);
EXPECT_EQ (centroid_d[3], 1.0);
}
// Test for non-finite data
{
point.getVector3fMap() << std::numeric_limits<float>::quiet_NaN (),
std::numeric_limits<float>::quiet_NaN (),
std::numeric_limits<float>::quiet_NaN ();
cloud.push_back (point);
cloud.is_dense = false;
ConstCloudIterator<PointXYZ> it (cloud);
EXPECT_EQ (8, compute3DCentroid (it, centroid_f));
EXPECT_EQ_VECTORS (Eigen::Vector4f (0.f, 0.f, 0.f, 1.f), centroid_f);
}
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationTranslation<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
getTransformationFromCorrelation (centroid_src, centroid_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
Matrix4 &transformation_matrix) const
{
source_it.reset (); target_it.reset ();
Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
// ignore z component
centroid_src[2] = 0.0f;
centroid_tgt[2] = 0.0f;
// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);
getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}