RotationAveraging.hh

#ifndef __RotationAveraging_hh__
#define __RotationAveraging_hh__

#include <bias_config.h>

#include <Base/Debug/Debug.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Geometry/RMatrix.hh>
#include <vector>

namespace BIAS {

/** @class   RotationAveraging
    @brief   Computes mean of rotations due to different measures.
    @ingroup g_geometry
    @author  esquivel
    @date    09/2011

    This tool computes the average rotation of given rotations R_1,...,R_n
    with respect to different distance measures d: SO(3) x SO(3) -> R
    on rotations as described in [1].
    The L2-mean for a given d is defined by the rotation matrix R from
    mean(R_1,...,R_n) = \argmin_R \sum_{i=1}^N d(R,R_i)^2
    while the L1-mean is given by \argmin_R \sum_{i=1}^N d(R,R_i).

    Implemented so far are:

    - geodesic L2-mean (Karcher mean): d_geod(R,S) = |log(RS')| where
      the rotation matrix logarithm computes the angle-axis representation
      theta*r for a rotation matrix R, hence d_geod(R,S) returns the
      rotation angle theta of the matrix RS'

    - quaternion L2-mean: d_quat(R,S) = min(|r-s|,|r+s|) where r,s are
      the unit quaternion representations of rotation matrices R,S,
      hence d_quat(R,S) returns 2*sin(theta/4)

    - chordal L2-mean: d_chor(R,S) = |R-S|_F = 2*sqrt(2)*sin(theta/2)

    - geodesic L1-mean (geodesic median): computed via Weiszfeld algorithm [2]

    - logarithmic L2-mean: computes R = exp(mean(log(R_1),...,log(R_n)))

    While closed form solutions exist for the L2-means (except geodesic mean),
    iterative algorithms are given for the geodesic L1-/L2-mean.

    The solution is in general only optimal under the assumptions that
    the rotation angles of all R_i are less than 180 degree resp. all
    rotations cover less than a hemisphere of the space SO(3).

    [1] Y. Dai, J. Trumpf, H. Li, N. Barnes & R. Hartley: "Rotation
        Averaging with Application to Camera-Rig Calibration", 2009.
    [2] R. Hartley, K. Aftab, J. Trumpf: "L1 Rotation Averaging Using the
        Weiszfeld Algorithm", 2011.
*/
  class BIASGeometry_EXPORT RotationAveraging : public BIAS::Debug
  {
    public:

      RotationAveraging();

      ~RotationAveraging() {}

      /** @brief Set threshold for convergence of iterative geodesic L1-/L2-mean
                 computation. The algorithm stops if the angular update is less.
          @param[in] epsilon Threshold for convergence in radians */
      inline void SetTolerance(double epsilon) { tolerance_ = epsilon; }

      /** @brief Set maximal number of iterations for geodesic L1-/L2-mean
                 computation.
          @param[in] num Maximal number of iterations */
      inline void SetMaxIterations(unsigned int num) { maxIters_ = num; }

      /** @brief Compute geodesic L1-mean (geodesic median) of rotations.
          @param[in] R Input rotation matrices to compute the average for
          @param[out] meanR Output average rotation matrix
          @return Returns 0 on success, < 0 otherwise. */
      int GeodesicL1Mean(const std::vector<BIAS::RMatrix> &R,
                         BIAS::RMatrix &meanR);

      /** @brief Compute geodesic L2-mean (Karcher/geometric mean) of rotations.
          @param[in] R Input rotation matrices to compute the average for
          @param[out] meanR Output average rotation matrix
          @return Returns 0 on success, < 0 otherwise. */
      int GeodesicL2Mean(const std::vector<BIAS::RMatrix> &R,
                         BIAS::RMatrix &meanR);

      /** @brief Compute quaternion L2-mean of rotations.
          @param[in] R Input rotation matrices to compute the average for
          @param[out] meanR Output average rotation matrix
          @return Returns 0 on success, < 0 otherwise. */
      int QuaternionL2Mean(const std::vector<BIAS::RMatrix> &R,
                           BIAS::RMatrix &meanR);

      /** @brief Compute quaternion L2-mean of rotations.
          @param[in] Q Input unit quaternions to compute the average for
          @param[out] meanQ Output average rotation as unit quaternion
          @return Returns 0 on success, < 0 otherwise. */
      int QuaternionL2Mean(const std::vector<BIAS::Quaternion<double> > &Q,
                           BIAS::Quaternion<double> &meanQ);

      /** @brief Compute matrix logarithm L2-mean of rotations.
          The matrix logarithm of a rotation matrix is the angle/axis vector
          phi * r, also known as the modified Rodrigues vector.
          @param[in] R Input rotation matrices to compute the average for
          @param[out] meanR Output average rotation matrix
          @return Returns 0 on success, < 0 otherwise. */
      int LogarithmicL2Mean(const std::vector<BIAS::RMatrix> &R,
                            BIAS::RMatrix &meanR);

      /** @brief Compute chordal L2-mean of rotations.
          @param[in] R Input rotation matrices to compute the average for
          @param[out] meanR Output average rotation matrix
          @return Returns 0 on success, < 0 otherwise. */
      int ChordalL2Mean(const std::vector<BIAS::RMatrix> &R,
                        BIAS::RMatrix &meanR);

      /** @brief Compute chordal L2-mean of rotations using unit quaternions.
          @param[in] Q Input unit quaternions to compute the average for
          @param[out] meanQ Output average rotation as unit quaternion
          @return Returns 0 on success, < 0 otherwise. */
      int ChordalL2Mean(const std::vector<BIAS::Quaternion<double> > &Q,
                        BIAS::Quaternion<double> &meanQ);

  private:

      /** Threshold for convergence of geodesic L1-/L2-mean computation */
      double tolerance_;

      /** Maximal number of iterations for geodesic L1-/L2-mean computation */
      unsigned int maxIters_;

      /** @brief Test if given rotations are appropriate for averaging. */
      bool TestRotations(const std::vector<BIAS::RMatrix> &R);

      /** @brief Test if unit quaternions are appropriate for averaging. */
      bool TestRotations(const std::vector<BIAS::Quaternion<double> > &Q);

  };

}

#endif //__RotationAveraging_hh__