AbsoluteOrientation.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 _AbsoluteOrientation_hh_
#define _AbsoluteOrientation_hh_

#include <bias_config.h>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Geometry/RMatrix.hh>
#include <vector>

namespace BIAS {

/** @class AbsoluteOrientation
    @brief Computes similarity transformation between 3D point sets.

    Estimates the relationship between two coordinate systems using
    pairs of measurements of the coordinates of a number of 3D points
    in both systems. A SVD-based solution to the least-squares problem
    for at least three points is used based on an approach proposed in:

    [1] B. Horn: Closed-form Solution of Absolute Orientation Using
        Unit Quaternions, 1987.

    Another SVD-based solution estimating a rotation matrix directly
    instead of using unit quaternions is given by Kabsch's algorithm:

    [2] W. Kabsch: A Solution for the Best Rotation to Relate Two Sets
        of Vectors, 1976.

    @note The transformation between the coordinate systems is given
    as rotation dR, translation dt, and isometric scale ds, such that
    points X in the first coordinate system are related to points Y
    in the second coordinate system by Y = ds*dR*X + dt.

    @todo Implement Horn's closed-form solution and compare with our
          SVD-based approach!

    @author esquivel
    @date 12/2008
*/
  class BIASGeometry_EXPORT AbsoluteOrientation
  {

    friend class AbsoluteOrientationRANSAC;

  public:

    /// @brief Methods for rotation estimation
    enum RotationEstimationMethod { HORN_ALGORITHM, KABSCH_ALGORITHM };

    static const unsigned int MIN_CORRESPONDENCES;

    AbsoluteOrientation();

    ~AbsoluteOrientation();

    /** @brief Computes rotation dR, translation dt and isometric scale ds
        between coordinate systems from corresponding 3D point sets X and Y.
        @return -1 if computation failed, else 0.
     */
    int Compute(const std::vector< Vector3<double> > &X,
                const std::vector< Vector3<double> > &Y,
                RMatrix &dR, Vector3<double> &dt, double &ds,
                const std::vector<double> &w = std::vector<double>(0)) {
      return Compute_(X, Y, dR, dt, ds, false, w);
    };

    /** @brief Computes rotation dR and translation dt between coordinate
        systems from corresponding 3D point sets X and Y.
        @return -1 if computation failed, else 0.
     */
    int Compute(const std::vector< Vector3<double> > &X,
                const std::vector< Vector3<double> > &Y,
                RMatrix &dR, Vector3<double> &dt,
                const std::vector<double> &w = std::vector<double>(0)) {
      double ds = 1.0;
      return Compute_(X, Y, dR, dt, ds, true, w);
    };

    /** @brief Computes rotation dR between corresponding 3D ray sets X and Y
               using the currently set algorithm.
        @return -1 if computation failed, else 0.
     */
    int ComputeRotation(const std::vector< Vector3<double> > &X,
                        const std::vector< Vector3<double> > &Y,
                        RMatrix &dR,
                        const std::vector<double> &w = std::vector<double>(0));

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

    /** @brief Computes residual errors 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
                and 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<double> &w
                             = std::vector<double>(0));

    /** @brief Computes residual errors 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);

    /** @brief Computes residual errors 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
                and 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<double> &w
                             = std::vector<double>(0));

    /** @brief Computes residual errors 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);

  private:

    /// @brief Threshold for comparison to zero
    double epsilon_;
    
    /// @brief Method used for rotation estimation
    RotationEstimationMethod rotationAlgorithm_;
 
    /** @brief Computes rotation dR, translation dt and isometric scale ds
        between coordinate systems from corresponding 3D point sets X and Y.
        @note Given scale is optionally enforced instead of being computed.
        @note Error term for each residual is weighted by w optionally.
        @return -1 if computation failed, else 0.
     */
    int Compute_(const std::vector< Vector3<double> > &X,
                 const std::vector< Vector3<double> > &Y,
                 RMatrix &dR, Vector3<double> &dt,
                 double &ds, bool enforceScale = false,
                 const std::vector<double> &w = std::vector<double>(0));

    /** @brief Computes rotation dR between corresponding 3D ray sets X and Y
               using method proposed by Horn [1].
        @return -1 if computation failed, else 0.
     */
    int ComputeRotationHorn_(const std::vector< Vector3<double> > &X,
                             const std::vector< Vector3<double> > &Y,
                             RMatrix &dR,
                             const std::vector<double> &w = std::vector<double>(0));

    /** @brief Computes rotation dR between corresponding 3D ray sets X and Y
               using the Kabsch algorithm [2].
        @return -1 if computation failed, else 0.
     */
    int ComputeRotationKabsch_(const std::vector< Vector3<double> > &X,
                               const std::vector< Vector3<double> > &Y,
                               RMatrix &dR,
                               const std::vector<double> &w = std::vector<double>(0));

  };

}
#endif // _AbsoluteOrientation_hh_