string Mapper::serializeTransParamCSV(const PM::TransformationParameters T, const bool getHeader, const string prefix)
{

  std::stringstream stream;
  for(int col=0; col < T.cols(); col++)
  {
    for(int row=0; row < T.rows(); row++)
    {
      if(getHeader)
      {
        stream << prefix << row << col;
      }
      else
      {
        stream << T(row,col);
      }

      if((col != (T.cols()-1)) || (row != (T.rows()-1)))
      {
        stream << ", ";
      }
    } 
  }

  return stream.str();
}
예제 #2
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;
	}
}