getResidualError example

[Ellon]

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
    icp.setDefault();

    // 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
    icp.matcher->init(ref);
    // 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
    ref.save("test_ref.vtk");
    data.save("test_data_in.vtk");
    data_out.save("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;
        exit(1);
    }
}

[pomerlef]

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);
matcher->init(ref);
PM::Matches matches = matcher->findClosests(data_out);

2) 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]

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

[pomerlef]

You're right, using PM::ICPSequence icp; instead of PM::ICP icp; would mess up the internal representation.

[Ellon]

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

[jstiefel]

I tried to follow this example after executing ICP between two point clouds:

void StaticRoomDeviations::getResidualError(const DP &dpref, const DP &dppointcloud_out) {
  icp_.matcher->init(dpref);
  PM::Matches matches = icp_.matcher->findClosests(dppointcloud_out);
  PM::OutlierWeights outlierWeights = icp_.outlierFilters.compute(dppointcloud_out, dpref, matches);
  float error = icp_.errorMinimizer->getResidualError(dppointcloud_out, dpref, outlierWeights, matches);
  std::cout << "Final residual error: " << error << std::endl;
}

However, I'm getting the error Field normals not found when executing the getResidualError() function. I then tried to add the surface normal to my ICP configuration YAML, but still get the same error:

readingDataPointsFilters:
  - SurfaceNormalDataPointsFilter
  - ObservationDirectionDataPointsFilter
  - OrientNormalsDataPointsFilter
  - RandomSamplingDataPointsFilter:
      prob: 0.5

referenceDataPointsFilters:
  - SurfaceNormalDataPointsFilter
  - ObservationDirectionDataPointsFilter
  - OrientNormalsDataPointsFilter
  - SamplingSurfaceNormalDataPointsFilter:
      knn: 10

Are these normal descriptors not saved to the DP? And why were surface normals not necessary before?

[PhiBabin]

Can you provide the code that load your YAML configuration and call getResidualError? Because otherwise it will be hard to find why the reference point cloud does not have normals. Some step in ICP requires normal for each point, for instance PointToPlaneErrorMinimizer need normal for the reference point cloud. If you use the default icp configuration PointToPointErrorMinimizer will be used, this minimizer does not require any normals.

[jstiefel]

I'm using PointToPlaneErrorMinimizer in ICP and this works fine. I also tried it with icp_.setDefault() as in the example. But surface normals are still missing in my case, but just for getResidualError(). I don't see any difference between my code and the example.

However, I have a working solution by using DataPointsFilters and just adding the normals. I conclude from this that the DataPointsFilters in the ICP configuration are only used for ICP and not saved as descriptors to DP. Still wondering why the example above worked without normals then.

This is what I'm doing without DataPointsFilters:

  // convert point clouds to DP
  const DP dppointcloud = pointCloudToDP(pointcloud);
  const DP dpref = pointCloudToDP(ref_pc);
  // alternatively, but also without success when normals are needed:
  //const DP dpref(DP::load("P.pcd"));
  //const DP dppointcloud(DP::load("P_deviated_transformed.pcd"));

  loadICPConfig(); // alternatively: icp_.setDefault();

  PM::TransformationParameters T = icp_(dppointcloud, dpref);
  DP dppointcloud_out(dppointcloud);
  icp_.transformations.apply(dppointcloud_out, T);

  getResidualError(dpref, dppointcloud_out);

void loadICPConfig() {
  // Load the ICP configuration
  std::ifstream ifs_icp_config(nh_private_.param<std::string>("icp_configuration_file", "fail").c_str());
  if (ifs_icp_config.good()) {
    icp_.loadFromYaml(ifs_icp_config);
  }
  else {
    icp_.setDefault();
  }
}

void getResidualError(const DP &dpref, const DP &dppointcloud_out) {
  icp_.matcher->init(dpref);
  PM::Matches matches = icp_.matcher->findClosests(dppointcloud_out);
  PM::OutlierWeights outlierWeights = icp_.outlierFilters.compute(dppointcloud_out, dpref, matches);
  float error = icp_.errorMinimizer->getResidualError(dppointcloud_out, dpref, outlierWeights, matches);
  std::cout << "Final residual error: " << error << std::endl;
}

