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();
}
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;
}
}
