/**
  * Code example for ICP taking a sequence of point clouds relatively close 
  * and build a map with them.
  * It assumes that: 3D point clouds are used, they were recorded in sequence
  * and they are express in sensor frame.
  */
int main(int argc, char *argv[])
{
	validateArgs(argc, argv);

	typedef PointMatcher<float> PM;
	typedef PointMatcherIO<float> PMIO;
	typedef PM::TransformationParameters TP;
	typedef PM::DataPoints DP;
	
	string outputFileName(argv[0]);
	
	// Rigid transformation
	PM::Transformation* rigidTrans;
	rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");

	// Create filters manually to clean the global map
	PM::DataPointsFilter* densityFilter(
					PM::get().DataPointsFilterRegistrar.create(
						"SurfaceNormalDataPointsFilter",
						map_list_of
						("knn", "10")
						("epsilon", "5") 
						("keepNormals", "0")
						("keepDensities", "1")
						)
					);

	PM::DataPointsFilter* maxDensitySubsample(
					PM::get().DataPointsFilterRegistrar.create(
						"MaxDensityDataPointsFilter",
						map_list_of
						("maxDensity", toParam(30))
						)
					);
	// Main algorithm definition
	PM::ICP icp;

	// load YAML config
	ifstream ifs(argv[1]);
	validateFile(argv[1]);
	icp.loadFromYaml(ifs);

	PMIO::FileInfoVector list(argv[2]);

	PM::DataPoints mapPointCloud, newCloud;
	TP T_to_map_from_new = TP::Identity(4,4); // assumes 3D

	for(unsigned i=0; i < list.size(); i++)
	{
		cout << "---------------------\nLoading: " << list[i].readingFileName << endl; 

		// It is assume that the point cloud is express in sensor frame
		newCloud = DP::load(list[i].readingFileName);
		
		if(mapPointCloud.getNbPoints()  == 0)
		{
			mapPointCloud = newCloud;
			continue;
		}

		// call ICP
		try 
		{
			// We use the last transformation as a prior
			// this assumes that the point clouds were recorded in 
			// sequence. 
			const TP prior = T_to_map_from_new;

			T_to_map_from_new = icp(newCloud, mapPointCloud, prior);
		}
		catch (PM::ConvergenceError& error)
		{
			cout << "ERROR PM::ICP failed to converge: " << endl;
			cout << "   " << error.what() << endl;
			continue;
		}

		// This is not necessary in this example, but could be
		// useful if the same matrix is composed in the loop.
		T_to_map_from_new = rigidTrans->correctParameters(T_to_map_from_new);

		// Move the new point cloud in the map reference
		newCloud = rigidTrans->compute(newCloud, T_to_map_from_new);

		// Merge point clouds to map
		mapPointCloud.concatenate(newCloud);

		// Clean the map
		mapPointCloud = densityFilter->filter(mapPointCloud);
		mapPointCloud = maxDensitySubsample->filter(mapPointCloud);

		// Save the map at each iteration
		stringstream outputFileNameIter;
		outputFileNameIter << outputFileName << "_" << i << ".vtk";

		cout << "outputFileName: " << outputFileNameIter.str() << endl;
		mapPointCloud.save(outputFileNameIter.str());
	}

	return 0;
}
Exemplo n.º 2
1
/**
  * Code example for ICP taking 2 points clouds (2D or 3D) relatively close
  * and computing the transformation between them.
  *
  * This code is more complete than icp_simple. It can load parameter files and
  * has more options.
  */
