Point clouds registration with scale estimation using ICP or directly with TransformationEstimationSVDScale.

classic Classic list List threaded Threaded
3 messages Options
Reply | Threaded
Open this post in threaded view
|

Point clouds registration with scale estimation using ICP or directly with TransformationEstimationSVDScale.

iras
This post has NOT been accepted by the mailing list yet.
 Hi guys,

I've posted on a new thread because I'm a bit stuck with point clouds registration/aligning. The point clouds have been extracted by using photogrammetry so each point cloud has a different scale.

I've tried to implement a tip suggested in an earlier post http://www.pcl-users.org/Kinect-Fusion-implementation-td4026495.html relative to setting the estimation object in the class IterativeClosestPoint class as an instance of the class TransformEstimationSVDScale in the way shown right below here.

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
pcl::registration::TransformationEstimationSVDScale <pcl::PointXYZ, pcl::PointXYZ> te;
Eigen::Matrix4f T;
icp.setInputSource (XPm2);
icp.setInputTarget (XPm1);
typedef pcl::registration::TransformationEstimationSVDScale <pcl::PointXYZ, pcl::PointXYZ> te;
boost::shared_ptr<te> teSVDscale (new te);
icp.setTransformationEstimation (teSVDscale);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align (Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
T = icp.getFinalTransformation();

The code compiles fine but I don't seem to be getting the hoped point clouds alignment. Same result if I use TransformationEstimationSVD.

If, instead, I use an instance of the class TransformationEstimationLM, I get a rough alignment but the point clouds aren't resized. By the way, I can get the same rough alignment as for the TransformationEstimationLM by directly an instance of the class TransformationEstimationSVDScale like below without ICP.

pcl::registration::TransformationEstimationSVDScale <pcl::PointXYZ, pcl::PointXYZ> te;
Eigen::Matrix4f T;
te.estimateRigidTransformation (*XPm2, *XPm1, T);
which is analogous to using the class TransformationEstimationSVD since the function estimateRigidTransformation hasn't been reimplemented in the class TransformationEstimationSVDScale.

I did some digging in the repository and couldn't find the unit test for the class TransformationEstimationSVDScale. I could find the test for the class TransformationEstimationSVD instead.

I should add that I'm coming from using the openCV's estimateAffine3D () to do the registration with no luck. Any suggestions ? I'm "a bit stuck" at the moment and any help would be much more than welcome.

Many thanks,
Ivano
Reply | Threaded
Open this post in threaded view
|

Re: Point clouds registration with scale estimation using ICP or directly with TransformationEstimationSVDScale.

iras
This post has NOT been accepted by the mailing list yet.
Ok. I managed to get the results I was looking for by hacking the class transformationEstimationSVDScale. Sorry.

The solution was straightforward and the hints where already there in the repository.
Martin changed implementation of the TransformationEstimationSVD to employ the Eigen's Umeyama method instead of the PCL re-implemention, in the commit e14e528 pushed on 2012-09-23. In a note he himself also adds that Umeyama could also be used in TransformationEstimationSVDScale.

That is precisely what I did. The result follows below and it's adapted to my own code (from above) but it's basically just the code of transformationEstimationSVD with the third parameter of pcl::umeyama (cloud_src, cloud_tgt, true) set to true to enable the scale estimation.

// Convert to Eigen format
const int npts = static_cast <int> ((*XPm2).size ());

Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_src (3, npts);
Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_tgt (3, npts);

for (int i = 0; i < npts; ++i)
{
      cloud_src (0, i) = XPm2->points[i].x;
      cloud_src (1, i) = XPm2->points[i].y;
      cloud_src (2, i) = XPm2->points[i].z;

      cloud_tgt (0, i) = XPm1->points[i].x;
      cloud_tgt (1, i) = XPm1->points[i].y;
      cloud_tgt (2, i) = XPm1->points[i].z;
}

// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
T = pcl::umeyama (cloud_src, cloud_tgt, true);

Let me know guys if you want me to change the transformationEstimationSVDScale to reflect this change and push it to the PCL repo.

Cheers,
Ivano
Reply | Threaded
Open this post in threaded view
|

Re: Point clouds registration with scale estimation using ICP or directly with TransformationEstimationSVDScale.

lanyouzibetty
This post has NOT been accepted by the mailing list yet.
Hello, iras´╝îI have two point clouds, which have different scale, different size, when I used the umeyama function, the scale dose not change, this is my codes, can you tell me why? Thank you in advance.

    Eigen::Matrix4f T;
    const int npts1 = static_cast<int>((*cloud_src_filtered).size());
    const int npts2 = static_cast<int>((*cloud_tgt_filtered).size());
    Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_src (3, npts1);
    Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_tgt (3, npts2);
    for(int i=0; i<npts1; ++i)
    {
        cloud_src(0, i)=cloud_src_filtered->points[i].x;
        cloud_src(1, i)=cloud_src_filtered->points[i].y;
        cloud_src(2, i)=cloud_src_filtered->points[i].z;      
    }
    for(int j=0; j<npts2; ++j)
    {
        cloud_tgt(0, j)=cloud_tgt_filtered->points[j].x;
        cloud_tgt(1, j)=cloud_tgt_filtered->points[j].y;
        cloud_tgt(2, j)=cloud_tgt_filtered->points[j].z;
    }

    // Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
    T = pcl::umeyama (cloud_src, cloud_tgt, true);
    cout << "T = "<<endl<<T<<endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>) ;
    pcl::transformPointCloud(*cloud_source, *cloud_transformed, T);