PMatrixEstimation.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 _PMatrixEstimation_h__
#define _PMatrixEstimation_h__

#include <iostream>
#include <fstream>
#include <string>

// BIAS:
#include "Base/Math/Matrix.hh"
#include "FMatrix.hh"
#include "RMatrix.hh"
#include "Geometry/PMatrix.hh"
#include "MathAlgo/Lapack.hh"

#define BIAS_MAXNUMAUTOCALIBITERS 200

namespace BIAS {
/** @class PMatrixEstimation
 @ingroup g_estimation
 @brief compute standard P1/P2 from F.
 P1 is set to [  I  | 0 ]. The epipole (determined by from F) tells us
 only the direction of C2, so choose a C2 with |C2| = 1
 @author frahm, koeser */
class BIASGeometry_EXPORT PMatrixEstimation : public Debug {

public:
  /** @brief Standard constructor does nothing */
  PMatrixEstimation();

  /** @brief Standard Destructor does nothing */
  ~PMatrixEstimation() {};

  /** @brief given an FMatrix set P1 as identity and compute P2 to be
   consistent with F and P1 such that P2 is euclidean with nonlinear 
   estimation.*/
  void ComputeFromFQuasiEuklid(BIAS::FMatrix &F, double BaselineMagnitude,
      BIAS::PMatrix &P1, BIAS::PMatrix &P2);

  /** @brief given an FMatrix set P1 as identity and compute P2 to be
   consistent with F and P1 such that P2 is euclidean with direct 
   solution of the problem 
   
   @return 0: ok    <0: if epipoles could not be extracted and 
   P computation failed  */
  int ComputeFromFDirect(BIAS::FMatrix &F, const double& BaselineMagnitude,
      BIAS::PMatrix &P1, BIAS::PMatrix &P2);

  /** @brief given an FMatrix, width and height of the images, and the 
   2D-correspondences of two images, the Levenberg-Marquardt algorithm
   is used to determine focal length, and the two projection matrices.
   @author sedlazeck 04/2008
   @return 0: ok
   */
  int AutoCalib(const BIAS::FMatrix &F, const int width, const int height,
      const std::vector<BIAS::HomgPoint2D> &p1,
      const std::vector<BIAS::HomgPoint2D> &p2,
      BIAS::PMatrix &P1, BIAS::PMatrix &P2);

  static void ComputeRotation(BIAS::FMatrix &F, BIAS::HomgPoint2D &Epipole1,
      BIAS::HomgPoint2D &Epipole2, BIAS::RMatrix &R);

  static void ComputeRotationCenter(BIAS::PMatrix P1, BIAS::PMatrix P2,
      BIAS::RMatrix &R,
      BIAS::Vector3<double> &C);

  /// pointer to vector of points in image1 (hack for Levenberg Marquardt)
  std::vector<BIAS::HomgPoint2D> const *p1_;
  /// pointer to vector of points in image2 (hack for Levenberg Marquardt)
  std::vector<BIAS::HomgPoint2D> const *p2_;
  /// image width (hack for Levenberg Marquardt)
  int width_;
  /// image height (hack for Levenberg Marquardt)
  int height_;
  /// remember error of PErrorFunc
  double lastError_;

  std::vector<double> errorSum_;
  std::vector<std::vector<double> > errorAll_;

  BIAS::Matrix<double> globalM2_;
  BIAS::HomgPoint2D globalEpipole2_;

protected:
  /** @brief set P1 to identity, 
   set P2 to identity with last column epipole2 */
  void InitP1P2Trans (BIAS::PMatrix &P1, BIAS::PMatrix &P2,
      BIAS::HomgPoint2D &Epipole2);

  /** @brief given approximate P2, compute the nearest value of P2 which is
   exactly compatible with P1 and F */
  void FindClosestP2 (BIAS::FMatrix &F,
      //BIAS::PMatrix &P1,
      BIAS::PMatrix &P2,
      Matrix3x3<double> &M2Matrix,
      Vector<double> &C,
      HomgPoint2D & Epipole2);

  /** @brief private function, that can determine focal length using different initial guesses for AutoCalib 
   * @author sedlazeck 04/2008
   * */
  int AutoCalib_(const BIAS::FMatrix &F, const long int numCorrs, BIAS::KMatrix &K, BIAS::PMatrix &P1,
      BIAS::PMatrix &P2);

};
}

#endif