icp.h

Go to the documentation of this file.

/*
Copyright 2011. All rights reserved.
Institute of Measurement and Control Systems
Karlsruhe Institute of Technology, Germany

Authors: Andreas Geiger

libicp is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or any later version.

libicp is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#ifndef ICP_H
#define ICP_H

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <vector>

#include "matrix.h"
#include "kdtree.h"

class Icp {

 public:
  // constructor
  // input: M ....... pointer to first model point
  //        M_num ... number of model points
  //        dim   ... dimensionality of model points (2 or 3)
  Icp(double *M, const int32_t M_num, const int32_t dim);

  // deconstructor
  virtual ~Icp();

  // if correspondences are clear (or given by default), the iterative process can worsen result. disable iterative portion of icp.
  void forceInstantResult(bool val) { instantForce = val; }

  // return matrices even if no convergence achieved
  void giveOutputIfNotConverged(bool val) { giveOutputIfNotConvergedB = val; }

  // set subsampling step size of coarse matching (1. stage)
  void setSubsamplingStep(int32_t val) { sub_step = val; }

  // set maximum number of iterations (1. stopping criterion)
  void setMaxIterations(int32_t val) { max_iter = val; }

  // set minimum delta of rot/trans parameters (2. stopping criterion)
  void setMinDeltaParam(double val) { min_delta = val; }

  void setEventTimeCheck(bool val) { checkEventTime = val; }

  // fit template to model yielding R,t (M = R*T + t)
  // input:  T ....... pointer to first template point
  //         T_num ... number of template points
  //         R ....... initial rotation matrix
  //         t ....... initial translation vector
  //         indist .. inlier distance (if <=0: use all points)
  // output: R ....... final rotation matrix
  //         t ....... final translation vector
  double fit(double *T, const int32_t T_num, Matrix &R, Matrix &t, const double indist); // returns delta between the last two transfomartion matrices before termination

  // Euclidean fitness score. The lower the better, but if it returns 0, something wrong has happened.
  double getFitnessScore();
  double computeFitnessRMSE(const double *T, const Matrix &R, const Matrix &t);
  double computeFitnessR2(const double *T, const Matrix &R, const Matrix &t);
  bool hasConverged() { return hasConvergedBool; }
  int32_t getInterations() { return iterations; }

 private:
  // iterative fitting
  bool fitIterate(double *T, const int32_t T_num, Matrix &R, Matrix &t, const std::vector<int32_t> &active);

  virtual double fitInstant(double *T, const int32_t T_num, Matrix &R, Matrix &t, const std::vector<int32_t> &active) = 0;

  // inherited classes need to overwrite these functions
  virtual double fitStep(double *T, const int32_t T_num, Matrix &R, Matrix &t, const std::vector<int32_t> &active) = 0;
  virtual std::vector<int32_t> getInliers(double *T, const int32_t T_num, const Matrix &R, const Matrix &t, const double indist) = 0;

 protected:
  // kd tree of model points
  kdtree::KDTree *M_tree;
  kdtree::KDTreeArray M_data;
  double *M_;

  int32_t dim;      // dimensionality of model + template data (2 or 3)
  int32_t sub_step; // subsampling step size
  int32_t max_iter; // max number of iterations
  int32_t T_num;
  int32_t iterations;
  double min_delta; // min parameter delta
  double current_delta;
  double euclidean_fitness; // fitness score for error estimation
  bool hasConvergedBool;
  bool checkEventTime;
  bool giveOutputIfNotConvergedB;
  bool instantForce;
};

#endif // ICP_H