/**
* 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;
}
/**
* 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;
}
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);
}
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);
}
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;
}
}
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;
}