AbsoluteOrientationRANSAC.hh

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

Copyright (C) 2003-2009    (see file CONTACT 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 _AbsoluteOrientationRANSAC_hh_
#define _AbsoluteOrientationRANSAC_hh_

#include <bias_config.h>
#include "AbsoluteOrientation.hh"
#include <Base/Math/Random.hh>
#include <Base/Math/Vector3.hh>
#include <Geometry/RMatrix.hh>
#include <vector>

namespace BIAS {

/** @class AbsoluteOrientationRANSAC
    @brief Robust computation of similarity transformation between two
           3D point sets using a RANSAC approach.
    @see #AbsoluteOrientation
    @author esquivel
    @date 12/2008
*/
  class BIASGeometry_EXPORT AbsoluteOrientationRANSAC
  {

  public:

    AbsoluteOrientationRANSAC();

    ~AbsoluteOrientationRANSAC();

    /** @brief Compute max. error for statistical outlier detection from
        given significance level.
        @note Significance defines the probability that an inlier is regarded
        as an outlier ("type I error"). This should typically be a very small
        value (below 0.05 resp. 5%). The max. error is computes by integrating
        the chi square distribution.
     */
    void SetSignificance(double significance);

    /** @brief Set max. error for heuristical outlier detection.
        @note Note that max. squared (and weighted) error is set here! */
    inline void SetMaxError(double error) { maxError_ = error; };

    /** @brief Set max. error angle for heuristical outlier detection for
               pure absolute rotation estimation.
        @param Max. angle between input ray and estimated ray in radians.
        @note Use only previous to ComputeRotation() without weights! */
    inline void SetMaxErrorAngle(double angle)
    { maxError_ = 2.0*sin(0.5*angle); maxError_ *= maxError_; };

    /** @brief Set min. inlier ratio for termination. */
    inline void SetMinRatio(double ratio) { minRatio_ = ratio; };

    /** @brief Set number of random samples to draw from data. */
    inline void SetNumSamples(unsigned int n) { numSamples_ = n; };

    /** @brief Use greedy search for good solution? */
    inline void SetGreedy(bool greedy) { greedy_ = greedy; };

    /** @brief Refine final solution with all inliers? */
    inline void SetRefine(bool refine) { refine_ = refine; };

    /** @brief Use residual errors instead of inlier count as target function? */
    inline void SetWeightedTargetFunction(bool use)
    { useWeightedTargetFunction_ = use; };

    /** @brief Set algorithm used for rotation estimation. */
    inline void SetRotationAlgorithm(AbsoluteOrientation::RotationEstimationMethod algorithm)
    { solver_.SetRotationAlgorithm(algorithm); };

    /** @brief Computes rotation dR, translation dt and isometric scale ds
        between coordinate systems from corresponding 3D point sets X and Y
        using heuristical outlier detection.
        Returns inlier flags and error residuals for each correspondence.
        @return -1 if computation failed, else 0.
     */
    inline int Compute(const std::vector< Vector3<double> > &X,
                       const std::vector< Vector3<double> > &Y,
                       RMatrix &dR, Vector3<double> &dt, double &ds,
                       std::vector<bool> &inliers,
                       std::vector<double> &errors, double &errorSum,
                       const std::vector<double> &w = std::vector<double>(0))
    {
      const std::vector< Matrix3x3<double> > dummyCov(0);
      return Compute_(X, Y, dummyCov, dummyCov, dR, dt, ds, false,
                      inliers, errors, errorSum, w);
    };

    /** @brief Computes rotation dR, translation dt and isometric scale ds
        between coordinate systems from corresponding 3D point sets X and Y
        with covariances CovX, CovY using statistical outlier detection.
        Returns inlier flags and error residuals for each correspondence.
        @return -1 if computation failed, else 0.
     */
    inline int Compute(const std::vector< Vector3<double> > &X,
                       const std::vector< Vector3<double> > &Y,
                       const std::vector< Matrix3x3<double> > &CovX,
                       const std::vector< Matrix3x3<double> > &CovY,
                       RMatrix &dR, Vector3<double> &dt, double &ds,
                       std::vector<bool> &inliers,
                       std::vector<double> &errors, double &errorSum)
    {
      return Compute_(X, Y, CovX, CovY, dR, dt, ds, false,
                      inliers, errors, errorSum);
    };

    /** @brief Computes rotation dR and translation dt between coordinate
        systems from corresponding 3D point sets X and Y using heuristical
        outlier detection.
        Returns inlier flags and residual errors for each correspondence.
        @return -1 if computation failed, else 0.
     */
    inline int Compute(const std::vector< Vector3<double> > &X,
                       const std::vector< Vector3<double> > &Y,
                       RMatrix &dR, Vector3<double> &dt,
                       std::vector<bool> &inliers,
                       std::vector<double> &errors, double &errorSum,
                       const std::vector<double> &w = std::vector<double>(0))
    {
      double ds = 1.0;
      const std::vector< Matrix3x3<double> > dummyCov(0);
      return Compute_(X, Y, dummyCov, dummyCov, dR, dt, ds, true,
                      inliers, errors, errorSum, w);
    };

    /** @brief Computes rotation dR and translation dt between coordinate
        systems from corresponding 3D point sets X and Y with covariances
        CovX, CovY using statistical outlier detection.
        Returns inlier flags and residual errors for each correspondence.
        @return -1 if computation failed, else 0.
     */
    inline int Compute(const std::vector< Vector3<double> > &X,
                       const std::vector< Vector3<double> > &Y,
                       const std::vector< Matrix3x3<double> > &CovX,
                       const std::vector< Matrix3x3<double> > &CovY,
                       RMatrix &dR, Vector3<double> &dt,
                       std::vector<bool> &inliers,
                       std::vector<double> &errors, double &errorSum)
    {
      double ds = 1.0;
      return Compute_(X, Y, CovX, CovY, dR, dt, ds, true,
                      inliers, errors, errorSum);
    };

    /** @brief Computes rotation dR between corresponding 3D ray sets X and Y
        using heuristical outlier detection.
        Returns inlier flags and error residuals for each correspondence.
        @return -1 if computation failed, else 0.
     */
    inline int ComputeRotation(const std::vector< Vector3<double> > &X,
                               const std::vector< Vector3<double> > &Y,
                               RMatrix &dR, std::vector<bool> &inliers,
                               std::vector<double> &errors, double &errorSum,
                               const std::vector<double> &w = std::vector<double>(0))
    {
      const std::vector< Matrix3x3<double> > dummyCov(0);
      return ComputeRotation_(X, Y, dummyCov, dummyCov, dR,
                              inliers, errors, errorSum, w);
    };

    /** @brief Computes rotation dR between corresponding 3D ray sets X and Y
        with covariances CovX, CovY using statistical outlier detection.
        Returns inlier flags and error residuals for each correspondence.
        @return -1 if computation failed, else 0.
     */
    inline int ComputeRotation(const std::vector< Vector3<double> > &X,
                               const std::vector< Vector3<double> > &Y,
                               const std::vector< Matrix3x3<double> > &CovX,
                               const std::vector< Matrix3x3<double> > &CovY,
                               RMatrix &dR, std::vector<bool> &inliers,
                               std::vector<double> &errors, double &errorSum,
                               const std::vector<double> &w = std::vector<double>(0))
    {
      return ComputeRotation_(X, Y, CovX, CovY, dR, inliers, errors, errorSum, w);
    };

    /** @brief Computes residual errors for inliers of 3D point sets X and Y with
        respect to given rotation dR, translation dt and isometric scale ds.
        @note Each error is given by (weighted) Euclidean distance dist(X',Y)
              with X' = ds*dR*X - dt for corresponding 3D points X and Y.
        @return Sum of squared errors dist(X',Y) with X' = ds*dR*X - dt
                as well as residual error for each single correspondence.
     */
    double GetResidualErrors(std::vector<double> &errors,
                             const std::vector< Vector3<double> > &X,
                             const std::vector< Vector3<double> > &Y,
                             const RMatrix &dR, const Vector3<double> &dt,
                             const double &ds = 1.0,
                             const std::vector<bool> &inliers
                             = std::vector<bool>(0),
                             const std::vector<double> &w
                             = std::vector<double>(0));

    /** @brief Computes residual errors for inliers of 3D point sets X and Y
        with covariances CovX, CovY with respect to given rotation dR,
        translation dt and isometric scale ds.
        @note Computes Mahalanobis distances instead of Euclidean distances!
        @return Sum of squared errors mahalanobis(X',Y) with X' = ds*dR*X - dt
                as well as residual error for each single correspondence.
     */
    double GetResidualErrors(std::vector<double> &errors,
                             const std::vector< Vector3<double> > &X,
                             const std::vector< Vector3<double> > &Y,
                             const std::vector< Matrix3x3<double> > &CovX,
                             const std::vector< Matrix3x3<double> > &CovY,
                             const RMatrix &dR, const Vector3<double> &dt,
                             const double &ds = 1.0,
                             const std::vector<bool> &inliers
                             = std::vector<bool>(0));

    /** @brief Computes residual errors for inliers of 3D point sets X and Y
        with respect to given rotation dR.
        @note Each error is given by (weighted) Euclidean distance dist(X',Y)
              with X' = dR*X for corresponding 3D points X and Y.
        @return Sum of squared errors dist(X',Y) with X' = dR*X
                as well as residual error for each single correspondence.
     */
    double GetResidualErrors(std::vector<double> &errors,
                             const std::vector< Vector3<double> > &X,
                             const std::vector< Vector3<double> > &Y,
                             const RMatrix &dR,
                             const std::vector<bool> &inliers
                             = std::vector<bool>(0),
                             const std::vector<double> &w
                             = std::vector<double>(0));

    /** @brief Computes residual errors for inliers of 3D point sets X and Y
        with covariances CovX, CovY with respect to given rotation dR.
        @note Computes Mahalanobis distances instead of Euclidean distances!
        @return Sum of squared errors mahalanobis(X',Y) with X' = dR*X
                as well as residual error for each single correspondence.
     */
    double GetResidualErrors(std::vector<double> &errors,
                             const std::vector< Vector3<double> > &X,
                             const std::vector< Vector3<double> > &Y,
                             const std::vector< Matrix3x3<double> > &CovX,
                             const std::vector< Matrix3x3<double> > &CovY,
                             const RMatrix &dR,
                             const std::vector<bool> &inliers
                             = std::vector<bool>(0));

  private:

    /// @brief Estimator and refiner for absolute orientation problem
    AbsoluteOrientation solver_;

    /// @brief Random sample generator
    Random randomizer_;

    /// @brief RANSAC parameters
    const unsigned int sampleSize_;
    unsigned int numSamples_;
    double maxError_;
    double minRatio_;
    bool refine_;
    bool greedy_;
    
    /// @brief Use residual error sum instead of inlier count as target function?
    bool useWeightedTargetFunction_;

    /** @brief Computes rotation dR, translation dt and isometric scale ds
        between coordinate systems from corresponding 3D point sets X and Y
        using heuristical outlier detection or statistical outlier detection
        if covariances CovX, CovY are given.
        @note Given scale is optionally enforced instead of being computed.
        @note Error term for each residual is weighted by w optionally for
              heuristical outlier detection.
        @return -1 if computation failed, else 0.
     */
    int Compute_(const std::vector< Vector3<double> > &X,
                 const std::vector< Vector3<double> > &Y,
                 const std::vector< Matrix3x3<double> > &CovX,
                 const std::vector< Matrix3x3<double> > &CovY,
                 RMatrix &dR, Vector3<double> &dt,
                 double &ds, bool enforceScale, std::vector<bool> &inliers,
                 std::vector<double> &errors, double &errorSum,
                 const std::vector<double> &w = std::vector<double>(0));

    /** @brief Computes rotation dR between corresponding 3D ray sets X and Y
        using heuristical outlier detection or statistical outlier detection
        if covariances CovX, CovY are given.
        Returns inlier flags and error residuals for each correspondence.
        @return -1 if computation failed, else 0.
     */
    int ComputeRotation_(const std::vector< Vector3<double> > &X,
                         const std::vector< Vector3<double> > &Y,
                         const std::vector< Matrix3x3<double> > &CovX,
                         const std::vector< Matrix3x3<double> > &CovY,
                         RMatrix &dR, std::vector<bool> &inliers,
                         std::vector<double> &errors, double &errorSum,
                         const std::vector<double> &w = std::vector<double>(0));

    /** @brief Draw random sample of given size from 3D point sets X, Y
        with optional weights w and covariances CovX, CovY.
        @return -1 if computation failed, else size of sample.
     */
    int CreateRandomSample_(unsigned int samplesize,
                            const std::vector< Vector3<double> > &X,
                            const std::vector< Vector3<double> > &Y,
                            const std::vector< Matrix3x3<double> > &CovX,
                            const std::vector< Matrix3x3<double> > &CovY,
                            const std::vector<double> &w,
                            std::vector<int> &samples,
                            std::vector< Vector3<double> > &sampleX,
                            std::vector< Vector3<double> > &sampleY,
                            std::vector< Matrix3x3<double> > &sampleCovX,
                            std::vector< Matrix3x3<double> > &sampleCovY,
                            std::vector<double> &samplew);

    /** @brief Determine inliers for 3D point sets X and Y with respect to
        given rotation dR, translation dt and scale ds heuristically or
        statistically if covariances CovX, CovY are given.
        @note The incidence test for statistical outlier detection is based
        on the chi square test which relates the H_0 hypothesis (identical)
        against the H_1 hypothesis (different) by fixing the significance
        level of the test, i.e. the probability that H_0 is rejected even
        though it is true ("type I error").
        @return Number of inliers found.
    */
    int EvaluateInliers_(std::vector<bool> &inliers,
                         std::vector<double> &errors, double &errorSum,
                         const std::vector< Vector3<double> > &X,
                         const std::vector< Vector3<double> > &Y,
                         const std::vector< Matrix3x3<double> > &CovX,
                         const std::vector< Matrix3x3<double> > &CovY,
                         const RMatrix &dR, const Vector3<double> &dt,
                         const double &ds, const std::vector<double> &w);

    /** @brief Determine inliers for 3D point sets X and Y with respect to
        given rotation dR heuristically or statistically if covariances CovX,
        CovY are given.
        @return Number of inliers found.
    */
    int EvaluateInliers_(std::vector<bool> &inliers,
                         std::vector<double> &errors, double &errorSum,
                         const std::vector< Vector3<double> > &X,
                         const std::vector< Vector3<double> > &Y,
                         const std::vector< Matrix3x3<double> > &CovX,
                         const std::vector< Matrix3x3<double> > &CovY,
                         const RMatrix &dR, const std::vector<double> &w);

  };

}
#endif // _AbsoluteOrientationRANSAC_hh_