/**
* 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;
}
void Assembler::publishCloud()
{
if(cloud.features.cols() != 0)
{
//mutexCloud.lock();
PM::TransformationParameters TOdomToScanner =
PointMatcher_ros::transformListenerToEigenMatrix<float>(
tfListener,
sensor_frame,
odom_frame,
scanTime
);
// Bring the scan back to local frame (i.e., centered to the scanner)
cloud = transformation->compute(cloud, TOdomToScanner);
if(firstPointCloud)
{
firstPointCloud = false;
ROS_INFO_STREAM("skipping first point cloud");
}
else
{
if(cloudPub.getNumSubscribers()) {
cloudPub.publish(
PointMatcher_ros::pointMatcherCloudToRosMsg<float>(
cloud, sensor_frame, scanTime));
}
}
cloud = DP();
//mutexCloud.unlock();
}
}
bool CloudMatcher::match(pointmatcher_ros::MatchClouds::Request& req, pointmatcher_ros::MatchClouds::Response& res)
{
// get and check reference
const DP referenceCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.reference));
const unsigned referenceGoodCount(referenceCloud.features.cols());
const unsigned referencePointCount(req.reference.width * req.reference.height);
const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
if (referenceGoodCount == 0)
{
ROS_ERROR("I found no good points in the reference cloud");
return false;
}
if (referenceGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 <<
"% of the cloud (received " << referenceGoodCount << ")");
}
// get and check reading
const DP readingCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.readings));
const unsigned readingGoodCount(referenceCloud.features.cols());
const unsigned readingPointCount(req.readings.width * req.readings.height);
const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
if (readingGoodCount == 0)
{
ROS_ERROR("I found no good points in the reading cloud");
return false;
}
if (readingGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
}
// check dimensions
if (referenceCloud.features.rows() != readingCloud.features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: reference cloud is " <<
referenceCloud.features.rows() - 1 << " while reading cloud is " <<
readingCloud.features.rows() - 1);
return false;
}
// call ICP
PM::TransformationParameters transform;
try
{
transform = icp(readingCloud, referenceCloud);
tf::transformTFToMsg(PointMatcher_ros::eigenMatrixToTransform<float>(transform), res.transform);
ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return false;
}
if(traceMatching)
{
std::stringstream ss;
ss << "reading_" << matchedCloudsCount << ".vtk";
readingCloud.save(ss.str());
ss.str(std::string());
ss << "reference_" << matchedCloudsCount << ".vtk";
referenceCloud.save(ss.str());
PM::Transformation* rigidTrans;
rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
PM::DataPoints matchedCloud = rigidTrans->compute(readingCloud, transform);
ss.str(std::string());
ss << "matched_" << matchedCloudsCount << ".vtk";
matchedCloud.save(ss.str());
}
matchedCloudsCount++;
return true;
}
/**
* 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;
}
