EParametrization.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 __EParametrization_hh__
#define __EParametrization_hh__

#include <Base/Math/Vector.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Matrix4x4.hh>
#include <Base/Geometry/Quaternion.hh>

#include <Base/Common/CompareFloatingPoint.hh>

#include <fstream>

namespace BIAS {

#define EP_TYPE double

  /** @class EParametrization
      @brief Slim class bundling essential matrix parameterization and
      associated covariance matrix.

      The essential is parameterized as vector of dimension 7 where the first 3
      entries represent the epipole and the last 4 entries represent the
      quaternion orientation.

      \author woelk 07/2006 */
  class BIASGeometryBase_EXPORT EParametrization
  {
  public:
    EParametrization();

    EParametrization(const Vector<EP_TYPE>& EQvec,
                     const Matrix<EP_TYPE>& covariance);

    EParametrization(const Vector3<EP_TYPE>& epipole,
                     const Quaternion<EP_TYPE>& orientation,
                     const Matrix<EP_TYPE>& covariance);

    /** Set this from E parametrization. Zeros associated
        covariance matrix */
    void SetEQ(const Vector<EP_TYPE>& EQvec);

    /** Set this from epipole and orientation. Zeros associated
        covariance matrix */
    void SetEQ(const Vector3<EP_TYPE>& epipole,
               const Quaternion<EP_TYPE>& orientation);

    /** Set this from epipole, orientation and covariance */
    void Set(const Vector3<EP_TYPE>& epipole,
             const Quaternion<EP_TYPE>& orientation,
             const Matrix<EP_TYPE>& covariance);

    inline void Set(const Vector<EP_TYPE>& EQvec, const Matrix<EP_TYPE>& Cov)
    { SetEQ(EQvec); SetCovarianceMatrix(Cov); }

    /** Sets covariance matrix from Cov. Cov must be a 7x7 symmetric positive
        definite matrix (i.e. a covariance matrix) */
    void SetCovarianceMatrix(const Matrix<EP_TYPE>& Cov);

    /** Returns a vector of dimension 7 where first 3 entries are
        the epipole and the last 4 entries are the quaternion orientation */
    inline const Vector<EP_TYPE>& GetEQ7() const
    { return E_; }

    /** returns the covariance matrix associated with the EQ vector */
    inline const Matrix<EP_TYPE>& GetCov() const
    { return Cov_; }

    /** set epipole part of vector and zero covariance matrix */
    void SetEpipole(const Vector3<EP_TYPE>& epipole);

    /** set orientation part of vector and zero covariance matrix */
    void SetOrientation(const Quaternion<EP_TYPE>& q);

    /** returns the epipole (first 3 entries) part of the  essential matrix
        parametrization vector */
    Vector3<EP_TYPE> GetEpipole3() const;

    /** returns the epipole (first 3 entries) part of the essential matrix
        parametrization vector */
    void GetEpipole(Vector3<EP_TYPE>& epipole) const;

    /** returns the essential matrix computed from parametrization */
    Matrix3x3<EP_TYPE> GetEssentialMatrix() const;

    /** returns the essential matrix computed from parametrization */
    void GetEssentialMatrix(Matrix3x3<EP_TYPE>& E) const;

    /** returns the quaternion (last 4 entries) part of the  essential matrix
        parametrization vector */
    Quaternion<EP_TYPE> GetOrientation() const;

    /** covariance to GetOrientation()
        @author woelk 09/2006 */
    Matrix4x4<EP_TYPE> GetOrientationCov() const;

    /** return whole (with orientation and epipole) covariance matrix */
    Matrix<EP_TYPE> GetCovarianceMatrix() const;

    /** covariance to GetEpipole(),
        @author woelk 09/2006 */
    //Matrix3x3<EP_TYPE> GetEpipoleCov() const;

    /** covariance to GetEpipole(Vector3<double>))
        @author woelk 09/2006 */
    void GetEpipoleCov(Matrix3x3<EP_TYPE>& cov) const;

    bool Load(const std::string& file);

    bool Save(const std::string& file) const;

    /** inverts the direction, */
    void Invert();

    /* normalizes the epipole and the quaternion */
    void Normalize(const bool update_covariance = true);

  protected:
    /** first 3 entries relate to the epipole, the last four to the quaternion
        orientation */
    Vector<EP_TYPE> E_;
    /** first 3 columns relate to epipole, the last four columns relate to
        the orientation */
    Matrix<EP_TYPE> Cov_;

    inline void E2EQ_(const Vector<EP_TYPE>& E,
                      Vector3<EP_TYPE>& epipole,
                      Quaternion<EP_TYPE>& orientation) const{
      epipole[0] = E[0];
      epipole[1] = E[1];
      epipole[2] = E[2];
      orientation[0] = E[3];
      orientation[1] = E[4];
      orientation[2] = E[5];
      orientation[3] = E[6];
    };

    inline void EQ2E_(const Vector3<EP_TYPE>& epipole,
                      const Quaternion<EP_TYPE>& orientation,
                      Vector<EP_TYPE>& E) const{
      E[0] = epipole[0];
      E[1] = epipole[1];
      E[2] = epipole[2];
      E[3] = orientation[0];
      E[4] = orientation[1];
      E[5] = orientation[2];
      E[6] = orientation[3];
    };

  };

  BIASGeometryBase_EXPORT std::ostream&
    operator<<(std::ostream &os, const EParametrization& p);

  BIASGeometryBase_EXPORT std::istream&
    operator>>(std::istream &is, EParametrization& p);

}

#endif