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