ExampleRANSACPlane.hh

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003, 2004    (see file CONTACTS for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef __ExampleRANSAC_Plane_hh__
#define __ExampleRANSAC_Plane_hh__

#include <Base/Common/BIASpragmaStart.hh>


#include "MathAlgo/RANSAC.hh"
#include "MathAlgo/SVD.hh"
#include "Base/Geometry/HomgPoint3D.hh"
#include "Base/Geometry/HomgPlane3D.hh"

#include <string>

namespace BIAS {

  /** @class PlaneRANSAC
      @brief Robust fitting of 3D plane to 3D point set using RANSAC.

      This class implements an example for RANSAC class overloading.

      Method PlaneRANSAC::GetSampleSolutions picks three 3D points and
      computes a 3D plane from them.

      Method PlaneRANSAC::EvaluateSolution evaluates how many points are
      considered as "inliers" with respect to the computed plane, i.e.
      their distance to the plane does not exceed a certain threshold.
      
      In PlaneRANSAC::RefineSolution, a linear least squares computation
      of the plane is done using all inliers to refine the plane parameters.
      
      You have to set the inlier threshold (max. distance to plane to accept
      a point as being on the plane) and the fraction of valid 3D points
      which defines the number of samples performed by RANSAC.
      
      When you call PlaneRANSAC::SolveMaster then, you will get the best
      plane according to your input.

      @author MIP
  */
  class PlaneRANSAC : public RANSAC<HomgPlane3D>
  {
    
  public:

    PlaneRANSAC();

    ~PlaneRANSAC();

    /**
     * @brief Initializes RANSAC. After calling the base RANSAC::Init method, the
     *        other parameters are set.
     * @attention RANSAC::Init resets a lot of parameters to default values,
     *            for example explicitSampleCount. So setting these values before
     *            calling Init will have no effect.
     */
    void Init(std::vector<HomgPoint3D>& data,
              double inlier_distance_threshold,
              bool refine_solution,
              double expected_inlier_fraction,
              double solution_error_threshold);

    /** takes sample and computes solution for this sample. This is usually done
     *  in an easy way, e.g. linearly. Sometimes solutions based on a minimum
     *  sample size are not unambiguous as for example in case of the 7 point
     *  algorithm for essential matrix estimation. In those cases all possible
     *  solutions have to be returned in the solutions vector.
     */
    virtual int GetSampleSolutions(std::vector<unsigned int> &which_samples,
                                   std::vector<HomgPlane3D> &solutions);

    /**
     * The solution denoted by solution needs to be ranked. This is done based
     * on the number of inliers found for this solution and based on the accuracy
     * reached on these inliers. So some kind of measurement is needed. Here it
     * is the absolute distance between the homogeneous 3D points and the plane. If
     * such a distance is below the threshold, and inlier has been found. The
     * average distance over all inliers is the evaluate_score. While the number
     * of inliers is entered in accept_count. For more details refer to
     * documentation of this method in RANSAC.hh.
     */
    virtual bool EvaluateSolution(const HomgPlane3D& solution,
                                  bool existing_solution_flag,
                                  std::vector<bool>& inlier,
                                  int& accept_count,
                                  double& evaluate_score,
                                  bool& ok_to_terminate_flag);

    /** RefineSolution is only called, if the RefineSolutionFlag is set to true.
     * In any case, it is only applied to solutions, that have been found to be good,
     * indicated by the return value of EvaluateSolution.
     *
     */
    virtual bool RefineSolution(std::vector<unsigned int>& which_samples,
                                HomgPlane3D& solution,
                                std::vector<bool>& new_inliers);
    
  protected:

    /// input 3D points to fit 3D plane to
    std::vector<HomgPoint3D> _vData;

    /// 3D points with max distance _dInlierDistanceThreshold to plane are
    /// considered as inliers
    double _dInlierDistanceThreshold;

    /// if a sample yields more than _dExpectedInlierFraction inliers and
    /// evaluate_score is lower than _dSolutionErrorThreshold, RANSAC will stop
    double _dExpectedInlierFraction;

    /// if a sample yields more than _dExpectedInlierFraction inliers and
    /// evaluate_score is lower than _dSolutionErrorThreshold, RANSAC will stop
    double _dSolutionErrorThreshold;
  };

  PlaneRANSAC::PlaneRANSAC() : RANSAC<HomgPlane3D>()
  {}

  PlaneRANSAC::~PlaneRANSAC()
  {}

  void PlaneRANSAC::Init(std::vector<HomgPoint3D>& data,
                         double inlier_distance_threshold,
                         bool refine_solution,
                         double expected_inlier_fraction,
                         double solution_error_threshold)
  {
    RANSAC<HomgPlane3D>::Init(3, 1, refine_solution);
    _vData = data;
    _uiDataSize = data.size();
    _dInlierDistanceThreshold = inlier_distance_threshold;
    _dExpectedInlierFraction = expected_inlier_fraction;
    _dSolutionErrorThreshold = solution_error_threshold;
  }

  int PlaneRANSAC::
  GetSampleSolutions(std::vector<unsigned int> &which_samples,
                     std::vector<HomgPlane3D> &solutions)
  {
    solutions.clear();

    // computing a plane out of three 3D points is really easy,
    // and there is only one solution in this case
    solutions.push_back(HomgPlane3D(_vData[which_samples[0]],
                                    _vData[which_samples[1]],
                                    _vData[which_samples[2]]));

    return solutions.size();
  }

  bool PlaneRANSAC::EvaluateSolution(const HomgPlane3D& solution,
                                     bool existing_solution_flag,
                                     std::vector<bool>& inlier,
                                     int& accept_count,
                                     double& evaluate_score,
                                     bool& ok_to_terminate_flag)
  {
    bool good_flag = true;
    inlier.clear();
    inlier.resize(_uiDataSize);

    int my_accept_count = 0;
    evaluate_score = 0.0;

    for (unsigned int i=0; i<_uiDataSize; i++){
      inlier[i] = (solution.DistanceSquared(_vData[i]) <
                   _dInlierDistanceThreshold);
      if (inlier[i]) {
        my_accept_count++;
        evaluate_score += solution.DistanceSquared(_vData[i]);
      }
    }

    // determine average score
    if (my_accept_count != 0) {
      evaluate_score /= my_accept_count;
    } else {
      evaluate_score = DBL_MAX;
      good_flag = false;
    }

    // set new accept count
    accept_count = my_accept_count;

    // ok_to_terminate_flag is important to set, if greedy evaluation is needed.
    // If set to true, the algorithm will stop after this sample
    ok_to_terminate_flag =
      (((accept_count/(double)_uiDataSize) > _dExpectedInlierFraction) &&
       (evaluate_score < _dSolutionErrorThreshold));

    // this return value decides if the solution will be considered further by refining it.
    return good_flag;
  }

  bool PlaneRANSAC::RefineSolution(std::vector<unsigned int>& which_samples,
                                   HomgPlane3D& solution,
                                   std::vector<bool>& new_inliers)
  {
    // create linear equation system A*x = 0 describing the relation between
    // plane parameters x and 3D points A_i
    const unsigned int nr_of_points = which_samples.size();
    BIAS::Matrix<double>  A(nr_of_points, 4);
    for (unsigned int i = 0; i < nr_of_points; i++) {
      A[i][0] = _vData[which_samples[i]][0];
      A[i][1] = _vData[which_samples[i]][1];
      A[i][2] = _vData[which_samples[i]][2];
      A[i][3] = _vData[which_samples[i]][3];
    }
    A = A.Transpose()*A;
    // get a solution (linear fit) of this equation system by its SVD
    SVD svd(A, 1e-5, false);

    // compute plane parameters x from last null vector
    if (svd.Rank() < 3) return false;
    Vector4<double> x = svd.GetNullvector();
    double norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
    if (norm > 1e-5) x /= norm;
    solution = HomgPlane3D(x);
    
    return true;
  }

}

#include <Base/Common/BIASpragmaEnd.hh>

#endif // __ExampleRANSAC_double_hh__