int main(int argc, const char *argv[])
{
	bool isTransfoSaved = false;
	string configFile;
	string outputBaseFile("test");
	string format("vtk")
	string initTranslation("0,0,0");
	string initRotation("1,0,0;0,1,0;0,0,1");
	const int ret = validateArgs(argc, argv, isTransfoSaved, configFile,
								 outputBaseFile, format, initTranslation, initRotation);
	if (ret != 0)
	{
		return ret;
	}
	const char *refFile(argv[argc-2]);
	const char *dataFile(argv[argc-1]);

	// Load point clouds
	const DP ref(DP::load(refFile));
	const DP data(DP::load(dataFile));

	// Create the default ICP algorithm
	PM::ICP icp;

	if (configFile.empty())
	{
		// See the implementation of setDefault() to create a custom ICP algorithm
		icp.setDefault();
	}
	else
	{
		// load YAML config
		ifstream ifs(configFile.c_str());
		if (!ifs.good())
		{
			cerr << "Cannot open config file " << configFile << ", usage:"; usage(argv); exit(1);
		}
		icp.loadFromYaml(ifs);
	}

	int cloudDimension = ref.getEuclideanDim();
	
	if (!(cloudDimension == 2 || cloudDimension == 3)) 
	{
		cerr << "Invalid input point clouds dimension = " << cloudDimension << endl;
		exit(1);
	}

	PM::TransformationParameters translation =
			parseTranslation(initTranslation, cloudDimension);
	PM::TransformationParameters rotation =
			parseRotation(initRotation, cloudDimension);
	PM::TransformationParameters initTransfo = translation*rotation;

	PM::Transformation* rigidTrans;
	rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");

	if (!rigidTrans->checkParameters(initTransfo)) {
		cerr << endl
			 << "Initial transformation is not rigid, identiy will be used"
			 << endl;
		initTransfo = PM::TransformationParameters::Identity(
					cloudDimension+1,cloudDimension+1);
	}

	const DP initializedData = rigidTrans->compute(data, initTransfo);

	// Compute the transformation to express data in ref
	PM::TransformationParameters T = icp(initializedData, ref);
	// cout << outputBaseFile << " match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl;

	// Transform data to express it in ref
	DP data_out(initializedData);
	icp.transformations.apply(data_out, T);

	// Safe files to see the results
	ref.save(outputBaseFile + "_ref." + format);
	data.save(outputBaseFile + "_data_in" + format);
	data_out.save(outputBaseFile + "_data_out" + format);
	if(isTransfoSaved) {
		ofstream transfoFile;
		string initFileName = outputBaseFile + "_init_transfo.txt";
		string icpFileName = outputBaseFile + "_icp_transfo.txt";
		string completeFileName = outputBaseFile + "_complete_transfo.txt";

		transfoFile.open(initFileName.c_str());
		if(transfoFile.is_open()) {
			transfoFile << initTransfo << endl;
			transfoFile.close();
		} else {
			cout << "Unable to write the initial transformation file\n" << endl;
		}

		transfoFile.open(icpFileName.c_str());
		if(transfoFile.is_open()) {
			transfoFile << T << endl;
			transfoFile.close();
		} else {
			cout << "Unable to write the ICP transformation file\n" << endl;
		}

		transfoFile.open(completeFileName.c_str());
		if(transfoFile.is_open()) {
			transfoFile << T*initTransfo << endl;
			transfoFile.close();
		} else {
			cout << "Unable to write the complete transformation file\n" << endl;
		}
	}
	else {
		cout << "ICP transformation:" << endl << T << endl;
	}

	return 0;
}
Exemplo n.º 3
0
TEST(IOTest, loadYaml)
{

    // Test loading configuration files for data filters
    std::ifstream ifs0((dataPath + "default-convert.yaml").c_str());
    EXPECT_NO_THROW(PM::DataPointsFilters filters(ifs0));

    // Test loading configuration files for ICP
    PM::ICP icp;

    std::ifstream ifs1((dataPath + "default.yaml").c_str());
    EXPECT_NO_THROW(icp.loadFromYaml(ifs1));

    std::ifstream ifs2((dataPath + "unit_tests/badIcpConfig_InvalidParameter.yaml").c_str());
    EXPECT_THROW(icp.loadFromYaml(ifs2), PointMatcherSupport::Parametrizable::InvalidParameter);

    std::ifstream ifs3((dataPath + "unit_tests/badIcpConfig_InvalidModuleType.yaml").c_str());
    EXPECT_THROW(icp.loadFromYaml(ifs3), PointMatcherSupport::InvalidModuleType);
}
Exemplo n.º 4
0
void transformation (int argc, char *argv[])
{
	bool isCSV = true;
	
	typedef PointMatcher<float> PM;
	typedef PM::DataPoints DP;
	typedef PM::Parameters Parameters;
	
	// Load point clouds
	//const DP ref(DP::load(argv[1]));
	//const DP data(DP::load(argv[2]));

	const DP ref(DP::load("Golden.csv"));
	const DP data(DP::load("Final.csv"));
	// Create the default ICP algorithm
	PM::ICP icp;
	
    string configFile = "my_config.yaml";
    
	if (configFile.empty())
	{
		// See the implementation of setDefault() to create a custom ICP algorithm
		icp.setDefault();
	}
	else
	{
		// load YAML config
		ifstream ifs(configFile.c_str());
		if (!ifs.good())
		{
			cout << "Cannot open config file" << configFile;
		}
		icp.loadFromYaml(ifs);
	}


	// Compute the transformation to express data in ref
	PM::TransformationParameters T = icp(data, ref);
	

	// Transform data to express it in ref
	DP data_out(data);
	icp.transformations.apply(data_out, T);
	
	// Safe files to see the results
	ref.save("test_ref.vtk");
	data.save("test_data_in.vtk");
	data_out.save("test_data_out.vtk");
	cout << "Final transformation:" << endl << T << endl;

    // Updating transform matrix 
    T(0,3) = offset_x;
    T(1,3) = offset_y;
    T(2,3) = offset_z;
	cout << "Final transformation:" << endl << T << endl;

    if(MATRIX_DUMP == 1) {
        for(int i = 0; i < 4; i++)
        {
            Trans_dump << T(i,0) <<"," <<T(i,1) <<"," << T(i,2) << "," << T(i,3) <<endl;
        }
         Trans_dump << endl << endl;
    }
     
    float R[3][3];
    
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
            R[j][i] = T(i,j); // Euler angle function uses Column Major
    }
    	
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    //Eigen::Affine3f A;
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
            transform_1 (i,j) = T(i,j); // Euler angle function uses Column Major
            //transform_1 (i,j) = R[i][j]; // Euler angle function uses Column Major
    }
    
    cout << "Writing to cloud transformed" <<endl;
    pcl::transformPointCloud (*cloud_golden, *cloud_transformed, transform_1);
    eulerAngles(R); 

    cout << "Euler Angles : X  = "  << Angle[0] * 180/ PI  << ", Y = " << Angle[1] * 180/ PI << " , Z = " <<    Angle[2] * 180/ PI << endl;
    //pcl::getTransformation(0,0,0,-1*Angle[2],-1*Angle[0],-1*Angle[1]);
    //transform_1 = A.matrix();
    //pcl::transformPointCloud (*cloud_golden, *cloud_transformed, transform_1);

}
Exemplo n.º 5
0
TEST(icpTest, icpTest)
{
	DP ref  = DP::load(dataPath + "cloud.00000.vtk");
	DP data = DP::load(dataPath + "cloud.00001.vtk");

	namespace fs = boost::filesystem;
	fs::path config_dir(dataPath + "icp_data");
	EXPECT_TRUE( fs::exists(config_dir) && fs::is_directory(config_dir) );

	fs::directory_iterator end_iter;
	for( fs::directory_iterator d(config_dir); d != end_iter; ++d)
	{
		if (!fs::is_regular_file(d->status()) ) continue;

		// Load config file, and form ICP object
		PM::ICP icp;
		std::string config_file = d->path().string();
		if (fs::extension(config_file) != ".yaml") continue;
		std::ifstream ifs(config_file.c_str());
		EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << "   " << config_file;

		// Compute current ICP transform
		PM::TransformationParameters curT = icp(data, ref);

		// Write current transform to disk (to easily compare it
		// with reference transform offline)
		fs::path cur_file = d->path();
		cur_file.replace_extension(".cur_trans");
		//std::cout << "Writing: " << cur_file << std::endl;
		std::ofstream otfs(cur_file.c_str());
		otfs.precision(16);
		otfs << curT;
		otfs.close();
                
		// Load reference transform
		fs::path ref_file = d->path();
		ref_file.replace_extension(".ref_trans");
		PM::TransformationParameters refT = 0*curT;
		//std::cout << "Reading: " << ref_file << std::endl;
		std::ifstream itfs(ref_file.c_str());
		for (int row = 0; row < refT.cols(); row++)
		{
			for (int col = 0; col < refT.cols(); col++)
			{
				itfs >>refT(row, col);
			}
		}

		// Dump the reference transform and current one
		//std::cout.precision(17);
		//std::cout << "refT:\n" << refT << std::endl;
		//std::cout << "curT:\n" << curT << std::endl;

		// Tolerance for change in rotation and translation
		double rotTol = 0.1, transTol = 0.1;

		// Find how much the reference rotation and translation
		// differ from the current values.
		PM::TransformationParameters refRot   = refT.block(0, 0, 3, 3);
		PM::TransformationParameters refTrans = refT.block(0, 3, 3, 1);
		PM::TransformationParameters curRot   = curT.block(0, 0, 3, 3);
		PM::TransformationParameters curTrans = curT.block(0, 3, 3, 1);
		PM::TransformationParameters rotErrMat = refRot*(curRot.transpose())
		  - PM::TransformationParameters::Identity(3, 3);
		PM::TransformationParameters transErrMat = refTrans - curTrans;
		double rotErr = rotErrMat.array().abs().sum();
		double transErr = transErrMat.array().abs().sum();

		//std::cout << "Rotation error:    " << rotErr   << std::endl;
		//std::cout << "Translation error: " << transErr << std::endl;
		
		EXPECT_LT(rotErr,   rotTol) << "This error was caused by the test file:" << endl << "   " << config_file;
		EXPECT_LT(transErr, transTol) << "This error was caused by the test file:" <<  endl << "   " << config_file;
	}
}
Exemplo n.º 6
0
void transformation (Mat& rgbframe)
{
	bool isCSV = true;
    typedef PointMatcher<float> PM;
    typedef PM::DataPoints DP;	
	typedef PM::Parameters Parameters;
	
	// Load point clouds
	//const DP ref(DP::load(argv[1]));
	//const DP data(DP::load(argv[2]));

	const DP ref(DP::load("Golden.csv"));

    std::ostringstream fileNameStream("");
    fileNameStream << "Capture" << filenumber - 1 << ".csv";
    string Inputname = fileNameStream.str();

	const DP data(DP::load(Inputname.c_str()));
	//const DP data(DP::load("Final.csv"));
	// Create the default ICP algorithm
	PM::ICP icp;
	
    string configFile = "my_config.yaml";
    
	if (configFile.empty())
	{
		// See the implementation of setDefault() to create a custom ICP algorithm
		icp.setDefault();
	}
	else
	{
		// load YAML config
		ifstream ifs(configFile.c_str());
		if (!ifs.good())
		{
			cout << "Cannot open config file" << configFile;
		}
		icp.loadFromYaml(ifs);
	}


	// Compute the transformation to express data in ref
	PM::TransformationParameters T = icp(data, ref);
	

	// Transform data to express it in ref
	DP data_out(data);
	icp.transformations.apply(data_out, T);
	
	// Safe files to see the results
    cout << "match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl;
    //cout << "Iteration Count: " << icp.iterationCount  << endl;
	//ref.save("test_ref.vtk");
	//data.save("test_data_in.vtk");
	//data_out.save("test_data_out.vtk");
	//cout << "Final transformation:" << endl << T << endl;

    // Updating transform matrix 
    T(0,3) += offset_x;
    T(1,3) += offset_y;
    T(2,3) += offset_z;
	cout << "Final transformation:" << endl << T << endl;


 //   float R[3][3];
 //   
 //   for(int i = 0; i < 3; i++)
 //   {
 //       for(int j = 0; j < 3; j++)
 //           R[j][i] = T(i,j); // Euler angle function uses Column Major
 //   }

//    eulerAngles(R); 
//
//	cout << "Euler Angles : X  = "  << Angle[0] * 180/ PI  << ", Y = " << Angle[1] * 180/ PI << " , Z = " <<    Angle[2] * 180/ PI << endl;
//
//    if(MATRIX_DUMP == 1) {
//        //for(int i = 0; i < 4; i++)
//        //{
//            Trans_dump << T(0,3) <<","<< T(1,3) << "," << T(2,3) << ","  << Angle[0] * 180/ PI  << "," << Angle[1] * 180/ PI << "," << Angle[2] * 180/ PI << endl;
        //}
         //Trans_dump << endl << endl;
    

    // Tranforming golden and showing it as image

//    for(int i = 0; i < 3; i++)
//    {
//        for(int j = 0; j < 3; j++)
//            if( i != j )
//            T(i,j) = -T(i,j); // Euler angle function uses Column Major
//    }

    
    //T(0,3) = 0;
    //T(1,3) = 0;
    //T(2,3) = 0;

 //   T = T.inverse();

 //   PM::Transformation* rigidTrans;
 //   rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");

 //   if (!rigidTrans->checkParameters(T)) {
 //       std::cout << "WARNING: T does not represent a valid rigid transformation\nProjecting onto an orthogonal basis"
 //               << std::endl;
 //       T = rigidTrans->correctParameters(T);
 //   }


    // Compute the transformation w.r.t GOLDEN
   // PM::DataPoints outputCloud =  rigidTrans->compute(ref,T);

    //outputCloud.save("Ref_transform.vtk");

        // Showing GREEN POINTSS !!
    //    const int pointCount(outputCloud.features.cols());
    //	const int dimCount(outputCloud.features.rows());
    //	const int descDimCount(outputCloud.descriptors.rows());
   
    	//for (int p = 1; p < pointCount; p = p + 2)
    	//{

        //    rgbframe.at<cv::Vec3b>( outputCloud.features(1,p)  ,  outputCloud.features(0,p) ) [0]  = 0;
        //    rgbframe.at<cv::Vec3b>( outputCloud.features(1,p)  ,  outputCloud.features(0,p) ) [1]  = 255;
        //    rgbframe.at<cv::Vec3b>( outputCloud.features(1,p)  ,  outputCloud.features(0,p) ) [2]  = 0;
    	//}

        //imshow("Tracked", rgbframe );


    //}
}
/**
  * Code example for ICP taking 2 points clouds (2D or 3D) relatively close
  * and computing the transformation between them.
  *
  * This code is more complete than icp_simple. It can load parameter files and
  * has more options.
  */
