getResidualError example #193

Ellon opened this Issue Jan 29, 2017 · 4 comments


None yet

2 participants

Ellon commented Jan 29, 2017 edited

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?

#include "pointmatcher/PointMatcher.h"
#include <cassert>
#include <iostream>
#include "boost/filesystem.hpp"

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[1]));
	const DP data(DP::load(argv[2]));

	// Create the default ICP algorithm
	PM::ICP icp;
	// 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
	DP data_out(data);
	icp.transformations.apply(data_out, T);

	// 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"test_ref.vtk");"test_data_in.vtk");"test_data_out.vtk");
	cout << "Final transformation:" << endl << T << endl;
	cout << "Final residual error: " << error << endl;

	return 0;

void validateArgs(int argc, char *argv[], bool& isCSV )
	if (argc != 3)
		cerr << "Wrong number of arguments, usage " << argv[0] << " 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[0] << " ../../examples/data/2D_twoBoxes.csv ../../examples/data/2D_oneBox.csv" << endl;
		cerr << endl << "3D Example:" << endl;
		cerr << "  " << argv[0] << " ../../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,

  1. generate a separate matcher object the same way you would generate an separate DataFilter:
PM::Matcher* matcher;

const int knn = 1;
PM::Parameters params;
params["knn"] = toParam(knn);
// other parameters possible

matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
PM::Matches matches = matcher->findClosests(data_out);
  1. directly use libnabo:
typedef typename Nabo::NearestNeighbourSearch<float> NNS;
NNS* kdtree;

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);
Ellon commented Jan 30, 2017

@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.

Ellon commented Jan 30, 2017

All right. :) Thanks for your time. I'm closing this issue.

@Ellon Ellon closed this Jan 30, 2017
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment