FMatrixEstimation.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 _FMatrixEstimation_h__
#define _FMatrixEstimation_h__

#include <Base/Common/BIASpragmaStart.hh>

#include <vector>
#include "FMatrix.hh"
#include "Base/Geometry/HomgPoint2D.hh"
#include "MathAlgo/PolynomialSolve.hh"
#include "Base/Math/Vector.hh"
#include "Base/Geometry/Normalization.hh"
#include "Parametrization.hh"

namespace BIAS {
/**  @class FMatrixEstimation
 @ingroup g_estimation
 @brief functions for estimating a fundamental matrix (FMatrix) given
 a set of 2d-2d correspondences (no outlier checks, not robust !)
 in two views
 @author koeser 11/2003
 */
class BIASGeometry_EXPORT FMatrixEstimation : public Debug {
public:

  /** @brief constructor with general parameters
   @param NormalizeHartley do hartley normalization on 7/8 points */
  explicit inline FMatrixEstimation(bool NormalizeHartley = true) {
    NormalizeHartley_ = NormalizeHartley;
  }

  /** @brief compute a vector of FMatrices from seven point correspondences

   Depending on the setting of the 3d points and the cameras of the
   originating epipolar geometry a vector of no, one or three solutions
   is computed:
   - degenrate case: no solution
   - 3d points and cam centers all lie on a ruled quadric: 3 solutions
   - 3d points and cam centers all lie on an unruled quadric: 1 solution

   @attention For robust computation it is strongly recommended that you
   normalize your correspondences (e.g. "Hartley")
   @param Fvec (output) vector of F matrices which are computed
   @param p1 (homogenized!) points in image 1
   @param p2 corresponding (homogenized!) points in image 2
   @return 0=success, !=0: error
   @status tested, not-optimized
   @author koeser 11/2003 */
  int SevenPoint(std::vector<BIAS::FMatrix>& Fvec,
      const std::vector<BIAS::HomgPoint2D> &p1,
      const std::vector<BIAS::HomgPoint2D> &p2);

  /** @brief compute an FMatrix from more than seven point correspondences

   This algorithm computes a least squares F on all point correspondences
   using SVD of a model matrix. Rank 2 is enforced afterwards.
   @attention For robust computation it is strongly recommended that you
   normalize your correspondences (e.g. "Hartley")
   @param F matrix which is computed
   @param p1 (homogenized!) points in image 1
   @param p2 corresponding (homogenized!) points in image 2
   @return 0=success, !=0: error
   @author koeser 11/2003 */
  int Linear(BIAS::FMatrix& F,
      const std::vector<BIAS::HomgPoint2D> &p1,
      const std::vector<BIAS::HomgPoint2D> &p2);

  /** @brief compute an FMatrix from eight point correspondences

   This algorithm computes a least squares F on all point correspondences
   using SVD of a model matrix. Rank 2 is enforced afterwards, therefore
   @attention For robust computation it is strongly recommended that you
   normalize your correspondences (e.g. "Hartley")
   @param F matrix which is computed
   @param p1 (homogenized!) points in image 1
   @param p2 corresponding (homogenized!) points in image 2
   @return 0=success, !=0: error
   @author koeser 11/2003 */
  inline int EightPoint(BIAS::FMatrix& F,
      const std::vector<BIAS::HomgPoint2D> &p1,
      const std::vector<BIAS::HomgPoint2D> &p2)
  { return Linear(F, p1, p2);};

  /** @brief optimize a given FMatrix regarding n 2d point correspondences

   minimize an error function on F matrix using iterative
   nonlinear optimization techniques such as Levenberg/Marquardt
   @attention susceptable to local minima, provide good starting point F
   @param F matrix which is optimized
   @param p1 (homogenized!) points in image 1
   @param p2 corresponding (homogenized!) points in image 2
   @return 0=success, !=0: error
   @author koeser 11/2003 */
  int Optimize(BIAS::FMatrix& F,
      const std::vector<BIAS::HomgPoint2D> &p1,
      const std::vector<BIAS::HomgPoint2D> &p2);

  /** @brief Compute FMatrix using the Gold Standard algorithm as in
   *   Hartley and Zisserman p. 285
   *  @param F matrix which is calculated
   *  @param p1 (homogenized points in image1
   *  @param p2 corresponding (homogenized!) points in image 2
   *  @return 0=success, !=0: error
   *  @author sedlazeck 04/2008
   */
  int GoldStandard(BIAS::FMatrix& F,
      const std::vector<BIAS::HomgPoint2D> &p1,
      const std::vector<BIAS::HomgPoint2D> &p2);

  /** @brief enforces a maximum rank of two by setting the smallest singular
   value to zero

   @param F matrix which has rank MIN{2, previous_rank} afterwards
   @author koeser 11/2003 */
  void EnforceMaxRank2(BIAS::FMatrix &F);

  /// pointer to vector of points in image1 (hack for Optimize)
  std::vector<BIAS::HomgPoint2D> const *p1_;
  /// pointer to vector of points in image2 (hack for Optimize)
  std::vector<BIAS::HomgPoint2D> const *p2_;
  /// count Levenberg-Marquardt iterations
  int itersLM_;


  /** @brief set up a polynomial in alpha given Det(alpha*F1+(1-alpha)*F2)=0,
   where the f vectors are interpreted as the rows of a 3x3 matrix

   @param f1 vector to be interpreted as a 3x3 matrix F1
   @param f2 vector to be interpreted as a 3x3 matrix F2
   @param v vector of polynomial coefficients in PolynomialSolve style
   @author koeser 11/2003 */
  void GetDetPolynomial(const BIAS::Vector<double> &f1,
      const BIAS::Vector<double> &f2,
      std::vector<POLYNOMIALSOLVE_TYPE> &v);


  void ComputeNormFParam(const BIAS::FMatrix& initialF, BIAS::FMatrix& normF);

  /// matrix for saving normalization of 2D points - important for f parametrization
  Matrix3x3<double> NormF1_;
  /// matrix for saving normalization of 2D points - important for f parametrization
  Matrix3x3<double> NormF2_;

  BIAS::Parametrization *currentPara_;

protected:
  /// flag indicating whether we should normalize the given points before
  /// and denormalize the fmatrix after computation, so that it refers to
  /// the original points
  bool NormalizeHartley_;

  /// the normalization object
  Normalization N_;

}; // end class

}
// end namespace

#include <Base/Common/BIASpragmaEnd.hh>

#endif