HMatrixEstimation.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 __HMatrixEstimation_hh__
#define __HMatrixEstimation_hh__
#include "bias_config.h" 

#include <Geometry/HMatrix.hh>
#include <Base/Geometry/Normalization.hh>

namespace BIAS {

#define HE_OK 0
#define HE_NOT_ENOUGH_POINTS -78
#define HE_SVD_ERROR -79
#define HE_LM_ERROR -80


  /** @class HMatrixEstimation
      @ingroup g_geometry
      @brief Estimate 3x3 matrix relating image coordinates with each other,
             i.e. a homography or affine transformation.

      @author koeser 12/2005 
  */
  class BIASGeometry_EXPORT HMatrixEstimation
  {
  public:
    
    /** @brief Create homography estimation instance.
        @param normalizeHartley Use normalization of input vectors proposed by
               Hartley (strongly recommended) or use the DLT on raw data
    */
    explicit HMatrixEstimation(bool normalizeHartley = true)
    { normalizeHartley_ = normalizeHartley; }

    ~HMatrixEstimation() {};

    /** @brief Calculate homography between 2d points using the DLT.

        The direct linear transformation algorithm (see Hartley & Zisserman:
        "Multiple View Geometry", p.73) solves the equation system
        (p' x (H * p) = 0 in order to solve p' ~ H * p where p, p' are
        2d image coordinates in homogeneous coordinates.
        Each correspondence provides 3 equation, but the linear equation
        system has but rank 2, so we take only the first two rows.
        The resulting equation system A*h = 0 s.t. |h| = 1 is solved via
        SVD where h is vector representation of matrix H.

        @param fromPoints Homogenized points in image 1
        @param toPoints Corresponding homogenized points in image 2
        @param H Returns estimated homography matrix
        @author woelk/frahm
    */
    int Compute(const std::vector<BIAS::HomgPoint2D>& fromPoints, 
                const std::vector<BIAS::HomgPoint2D>& toPoints, 
                HMatrix& H);

    /** @brief Optimize a given homography matrix from 2d point correspondences.     

        Minimize an error function on the homography matrix using iterative 
        nonlinear optimization techniques such as Levenberg-Marquardt.

        @attention Susceptible to local minima, provide good starting point!
        @param H Homography matrix to optimize, provides starting point
        @param p1 Homogenized points in image 1
        @param p2 Corresponding homogenized points in image 2
        @return Return 0 on success, <0 for errors
        @author koeser 06/2006
    */
    int Optimize(BIAS::HMatrix& H,
                 const std::vector<BIAS::HomgPoint2D> &p1, 
                 const std::vector<BIAS::HomgPoint2D> &p2);

    /** @brief Compute affine transformation between 2d points, i.e. rotation
               and translation. This method is not using the DLT!
        @param fromPoints Homogenized points in image 1
        @param toPoints Corresponding homogenized points in image 2
        @param H Returns estimated affine transformation matrix
        @author koeser 12/2005
    */
    int ComputeAffine(const std::vector<BIAS::HomgPoint2D>& fromPoints, 
                      const std::vector<BIAS::HomgPoint2D>& toPoints, 
                      HMatrix& H);

    /** @brief Returns the average squared error of the estimation applying the
               estimated homography matrix to all correspondences.
    */
    double GetError() const;

    /** @brief Decide whether to normalize input vector as proposed by Hartley
               (strongly recommended) or use the DLT on raw data.
    */  
    void SetNormalizeHartley(bool normalize) { normalizeHartley_ = normalize; }
    
    /** @brief Calculate homography between 2d points using the DLT.
        @param fromPoints Homogenized points in image 1
        @param toPoints Corresponding homogenized points in image 2
        @param H Returns estimated homography matrix
        @author woelk/frahm
    */ 
    int Compute(const std::vector<BIAS::Vector2<double> >& fromPoints, 
                const std::vector<BIAS::Vector2<double> >& toPoints, 
                HMatrix &H);

  protected:

    /** @brief Flag indicating whether we should normalize the given points before
               and denormalize the homography matrix after computation, so that it
               refers to the original points */
    bool normalizeHartley_;
    
    /** @brief The normalization algorithm instance */
    Normalization normalization_;

    /** @brief Stores the estimated homography matrix (used in GetError) */
    HMatrix H_;

    /** @brief Stores the points in image 1 (used in Optimize and GetError) */
    std::vector<BIAS::HomgPoint2D> pointsFrom_;
 
    /** @brief Stores the points in image 2 (used in Optimize and GetError) */
    std::vector<BIAS::HomgPoint2D> pointsTo_;

    /** @brief Objective function for Levenberg-Marquardt optimization */
    static int HErrorFunction_(void *p, int m, int n, const double *x,
                               double *fvec, int iflag);
   
  };


}// namespace

#endif // __HMatrixEstimation_hh__