GaussHelmert.hh

#ifndef _GaussHelmert_hh_
#define _GaussHelmert_hh_
#include <bias_config.h>

#include <Base/Debug/Debug.hh>


#include <Base/Math/Vector.hh>
#include <Base/Math/Matrix.hh>
//template <class T> class Vector;
//template <class T> class Matrix;


namespace BIAS { 

  //template <class T> class Vector;
  //template <class T> class Matrix;

/** @class GaussHelmert
  * @ingroup g_mathalgo
    @brief see Foerstner, 2004, "Uncertainty And Projective Geometry" 
    ! untested !
    @author woelk 04/2006 
*/
  class BIASMathAlgo_EXPORT GaussHelmert
    : public Debug
  {
  public:

    virtual ~GaussHelmert();

    int Solve(BIAS::Vector<double>& observations, 
      const BIAS::Matrix<double>& obs_cov, 
      BIAS::Vector<double>& x, 
      BIAS::Matrix<double>& cov);


  protected:
    int MaxNumIter_;
    double Precision_;

    /// protected constructor to prohibit instantiation of class
    GaussHelmert();

    void ComputeNormalSystem_(const Matrix<double>& A, const Matrix<double>& B,
                              const Matrix<double>& H, 
                              const Matrix<double>& Sigma_ll,
                              const Vector<double>& x_hat, 
                              const Vector<double>& l_hat,
                              const Vector<double>& v_hat,
                              Matrix<double>& normal_matrix, 
                              Vector<double>& normal_vector);

    /// jacobian of g_() with respect to x at x, l
    virtual Matrix<double>& GetA_(const Vector<double>& l, 
                                  const Vector<double>& x) = 0;
    
    /// transposed of jacobian of g_() with respect to l at x, l
    virtual Matrix<double>& GetB_(const Vector<double>& l,
                                  const  Vector<double>& x) = 0;

    /// transposed of jacobian of h_() with respect to x at x
    virtual Matrix<double>& GetH_(const Vector<double>& x) = 0;

    /// joint constraints on obseravtions and unknowns
    virtual Vector<double>& g_(const Vector<double>& l, 
                               const Vector<double>& x) = 0;
    
    /// constraints on the unknowns
    virtual Vector<double>& h_(const Vector<double>& x) = 0;

  }; // class

} // namespace

#endif // _GaussHelmert_hh_