Triangulation.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 __Triangulation_hh__
#define __Triangulation_hh__
#include <bias_config.h>

//#include <Base/Common/BIASpragmaStart.hh>


#include <Base/Debug/Debug.hh>
#include <Base/Math/Matrix3x3.hh>
// #include <Base/Math/Matrix4x4.hh>
// #include <Base/Geometry/HomgPoint2DCov.hh>
// #include <Base/Geometry/HomgPoint3DCov.hh>
// #include "PMatrix.hh"
// #include "FMatrix.hh"


#include <vector>

/// minimal length of difference between two unit vectors
/// before they are considered to be not parallel
#define PARALLEL_THRESHOLD 1e-8

/// default step value left/right/up/down in pixels for triangulation
#define TRIANGULATION_DEFAULT_DELTAPIXEL 1.0
#define TRIANGULATION_DEFAULT_SIGMA 1.0

#define TRIANG_RES_PARALLEL       -42
#define TRIANG_RES_BEHIND_CAMERA1  1
#define TRIANG_RES_BEHIND_CAMERA2  2
#define TRIANG_RES_BEHIND_CAMERA   42

namespace BIAS{

  // forward declarations
  class BIASGeometry_EXPORT ProjectionParametersBase;
  class BIASGeometry_EXPORT Pose;
  class BIASGeometry_EXPORT FMatrix;
  class BIASGeometry_EXPORT PMatrix;
  class BIASGeometryBase_EXPORT HomgPoint2D;
  class BIASGeometryBase_EXPORT HomgPoint2DCov;
  class BIASGeometryBase_EXPORT HomgPoint3D;
  class BIASGeometryBase_EXPORT HomgPoint3DCov;
  template <class T> class BIASMathBase_EXPORT Matrix4x4;
  template <class T> class BIASGeometryBase_EXPORT Quaternion;
  template <class T> class BIASMathBase_EXPORT Vector3;

  /** @class Triangulation
   *  @test  tested with TestTriangulation.cpp
      @ingroup g_estimation
      @brief Class for triangulation of 3Dpoints from 2D matches.
      Covariance matrix (refering to an uncertainty ellipsoid) is computed also

      all functions so far only for 2 images (no trifocal).
      @author: Felix Woelk and Jan Frahm */
  class BIASGeometry_EXPORT Triangulation : public Debug
  {
  public:
    Triangulation();

    ~Triangulation();

    /** @brief analytic correct intersection of two lines, point returned has
        minimum distance to both lines.

        Lines have parametric description by point and direction.
        IMPORTANT: directions MUST BE normalized!!
        @return 0 in case of no errror, <0 if errors, 1 if intersection
        point lies not in direction of dirB, 2 if intersection
        point lies not in direction of dirA

        Refer to Kanatani Statistical Opt. for Geometric Comp. , page 108
        @author grest, Sept. 2003 */
    int Intersect(Vector3<double> &pA, Vector3<double> &dirA,
                  Vector3<double> &pB, Vector3<double> &dirB,
                  Vector3<double> &res);

    /** @brief analytic correct intersection of two lines, point returned has
        minimum distance to both lines, distance between lines is returned in
        dist.

        Lines have parametric description by point and direction.
        IMPORTANT: directions MUST BE normalized!!
        @return 0 in case of no errror, <0 if errors, 1 if intersection
        point lies not in direction of dirB
        @attention sanity check regarding dirA is not done !

        Refer to Kanatani Statistical Opt. for Geometric Comp. , page 108
        @author grest, Sept. 2003 */
    int Intersect(Vector3<double> &pA, Vector3<double> &dirA,
                  Vector3<double> &pB, Vector3<double> &dirB,
                  Vector3<double> &res, double &dist);

    /** @brief analytic correct intersection of two lines, point returned has
        minimum distance to both lines.

        Line A is described by points a1,a2 and line B by b1,b2
        Refer to Kanatani Statistical Opt. for Geometric Comp. ,page108
        @author grest, Sept. 2003 */
    Vector3<double> Intersect(Vector3<double> &a1, Vector3<double> &a2,
                              Vector3<double> &b1, Vector3<double> &b2);

    /** method from Hartley Zisserman chapter 12.5  pp 315
        first uses CorrectCorrespondences()
        and then calls TriangulateLinear()
        @author woelk 08/2004 */
    int Optimal(PMatrix &P1, PMatrix &P2, HomgPoint2D& p1, HomgPoint2D& p2,
                HomgPoint3D& point3d);

