I adapted examples/icp_simple.cpp to the code below with the intent to show an example of how to use ErrorMinimizer::getResidualError.
I noticed that due to the use of the internal frame refMean here we are obliged to re-initialize the matcher using icp.matcher->init(ref); before computing the matches. Then I started to wonder if this re-initialization would somehow invalidate the ICP object. Is there any problem doing this? Or maybe is it possible to declare a matcher object outside of an ICP object?
using namespace std;
void validateArgs(int argc, char *argv, bool& isCSV);
* Code example for ICP taking 2 points clouds (2D or 3D) relatively close
* and computing the transformation between them.
int main(int argc, char *argv)
bool isCSV = true;
validateArgs(argc, argv, isCSV);
typedef PointMatcher<float> PM;
typedef PM::DataPoints DP;
// Load point clouds
const DP ref(DP::load(argv));
const DP data(DP::load(argv));
// Create the default ICP algorithm
// See the implementation of setDefault() to create a custom ICP algorithm
// Compute the transformation to express data in ref
PM::TransformationParameters T = icp(data, ref);
// Transform data to express it in ref
// To compute residual error:
// 1) Set reference cloud in the matcher
// 2) Get matches between transformed data and ref
PM::Matches matches = icp.matcher->findClosests(data_out);
// 3) Get outlier weights for the matches
PM::OutlierWeights outlierWeights = icp.outlierFilters.compute(data_out, ref, matches);
// 4) Compute error
float error = icp.errorMinimizer->getResidualError(data_out, ref, outlierWeights, matches);
// Safe files to see the results
cout << "Final transformation:" << endl << T << endl;
cout << "Final residual error: " << error << endl;
void validateArgs(int argc, char *argv, bool& isCSV )
if (argc != 3)
cerr << "Wrong number of arguments, usage " << argv << " reference.csv reading.csv" << endl;
cerr << "Will create 3 vtk files for inspection: ./test_ref.vtk, ./test_data_in.vtk and ./test_data_out.vtk" << endl;
cerr << endl << "2D Example:" << endl;
cerr << " " << argv << " ../../examples/data/2D_twoBoxes.csv ../../examples/data/2D_oneBox.csv" << endl;
cerr << endl << "3D Example:" << endl;
cerr << " " << argv << " ../../examples/data/car_cloud400.csv ../../examples/data/car_cloud401.csv" << endl;
The example you provide should work fine.
As for an external kd-tree, you have two options,
const int knn = 1;
params["knn"] = toParam(knn);
// other parameters possible
matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
PM::Matches matches = matcher->findClosests(data_out);
typedef typename Nabo::NearestNeighbourSearch<float> NNS;
const int dim = 3;
kdtree = NNS::create(ref.features, dim, NNS::KDTREE_LINEAR_HEAP);
const int knn = 1;
PM::Matches::Dists dists(knn, data_out.getNbPoints());
PM::Matches::Ids ids(knn, data_out.getNbPoints());
kdtree->knn(data_out.features.topRows(3), ids, dists, knn);
@pomerlef Thanks for the reply.
So, correct me if I'm wrong: I imagine that a similar example using ICPSequence would invalidate the object, since the matcher is set only by setMap, and re-initializing it to the reference cloud would change the internal reference, and thus disturb the following icp calls. In this case, the user would need to have a separated matcher like you showed, or call setMap with the reference again after computing the error the way I showed in the example, right?
You're right, using PM::ICPSequence icp; instead of PM::ICP icp; would mess up the internal representation.
All right. :) Thanks for your time. I'm closing this issue.