Esempio n. 1
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;
}
Esempio n. 2
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);

}
Esempio n. 3
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;
}