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);
}
}
int main (int argc, char** argv)
{
QApplication app(argc, argv);
pcl::PolygonMesh::Ptr mesh_src(new pcl::PolygonMesh);
pcl::PolygonMesh::Ptr mesh_dst(new pcl::PolygonMesh);
pcl::PointCloud<PointT>::Ptr cloud_src (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_dst (new pcl::PointCloud<PointT>);
if(argc < 3)
{
PCL_ERROR ("Incorrect usage\n");
print_usage();
}
// TODO do this with PCL console
pcl::PLYReader reader1;
//if (reader1.read(argv[1], *cloud_src) != 0) //* load the file
if (reader1.read(argv[1], *mesh_src) != 0) //* load the file
{
PCL_ERROR ("Couldn't read file %s \n", argv[1]);
return (-1);
}
pcl::fromPCLPointCloud2(mesh_src->cloud, *cloud_src);
pcl::PLYReader reader2;
//if (reader2.read(argv[2], *cloud_dst) != 0) //* load the file
if (reader2.read(argv[2], *mesh_dst) != 0) //* load the file
{
PCL_ERROR ("Couldn't read file %s \n", argv[2]);
return (-1);
}
pcl::fromPCLPointCloud2(mesh_dst->cloud, *cloud_dst);
cloud_src->sensor_orientation_ = Eigen::Quaternionf( 1.0, 0.0, 0.0, 0.0 );
cloud_src->sensor_origin_ = Eigen::Vector4f( 0.0, 0.0, 0.0, 0.0 );
cloud_dst->sensor_orientation_ = Eigen::Quaternionf( 1.0, 0.0, 0.0, 0.0 );
cloud_dst->sensor_origin_ = Eigen::Vector4f( 0.0, 0.0, 0.0, 0.0 );
ManualRegistration man_reg;
man_reg.setSrcCloud(cloud_src);
man_reg.setDstCloud(cloud_dst);
man_reg.showSrcCloud();
man_reg.showDstCloud();
man_reg.showCloud();
man_reg.show();
return (app.exec());
}
