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