    /** @brief optimize 2d correspondences regarding F for triangulation

        In projective reconstruction you have to optimize the
        correspondences regarding epipolar geometry for the 3d point to
        be projectively optimal before you use TriangulateLinear()
        See section Hartley/Zisserman "Multiple View Geometry",
        section "optimal triangulation", section 12.5, pp.315

        if refine==true, a non linear optimization for the real roots of the
        6 degree polynomial is done using the powell algorithm from minpack

        Hereby F maps point from image 1 into lines in image 2, i.e. F_{12}.

        @author woelk 08/2004 heavily tested (ExampleTriangulateOptimal) */
    int CorrectCorrespondences(const FMatrix& F, HomgPoint2D& p1,
                               HomgPoint2D& p2, bool refine=false);

    /** @brief Triangulates a 3d point without decomposition of P matrix, use
        this method in selfcalibration scenarios

        Homogeneous DLT algorithm using SVD,
        see Hartley/Zisserman: "Multiple View Geometry", p.297
        For projective reconstructions first optimize 2d points using
        CorrectCorrespondences() regarding epipolar geometry

        @attention NOT YET TESTED AT ALL, since CorrectCorrespondences missing
        @author koeser, 10/2003*/
    int TriangulateLinear(PMatrix &P1, PMatrix &P2,
                          HomgPoint2D& p1, HomgPoint2D& p2,
                          HomgPoint3D& point3d);

    /** @brief Triangulates a 3d point without decomposition of P matrix, use
        this method in selfcalibration scenarios

        Homogeneous DLT algorithm using SVD,
        see Hartley/Zisserman: "Multiple View Geometry", p.297
        For projective reconstructions first optimize 2d points using
        CorrectCorrespondences() regarding epipolar geometry

        @attention tested with only some examples
        @author cmenk, 10/2006*/
    int TriangulateLinear(std::vector<PMatrix> pmatrices,
                          std::vector<HomgPoint2D> homgPoints2D,
                          HomgPoint3D& point3d);

    /** @brief Triangulation for metric PMatrices (using C and Hinf)
        @attention Decomposition of PMatrix is used !
        @param point3d (output) euclidean 3d point
        @return 0 on success \n <0 on error \n >0 if point is behind a camera
        @status tested, Daniel Grest */
    int Triangulate(PMatrix &P1, PMatrix &P2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    BIAS::Vector3<double>& point3d);

    /// deprecated
    int Triangulate(PMatrix &P1, PMatrix &P2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    BIAS::HomgPoint3D& point3d);

    /** @brief Triangulation for metric Poses (using C and R)
        @param point3d (output) guaranteed to be homogenized, inf causes error
        @return 0 on success \n <0 on error \n >0 if point is behind a camera
        @status woelk 07/2006 */
    int Triangulate(const ProjectionParametersBase *P1,
                    const ProjectionParametersBase *P2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    Vector3<double>& point3d);
    /// deprecated
    int Triangulate(const ProjectionParametersBase *P1,
                    const ProjectionParametersBase *P2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    HomgPoint3D& point3d);

    /** @brief Triangulation for metric Poses (using C and R)
        @param point3d: resulting 3D point. It may be at infinity.
        @author woelk 06/2008 (c) www.vision-n.de */
    int TriangulateProjective(const ProjectionParametersBase *P1,
                              const ProjectionParametersBase *P2,
                              const HomgPoint2D& p1, const HomgPoint2D& p2,
                              HomgPoint3D& point3d);


    /** @brief Triangulation for metric Poses (using C and R)
        @param point3d (output) guaranteed to be homogenized, inf causes error
        @return 0 on success \n <0 on error \n >0 if point is behind a camera
        @status woelk 07/2006 */
    int Triangulate(const Pose& P1, const Pose& P2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    Vector3<double>& point3d);

    /// deprecated
    int Triangulate(const Pose& P1, const Pose& P2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    HomgPoint3D& point3d);

    /** @brief Triangulation for metric Poses (using C and Q)
        @author woelk 09/2007 */
    int Triangulate(const Vector3<double>& C1, const Quaternion<double>& Q1,
                    const Vector3<double>& C2, const Quaternion<double>& Q2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    Vector3<double>& point3d);

    /// deprecated
    int Triangulate(const Vector3<double>& C1, const Quaternion<double>& Q1,
                    const Vector3<double>& C2, const Quaternion<double>& Q2,
                    const HomgPoint2D& p1, const HomgPoint2D& p2,
                    HomgPoint3D& point3d);

    /** @brief does not use BackprojectPseudoInverse but get NormRayWorldCoo

    Also calculates the minimal distance between the two rays (dist)
    and the cos of the angle between the two rays (angle) (scalarproduct)
    input: P1, P2, p1, p2
    output: point3d, dist
    @return  0  on success
    -1  if interpolated point lies behind the cameras
    -2  if lines do not intersect
    -3  if camera center not extractable from P
    (faster) @author Woelk 11 2002 */
    int Triangulate2D(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
                      HomgPoint2D& p2, HomgPoint3D& point3d, double& dist,
                      double& angle);