int main(int argc, const char *argv[])
{
	bool isTransfoSaved = false;
	string configFile;
	string outputBaseFile("test");
	string initTranslation("0,0,0");
	string initRotation("1,0,0;0,1,0;0,0,1");
	const int ret = validateArgs(argc, argv, isTransfoSaved, configFile,
								 outputBaseFile, initTranslation, initRotation);
	if (ret != 0)
	{
		return ret;
	}
	const char *refFile(argv[argc-2]);
	const char *dataFile(argv[argc-1]);

	// Load point clouds
	const DP ref(DP::load(refFile));
	const DP data(DP::load(dataFile));

	// Create the default ICP algorithm
	PM::ICP icp;

	if (configFile.empty())
	{
		// See the implementation of setDefault() to create a custom ICP algorithm
		icp.setDefault();
	}
	else
	{
		// load YAML config
		ifstream ifs(configFile.c_str());
		if (!ifs.good())
		{
			cerr << "Cannot open config file " << configFile << ", usage:"; usage(argv); exit(1);
		}
		icp.loadFromYaml(ifs);
	}

	int cloudDimension = ref.getEuclideanDim();
	
	if (!(cloudDimension == 2 || cloudDimension == 3)) 
	{
		cerr << "Invalid input point clouds dimension" << endl;
		exit(1);
	}

	PM::TransformationParameters translation =
			parseTranslation(initTranslation, cloudDimension);
	PM::TransformationParameters rotation =
			parseRotation(initRotation, cloudDimension);
	PM::TransformationParameters initTransfo = translation*rotation;

	PM::Transformation* rigidTrans;
	rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");

	if (!rigidTrans->checkParameters(initTransfo)) {
		cerr << endl
			 << "Initial transformation is not rigid, identiy will be used"
			 << endl;
		initTransfo = PM::TransformationParameters::Identity(
					cloudDimension+1,cloudDimension+1);
	}

	const DP initializedData = rigidTrans->compute(data, initTransfo);

	// Compute the transformation to express data in ref
	PM::TransformationParameters T = icp(initializedData, ref);
	float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
	cout << "match ratio: " <<  matchRatio << endl;

	

	// Transform data to express it in ref
	DP data_out(initializedData);
	icp.transformations.apply(data_out, T);

	cout << endl << "------------------" << endl;
	
	// START demo 1
	// Test for retrieving Haussdorff distance (with outliers). We generate new matching module 
	// specifically for this purpose. 
	//
	// INPUTS:
	// ref: point cloud used as reference
	// data_out: aligned point cloud (using the transformation outputted by icp)
	// icp: icp object used to aligned the point clouds

	
	// Structure to hold future match results
	PM::Matches matches;

	Parametrizable::Parameters params;
	params["knn"] =  toParam(1); // for Hausdorff distance, we only need the first closest point
	params["epsilon"] =  toParam(0);

	PM::Matcher* matcherHausdorff = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
	
	// max. distance from reading to reference
	matcherHausdorff->init(ref);
	matches = matcherHausdorff->findClosests(data_out);
	float maxDist1 = matches.getDistsQuantile(1.0);
	float maxDistRobust1 = matches.getDistsQuantile(0.85);

	// max. distance from reference to reading
	matcherHausdorff->init(data_out);
	matches = matcherHausdorff->findClosests(ref);
	float maxDist2 = matches.getDistsQuantile(1.0);
	float maxDistRobust2 = matches.getDistsQuantile(0.85);

	float haussdorffDist = std::max(maxDist1, maxDist2);
	float haussdorffQuantileDist = std::max(maxDistRobust1, maxDistRobust2);

	cout << "Haussdorff distance: " << std::sqrt(haussdorffDist) << " m" << endl;
	cout << "Haussdorff quantile distance: " << std::sqrt(haussdorffQuantileDist) <<  " m" << endl;

	// START demo 2
	// Test for retrieving paired point mean distance without outliers. We reuse the same module used for 
	// the icp object.
	//
	// INPUTS:
	// ref: point cloud used as reference
	// data_out: aligned point cloud (using the transformation outputted by icp)
	// icp: icp object used to aligned the point clouds
	
	// initiate the matching with unfiltered point cloud
	icp.matcher->init(ref);

	// extract closest points
	matches = icp.matcher->findClosests(data_out);

	// weight paired points
	const PM::OutlierWeights outlierWeights = icp.outlierFilters.compute(data_out, ref, matches);
	
	// generate tuples of matched points and remove pairs with zero weight
	const PM::ErrorMinimizer::ErrorElements matchedPoints( data_out, ref, outlierWeights, matches);

	// extract relevant information for convenience
	const int dim = matchedPoints.reading.getEuclideanDim();
	const int nbMatchedPoints = matchedPoints.reading.getNbPoints(); 
	const PM::Matrix matchedRead = matchedPoints.reading.features.topRows(dim);
	const PM::Matrix matchedRef = matchedPoints.reference.features.topRows(dim);
	
	// compute mean distance
	const PM::Matrix dist = (matchedRead - matchedRef).colwise().norm(); // replace that by squaredNorm() to save computation time
	const float meanDist = dist.sum()/nbMatchedPoints;
	cout << "Robust mean distance: " << meanDist << " m" << endl;

	// END demo

	cout << "------------------" << endl << endl;


	// Safe files to see the results
	ref.save(outputBaseFile + "_ref.vtk");
	data.save(outputBaseFile + "_data_in.vtk");
	data_out.save(outputBaseFile + "_data_out.vtk");
	if(isTransfoSaved) {
		ofstream transfoFile;
		string initFileName = outputBaseFile + "_init_transfo.txt";
		string icpFileName = outputBaseFile + "_icp_transfo.txt";
		string completeFileName = outputBaseFile + "_complete_transfo.txt";

		transfoFile.open(initFileName.c_str());
		if(transfoFile.is_open()) {
			transfoFile << initTransfo << endl;
			transfoFile.close();
		} else {
			cout << "Unable to write the initial transformation file\n" << endl;
		}

		transfoFile.open(icpFileName.c_str());
		if(transfoFile.is_open()) {
			transfoFile << T << endl;
			transfoFile.close();
		} else {
			cout << "Unable to write the ICP transformation file\n" << endl;
		}

		transfoFile.open(completeFileName.c_str());
		if(transfoFile.is_open()) {
			transfoFile << T*initTransfo << endl;
			transfoFile.close();
		} else {
			cout << "Unable to write the complete transformation file\n" << endl;
		}
	}
	else {
		cout << "ICP transformation:" << endl << T << endl;
	}

	return 0;
}