My icp_configuration.yaml looks exactly like https://github.com/ethz-asl/libpointmatcher/blob/master/examples/data/default.yaml except for:

readingDataPointsFilters:
  - SurfaceNormalDataPointsFilter
  - ObservationDirectionDataPointsFilter
  - OrientNormalsDataPointsFilter
  - RandomSamplingDataPointsFilter:
      prob: 0.5

referenceDataPointsFilters:
  - SurfaceNormalDataPointsFilter
  - ObservationDirectionDataPointsFilter
  - OrientNormalsDataPointsFilter
  - SamplingSurfaceNormalDataPointsFilter:
      knn: 10

Thanks.

[PhiBabin]

Yes, the DP pass to ICP are copy before passing through the datapoints filters. If you are planning on reusing the reference point cloud, I suggest ICPSequence which kept a unfiltered and filtered copy of the map inside the icp instance. You would not need to reinitialize the matcher in that case. I don't know how the example provided in this issue used to work. Maybe the pointcloud used already had normal's descriptor before icp?

[tfwittwer]

I've run into the same issue with the pmicp program. Is there a simple solution to this either via the configuration or via modifying the pmicp code?

[danielaDESM]

Hi, regarding the function getResidualError(), I have 2 questions:

1. Here it has been mentioned how to create a separate matcher object and how to re-initialize the matcher in order to get the residual error. I was wondering if the following is also valid or if it was intentionally not mentioned for some reason.

   // Error minimizer
   PM::ErrorMinimizer::ErrorElements last_error_elements= icp.errorMinimizer.get()->getErrorElements();
   const PM::Matches matches = last_error_elements.matches;
   float max_dist = matches.dists.maxCoeff();
   std::cout << "Max dist: " << max_dist << std::endl;
   const PM::OutlierWeights outlierWeights = last_error_elements.weights;
   DP ref_aux = last_error_elements.reference;
   DP mov_aux = last_error_elements.reading;
   float error = icp.errorMinimizer->getResidualError(mov_aux, ref_aux, outlierWeights, matches);
   std::cout << "Final residual error: " << error << std::endl;

2. I get a pretty big number. I have saved and seen the point clouds that were used for the function and I also got the max distance saved in matches.dists.maxCoeff(), which gives me an expected small value. So, do you have any hint of why this could be happening?

[pomerlef]

You can have a look at #443 for a more detailed discussion. If I recall well, getResidualError will give you the sum of squared error.

[nhk035]

I got a fitnessScore like pcl library by follow method, and the value seems reasonable. Post here for discussion.

1. use SurfaceNormalDataPointsFilter in DataPointsFilters, and this will generate normal for points, remember to set keepNormals 1
2. use ICPSequence as PhiBabin suggested.

    DP data_out(icp.getReadingFiltered()); 
    icp.transformations.apply(data_out, T);  
    PM::Parameters params;
    params["knn"] = PointMatcherSupport::toParam(1);
    params["epsilon"] = PointMatcherSupport::toParam(0);
    params["maxDist"] = PointMatcherSupport::toParam(5);
    std::shared_ptr<PM::Matcher> matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
    DP data_target_filtered(icp.getPrefilteredMap());
    matcher->init(data_target_filtered);
    // 2) Get matches between transformed data and ref
    PM::Matches matches = matcher->findClosests(data_out);
    // 3) Get outlier weights for the matches
    PM::OutlierWeights outlierWeights = pm_icp_.outlierFilters.compute(data_out, data_target_filtered, matches);
    float error = pm_icp_.errorMinimizer->getResidualError(data_out, data_target_filtered, outlierWeights, matches);

3. Because i used a binary weight, so i compute the number of valid poins as follow. And then i can get a score like pcl.

    float valid_point_num = 0;
    for (size_t j = 0; j < outlierWeights.cols(); ++j) {
        valid_point_num += outlierWeights(0, j);
    }
    float score = error / valid_point_num;