TriangulationMidPoint.hh

#ifndef __TriangulationMidPoint_hh__
#define __TriangulationMidPoint_hh__


#include <Base/Debug/Debug.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/PoseParametrization.hh>
#include <Base/Math/Vector3.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <vector>

namespace BIAS
{

  /** @class TriangulationMidPoint
      @ingroup g_MultiView

      @brief Class for triangulation of 3d points from multiple projection
             rays using the mid-point method.

             Given n projection rays (parametrized by camera center points
             C_1,...,C_n and normalized direction vectors d_1,...,d_n in
             world coordinate system), the corresponding 3d point X is
             computed by the mid-point method as the point with minimal
             mean squared distance to all rays, i.e. minimize sum of
             |C_i + l_i*d_i - X|^2 with respect to parameters X,l_1,...,l_n.

             Note that this method is neither affine nor projective
             invariant and behaves very poorly under such transformations,
             it should be used only with Euclidean transformations.

             See also R. I. Hartley, P. Sturm: Triangulation, in: Computer
             Vision and Image Understanding 68 (2), p.146--157, 1997.

      @author esquivel 09/2010
  */
  class BIASGeometry_EXPORT TriangulationMidPoint : public BIAS::Debug
  {
  public:

    TriangulationMidPoint();

    ~TriangulationMidPoint();

    /** @brief Return point X that is closest in average to given rays
        which have parametric description by point and direction.
        @attention Directions MUST BE normalized!
        @return 0 in case of no errror, <0 if errors, n>0 if intersection
        point lies not in direction of n-th ray.
    */
    inline int Intersect(const std::vector< BIAS::Vector3<double> >& pos,
                         const std::vector< BIAS::Vector3<double> >& dir,
                         BIAS::Vector3<double>& res)
    {
      double dist;
      return Intersect_(pos, dir, res, dist, false);
    };

    /** @brief Return point X that is closest in average to given rays
        which have parametric description by point and direction.
        Average distance to rays is returned in parameter dist.
        @attention Directions MUST BE normalized!
        @return 0 in case of no errror, <0 if errors, n>0 if intersection
        point lies not in direction of n-th ray.
    */
    inline int Intersect(const std::vector< BIAS::Vector3<double> >& pos,
                         const std::vector< BIAS::Vector3<double> >& dir,
                         BIAS::Vector3<double>& res, double& dist)
    {
      return Intersect_(pos, dir, res, dist, true);
    };

    /** @brief Return point X that is closest in average to given rays
        which have parametric description by point and direction.
        @attention Directions MUST BE normalized!
        @return 0 in case of no errror, <0 if errors, n>0 if intersection
        point lies not in direction of n-th ray.
    */
    inline BIAS::Vector3<double> Intersect(
        const std::vector< BIAS::Vector3<double> >& pos,
        const std::vector< BIAS::Vector3<double> >& dir)
    {
      double dist;
      BIAS::Vector3<double> res;
      if (Intersect_(pos, dir, res, dist, false) >= 0)
        return res;
      else
        return BIAS::Vector3<double>(0,0,0);
    };

    /** @brief Triangulate 3d point X from projection parameters and
               corresponding 2d image points (in pixels).
        @attention 2d image points MUST BE homogenized!
        @return 0 on success, <0 on error, n>0 if point is behind n-th camera
    */
    inline int Triangulate(const std::vector< BIAS::ProjectionParametersBase* >& cams,
                           const std::vector< BIAS::HomgPoint2D >& points2d,
                           BIAS::Vector3<double>& point3d, double& dist)
    {
      return Triangulate_(cams, points2d, point3d, dist, true);
    };

    inline int Triangulate(const std::vector< BIAS::ProjectionParametersBase* >& cams,
                           const std::vector< BIAS::HomgPoint2D >& points2d,
                           BIAS::Vector3<double>& point3d)
    {
      double dist;
      return Triangulate_(cams, points2d, point3d, dist, false);
    };

    /** @brief Triangulate 3d point X from camera poses and corresponding
               rays in local (!) camera coordinate frames ("normalized").
        @attention Directions of rays MUST BE normalized!
        @return 0 on success, <0 on error, n>0 if point is behind n-th camera
    */
    inline int Triangulate(const std::vector< BIAS::PoseParametrization >& poses,
                           const std::vector< BIAS::HomgPoint2D >& rays,
                           BIAS::Vector3<double>& point3d, double& dist)
    {
      return Triangulate_(poses, rays, point3d, dist, true);
    };

    inline int Triangulate(const std::vector< BIAS::PoseParametrization >& poses,
                           const std::vector< BIAS::HomgPoint2D >& rays,
                           BIAS::Vector3<double>& point3d)
    {
      double dist;
      return Triangulate_(poses, rays, point3d, dist, false);
    };

  protected:

    int Intersect_(const std::vector< BIAS::Vector3<double> >& pos,
                   const std::vector< BIAS::Vector3<double> >& dir,
                   BIAS::Vector3<double>& res, double& dist,
                   const bool computeDist);

    int Triangulate_(const std::vector< BIAS::ProjectionParametersBase* >& cams,
                     const std::vector< BIAS::HomgPoint2D >& points2d,
                     BIAS::Vector3<double>& point3d, double& dist,
                     const bool computeDist);

    int Triangulate_(const std::vector< BIAS::PoseParametrization >& poses,
                     const std::vector< BIAS::HomgPoint2D >& rays,
                     BIAS::Vector3<double>& point3d, double& dist,
                     const bool computeDist);

  }; // class

} // namespace

#endif // __TriangulationMidPoint_hh__