PMatrixLinear.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 PMatrixLinear_hh_
#define PMatrixLinear_hh_
#include <Base/Common/BIASpragmaStart.hh>
#include <vector>

#include <Base/Debug/Debug.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <MathAlgo/SVD.hh>
#include "PMatrix.hh"

namespace BIAS {
  /** @class PMatrixLinear
      @ingroup g_estimation
      @brief This class computes a PMatrix from 2D/3D correspondences with 
      linear methods. 

      No nonlinear constraints are enforced: If you have noisy points, you 
      may get inexact rotations or non-metric PMatrices, although you 
      compute in a metric framework. however, this class provides 
      functions which can also be applied in projectively skewed space.
      @author koeser/frahm
  */

  class BIASGeometry_EXPORT PMatrixLinear : public Debug {
    
  public:
    
    PMatrixLinear();

    /** @brief computes a least squares solution P via SVD, if at least 6 2d-3d
        correspondences (pointers) are provided in points3D and points2D.
        
        @attention P may not exactly be decomposable due to noise in points
        @param points3D 3D points which project to points2D
        @param points2D 2D projections of points3D, any K allowed !
        @param P resulting (not always metric) PMatrix
        @return false on ambiguity, true on success */
    bool Compute(const std::vector<HomgPoint3D*> &points3D,
                 const std::vector<HomgPoint2D*> &points2D,
                 PMatrix& P);

    /** @brief computes a least squares solution P via SVD, if at least 6 2d-3d
        correspondences are provided in points3D and points2D
 
        Algorithm used is DLT. Fails if points3D are coplanar.
        @attention P may not exactly be decomposable due to noise in points
        @param points3D 3D points which project to points2D
        @param points2D 2D projections of points3D, any K allowed !
        @param P resulting (not always metric) PMatrix
        @return false on ambiguity, true on success */
    bool Compute(const std::vector<HomgPoint3D> &points3D,
                 const std::vector<HomgPoint2D> &points2D,
                 PMatrix& P);

    /** @brief given n>=6 2D/3D correspondences, compute approximate R and C 
        using linear methods (thats why we need 6, not 3 correspondences !)

        The lines of each two 3D points and their projections into the image
        are used to determine the rotation of P. For n points, we have
        n(n-1)/2 lines (quadratic in n!) and also linear equations,
        dont pass too many points if you are interested in performance ! After 
        R is determined, C is computed separately using linear methods (DLT)
        starting from the equation Rx ~= [  I  |-C] X
        Algorithm fails if points3D are coplanar
        @attention R is no real rotation, enforce afterwards, if you want that!
        @param points3D 3D points which project to points2D
        @param points2D 2D projections of points3D, K=Identity required !
        @param Pose result R^T*[  I  |-C]
        @return true on success, false on error (ambiguity)
        @author koeser 10/2004 */
    bool ComputeCalibrated(const std::vector<HomgPoint3D*> &points3D,
                           const std::vector<HomgPoint2D*> &points2D,
                           PMatrix& Pose);

 
    /** @brief sets up the homogeneous equation system A for pmatrix 
        computation using the 2d/3d correspondences provided 
        @author frahm */
    bool GetPEstSystemHom(Matrix<double>& A,
                          const std::vector<HomgPoint3D*>& points3D,
                          const std::vector<HomgPoint2D*>& points2D);

    /** @brief sets up an inhomogeneous equation system A for pmatrix 
        computation using the 2d/3d correspondences provided 
        @author frahm */
    bool GetPEstSystemInHom(Matrix<double>& A, Vector<double>& b,
                            const std::vector<HomgPoint3D*>& points3D,
                            const std::vector<HomgPoint2D*>& points2D);   
    

  };
}

#include <Base/Common/BIASpragmaEnd.hh>

#endif