void pairAlign3(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, Eigen::Matrix4f &final_transform )
{
    PointCloud::Ptr src (new PointCloud);
	PointCloud::Ptr tgt (new PointCloud);

	*src = *cloud_src;
	*tgt = *cloud_tgt;


	// ICP object.
	PointCloud::Ptr finalCloud(new PointCloud);
	pcl::IterativeClosestPointNonLinear<PointT , PointT> registration;
	registration.setInputSource( src );
	registration.setInputTarget( tgt );
	registration.setTransformationEpsilon( 0.001 );

	registration.align(*finalCloud);
	if (registration.hasConverged())
	{
		//std::cout << "ICP converged." << std::endl
		//		  << "The score is " << registration.getFitnessScore() << std::endl;
		//std::cout << "Transformation matrix:" << std::endl;
		//std::cout << registration.getFinalTransformation() << std::endl;
	}
	else std::cout << "ICP did not converge." << std::endl;

	final_transform = registration.getFinalTransformation();


}
Exemplo n.º 2
0
int
main(int argc, char** argv)
{
	// Objects for storing the point clouds.
	pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZ>);
 
	// Read two PCD files from disk.
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *sourceCloud) != 0)
	{
		return -1;
	}
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *targetCloud) != 0)
	{
		return -1;
	}
 
	// ICP object.
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration;
	registration.setInputSource(sourceCloud);
	registration.setInputTarget(targetCloud);
 
	registration.align(*finalCloud);
	if (registration.hasConverged())
	{
		std::cout << "ICP converged." << std::endl
				  << "The score is " << registration.getFitnessScore() << std::endl;
		std::cout << "Transformation matrix:" << std::endl;
		std::cout << registration.getFinalTransformation() << std::endl;
	}
	else std::cout << "ICP did not converge." << std::endl;
}
Exemplo n.º 3
0
int main(int argc, char* argv[])
{
    std::cout<<"Merging maps...\n";

    // *** Check input
    if(argc < 4)
    {
        std::cout<<"Usage: ./mergeMaps <map1.pcd> <map2.pcd> <mergedMap.pcd> [transform-2-to-1.txt]";
        return -1;
    }

    // *** Read data
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>());

    if (pcl::io::loadPCDFile (argv[1], *cloud1) < 0)
    {
          std::cout << "Error loading point cloud " << argv[1] << std::endl;
          return -1;
    }

    if (pcl::io::loadPCDFile (argv[2], *cloud2) < 0)
    {
          std::cout << "Error loading point cloud " << argv[2] << std::endl;
          return -1;
    }

    // *** Transformation (optional)
    Eigen::Matrix4f transform;

    if(argc > 4)
        readTransform(argv[4], transform);
    else
        transform.setIdentity();

    pcl::transformPointCloud(*cloud1, *cloud1, transform);

    // *** Merging
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    *finalCloud = *cloud1 + *cloud2;

    // *** Save to file
    if (pcl::io::savePCDFile(argv[3], *finalCloud) < 0)
    {
        std::cout << "Error saving the point cloud in "<<argv[4] << std::endl;
        return -1;
    }

    std::cout << "Successfully merged maps" << std::endl;
    return 0;
}