    /** @brief as above, but also determines the covariance matrix
        @return -4  if GetCovariance2D failed
        @author Woelk 11 2002 */
    int Triangulate2D(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
                      HomgPoint2D& p2, HomgPoint3D& point3d,
                      Matrix3x3<double>& CovMatrix, double& dist,
                      double& angle, const double &triangdeltapixel =
                      TRIANGULATION_DEFAULT_DELTAPIXEL);

    /** @brief Calculates and returns the uncertainty as normalized
        covariance matrix in 'covariance'.

        It is calculated by triangulation of all neighboring
        pixels within +- triangdeltapixel of p1 and p2 and taking the variance
        with 'point'.
        Uses Triangulate2D.
        returns 0 in case of success
        returns -4 in case of error (too little triangulations succeeded)
        @author Daniel Grest, Oct 2002
        @status tested */
    int GetCovariance2D(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
                        HomgPoint2D& p2, HomgPoint3D &point3D,
                        Matrix3x3<double>& covariance,
                        const double &triangdeltapixel =
                        TRIANGULATION_DEFAULT_DELTAPIXEL);

    /** @brief Calculates and returns the uncertainty as normalized
        covariance matrix in 'covariance'.

        It is calculated by triangulation of all neighboring
        pixels within +- triangdeltapixel of p1 and p2 and taking the variance
        with 'point'.
        Uses Triangulate.
        @author Daniel Grest, Oct 2002
        @param P1 PMatrix for image one
        @param P1 PMatrix for image two
        @param p1 Homogenized(!) point in image one
        @param p2 Homogenized(!) point in image two
        @param triangdeltapixel offset in all directions and both images
        @status tested
        @return 0 in case of success, <0 otherwise */
    int GetCovarianceAnalytic(PMatrix& P1, PMatrix& P2, HomgPoint2D& p1,
                              HomgPoint2D& p2, HomgPoint3D &point3D,
                              Matrix3x3<double>& covariance,
                              const double &triangdeltapixel =
                              TRIANGULATION_DEFAULT_DELTAPIXEL,
                              const double &sigma=
                              TRIANGULATION_DEFAULT_SIGMA);

    /** @brief returns an approximation to the 3D point p with uncertainty
        cov as derived from the 2D points p1 and p2 with uncertainties
        cov1 and cov2

        Generates randomly num 2D coordinates distributed according to cov1
        centered around p1 and num 2D coordinates distributed according to
        cov2 centered around p2. For every possible combination of these two
        2D point clouds a 3D point is triangulated resulting in a 3D point
        cloud of maximum num*num points. The mean and the covariance of this
        3D point cloud is calculated and returned in p (mean) and cov.
        The 3D point p, which is the mean of the point cloud, is generally
        *not*  aequivalent to the 3D point resulting from simple triangulation
        using p1 and p2.

        The 2D points p1 and p2 must be homogenized and are not allowed to be
        at infinity.

        !! untested !!

        @returns the number of points in 3D cloud. The covariance of the 3D
        approximation cov is only valid if *more* than 1 point was triangulated

        @author woelk 07/2005 */
    int GetCovariance(PMatrix& P1, PMatrix& P2,
                      const HomgPoint2D& p1, const Matrix2x2<double>& cov1,
                      const HomgPoint2D& p2, const Matrix2x2<double>& cov2,
                      HomgPoint3D& p, Matrix3x3<double>& cov, const int num, bool quasiEuclidean=false);


    int GetCovarianceProjective(PMatrix& P1, PMatrix& P2,
                      const HomgPoint2D& p1, const HomgPoint2DCov& cov1,
                      const HomgPoint2D& p2, const HomgPoint2DCov& cov2,
                      HomgPoint3D& p, HomgPoint3DCov& cov);

#ifdef BIAS_TRIANG_DEBUG
    void GetCovPoints(std::vector<HomgPoint3D>& pvec,
                      std::vector<double>& weight);
#endif



  protected:
    /** @brief called by constructor to set up neighbourhood weights for
        covariance computation */
    void _UpdateCovWeights(const double &TriangDeltaPixels=
                           TRIANGULATION_DEFAULT_DELTAPIXEL,
                           const double &sigma=
                           TRIANGULATION_DEFAULT_SIGMA);

    /// neighbourhood weights for covariance computation
    Matrix3x3<double>  cov_weights_;
    /// value to go up/down/left/right for "manual" covariance computation
    double _dTriangDeltaPixels;
    double _dTriangSigmaPixels;
#ifdef BIAS_TRIANG_DEBUG
    std::vector<HomgPoint3D> covpoints_;
    std::vector<double> covweights_;
#endif
  };


} // namespace


//#include <Base/Common/BIASpragmaEnd.hh>

#endif // __Triangulation_hh__