CovMatrix3x3.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 __CovMatrix3x3_hh__
#define __CovMatrix3x3_hh__

#include <Base/Math/Matrix3x3.hh>
#include <Base/Geometry/PMatrixBase.hh>
#include <Base/Geometry/HomgPoint3DCov.hh>
#include <MathAlgo/SVD.hh>

#define COVMATRIX3X3_TYPE double

namespace BIAS {
  /** @class CovMatrix3x3
      @ingroup g_math
      @ingroup g_geometry
      @brief class for 3x3 covariance matrices

      This class represents a 3x3 convariance matrix with the capability of
      doing a PCA (principle component analysis).

      @attention For performance reasons, the PCA is only done once and saved,
      subsequent calls to GetPCA() only return a copy.
      If the covariance matrix is changed, you must call InvalidatePCA()
      to force a recomputation at the next GetPCA()-Call ! */

  class BIASGeometry_EXPORT CovMatrix3x3 : public Matrix3x3<COVMATRIX3X3_TYPE>
  {
  public:
    CovMatrix3x3();

    ~CovMatrix3x3();

    CovMatrix3x3(const CovMatrix3x3& mat); 

    explicit CovMatrix3x3(const MatrixInitType& i)
      : Matrix3x3<COVMATRIX3X3_TYPE>(i), _IsDecomposed(false) {};
    
    CovMatrix3x3(const Matrix3x3<COVMATRIX3X3_TYPE> & mat) 
      { (*this) = mat; };
    

    int GetPCA(Vector3<COVMATRIX3X3_TYPE>& S, 
               Matrix3x3<COVMATRIX3X3_TYPE>& VT);
    
    int GetS(Vector3<COVMATRIX3X3_TYPE>& S);
    
    CovMatrix3x3& operator=(const CovMatrix3x3& mat); 
    
    CovMatrix3x3& operator=(const BIAS::Matrix3x3<COVMATRIX3X3_TYPE>& mat) {
      Matrix3x3<COVMATRIX3X3_TYPE>::operator=(mat);
      _IsDecomposed = false;  return (*this); };

    /** @brief returns matrix's trace. If this = V * S * V^T, trace means |S|^2
        and provides a scalar measure of how uncertain a 3D point is. 

        Note the difference to det() which means geometric averaging of 
        singular values rather than arithmetic. If you want to imagine an
        ellipsoid, trace is proportional to the mean axis length, while det 
        is more like the space inside the ellipsoid.
        @author koeser */
    double Trace() const {
      return ((*this)[0][0] + (*this)[1][1] + (*this)[2][2]);
    }


    /** Computes the bounding box x=[xmin,xmax] , y=[ymin,ymax] in image  as 
        determined from the projected covariance matrix covmat. multiplied with
        scale
        @author frahm, woelk 02 2003 */
    int GetSearchArea(PMatrixBase& P, HomgPoint3D& point3D, 
                      Vector2<double>& x, Vector2<double>& y, double scale);

    /** Computes the bounding box x=[xmin,xmax] , y=[ymin,ymax] in image  as 
        determined from the projected covariance matrix covmat. 
        A min Search Area can be specified with minSearchArea(which is the
        radius of the uncertainty sphere).
        @author Daniel Grest, Feb 2003
    */
    int GetSearchArea(PMatrixBase& P, HomgPoint3D& point3D, 
              Vector2<double>& x, Vector2<double>& y, 
              double scale, double minSearchArea);


    /** Call this function to indicate that the matrix fields have changed
        The saved PCA is declared invalid and the next call to GetPCA forces
        a recomputation
        @author koeser 09/2003 */
    inline void InvalidatePCA() { _IsDecomposed = false; };

    /** Type conversion for homogenous covariances
        @author streckel 05/2006 */
    HomgPoint3DCov GetHomogenized() const {
      Matrix4x4<double> dum((*this)[0][0],(*this)[0][1],(*this)[0][2],0,
                            (*this)[1][0],(*this)[1][1],(*this)[1][2],0,
                            (*this)[2][0],(*this)[2][1],(*this)[2][2],0,
                            0,0,0,0);
      HomgPoint3DCov ch(dum);
      return ch;
    }

    /** Type conversion for homogenous covariances
        @author streckel 05/2006 */
    void SetFromHomogenized(const HomgPoint3DCov& ch, const HomgPoint3D& ph) {
      //unconst variables
      HomgPoint3D myph(ph);
      HomgPoint3DCov mych(ch);
      mych.Homogenize(myph);
      Matrix3x3<double> mat(mych[0][0],mych[0][1],mych[0][2],
                            mych[1][0],mych[1][1],mych[1][2],
                            mych[2][0],mych[2][1],mych[2][2]);
      CovMatrix3x3 cov(mat);
      *this = cov;
    }

  protected:
    /// flag indicating whether the stored PCA is still valid
    bool _IsDecomposed;
 
     
      
#ifdef BIAS_DEBUG
    /// saves matrix fields to make consistency checks possible later
    Matrix3x3<COVMATRIX3X3_TYPE> ConsistencyBackup_;
#endif
   
    SVD _svd;
    Vector3<COVMATRIX3X3_TYPE> _S;
    Matrix3x3<COVMATRIX3X3_TYPE> _U, _VT;

    int _Decompose();
  }; // class



} // namespace


#endif // __CovMatrix3x3_hh__