lineTools.hpp

Go to the documentation of this file.

/* $Id: lineTools.hpp 2059 2006-10-24 06:52:21Z tlemaire $ */

#ifndef SLAM_PLUCKER_TOOLS_HPP
#define SLAM_PLUCKER_TOOLS_HPP

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "boost/tuple/tuple.hpp"
#include "slam/eulerTools.hpp"

namespace jafar {
  namespace slam {
    namespace lineTools { 
      
      template<class Mat>
      void pluckerIntrinsicMatrix(double u0, double v0, double au, double av, 
                    Mat& K);
      
      template<class Vec, class Mat>
      void pluckerIntrinsicMatrix(Vec const& k, 
                    Mat& K);
      
      
      
      void pluckerInverseIntrinsicMatrix(double u0, double v0, double au, double av, 
                         jblas::mat& iK);
      
      
      void pluckerInverseIntrinsicMatrix(jblas::vec4 const& k, 
                         jblas::mat& iK);
      
      
      
      jblas::vec2 homogeneousToRhoTheta(jblas::vec3 const& l);
      
      
      void homogeneousToRhoTheta(jblas::vec3 const& l, 
                     jblas::vec2& rt,
                     jblas::mat& RT_l) ;
      
      
      
      jblas::vec3 rhoThetaToHomogeneous(jblas::vec2 const& rt);
      
      
      void rhoThetaToHomogeneous(jblas::vec2 const& rt, 
                     jblas::vec3& l, 
                     jblas::mat&  L_rt);
      
      
      
      jblas::vec3 projectPlucker(jblas::mat33 const& K, 
                     jblas::vec6 const& L);
      
      
      void projectPlucker(jblas::mat33 const& K, 
                jblas::vec6 const& L, 
                jblas::vec3& l, 
                jblas::mat& L_l);
      
      
      
      jblas::vec3 retroProjectPluckerPlaneNormal(jblas::mat33 const& iK, 
                             jblas::vec3  const& l);
      
      
      void retroProjectPluckerPlaneNormal(jblas::mat33  const& iK, 
                        jblas::vec3   const& l,
                        jblas::vec3&  n,
                        jblas::mat33& N_l);
      
      
      jblas::vec6 retroProjectPlucker(jblas::mat33  const& iK, 
                      jblas::vec3 const& l, 
                      jblas::vec3 const& v);
      
      
      void retroProjectPlucker(jblas::mat33  const& iK, 
                   jblas::vec3 const& l, 
                   jblas::vec3 const& v, 
                   jblas::vec6& L, 
                   jblas::mat& L_l, 
                   jblas::mat& L_v);
      
      
      
      
      jblas::vec6 retroProjectPluckerFromBase(jblas::mat33 const& iK,
                          jblas::vec3 const& l,
                          jblas::vec2 const& beta);
      
      
      void retroProjectPluckerFromBase(jblas::mat33 const& iK,
                       jblas::vec3 const& l,
                       jblas::vec2 const& beta,
                       jblas::vec6& L,
                       jblas::mat&  L_l,
                       jblas::mat&  L_beta);
      
      
      
      
      jblas::mat planeVecToBaseMatrix(jblas::vec3 const& n);
      
      
      
      
      jblas::vec3 planeBaseToDirVector(jblas::vec3 const& n, 
                       jblas::vec2 const& beta);
      
      void planeBaseToDirVector(jblas::vec3 const& n, 
                    jblas::vec2 const& beta, 
                    jblas::vec3& v, 
                    jblas::mat33& V_n, 
                    jblas::mat& V_beta);
      
      void planeBaseToDirVector(jblas::vec_range const& n, 
                    jblas::vec2 const& beta, 
                    jblas::vec_range& v, 
                    jblas::mat33& V_n, 
                    jblas::mat& V_beta);
      
      
      
      jblas::vec3 normVec3(jblas::vec3 const& v);
      
      void normVec3(jblas::vec3 const& v, 
              jblas::vec3& vn, 
              jblas::mat33& VN_v);
      
      
      void normVec3(jblas::vec3 const& v, 
              jblas::vec_range& vn, 
              jblas::mat33& VN_v);
      
      
      jblas::vec3 adjustVec3(jblas::vec3 const& v,
                   jblas::vec3 const& n);
      
      void adjustVec3(jblas::vec3 const& v,
              jblas::vec3 const& n,
              jblas::vec3& w,
              jblas::mat33& W_v);
      
      
      
      jblas::vec3 pluckerOrigin(jblas::vec6 const& L);
      
      
      void pluckerOrigin(jblas::vec6 const& L, 
                 jblas::vec3& p0, 
                 jblas::mat& P0_l);
      
      
      jblas::mat33 skewSymmMat33(jblas::vec3 const& v);
      
      
      jblas::vec3 pluckerEndpoint(jblas::vec6 const& L, 
                double const s);
      
      void pluckerEndpoint(jblas::vec6 const& L, // Plucker line
               double const s, // Abscissa
               jblas::vec3& e, // Endpoint
               jblas::mat& E_l,  // Jac wrt the line
               jblas::mat& E_s); // Jac. wrt abscissa
      
      
      jblas::vec6 pluckerRay(jblas::mat33 const& K, 
                   jblas::vec2 const& p);
      
      
      void pluckerRay(jblas::mat33 const& K, 
              jblas::vec2 const& p,
              jblas::vec6 r,
              jblas::mat R_p);
      
      
      
      boost::tuple<double,double,double> pluckerSoftIntersectionAbscissa(jblas::vec6 const& line1, 
                                         jblas::vec6 const& line2);
      
      
      jblas::vec6 pluckerToEuclideanPlucker(jblas::vec6 const& L);
      
      void pluckerToEuclideanPlucker(jblas::vec6 const& L, 
                       jblas::vec6& eL, 
                       jblas::mat66& EL_l);
      
      
      
      jblas::vec3 extToHomogeneous(jblas::vec2 const& p, jblas::vec2 const& q);
      
      
      void extToHomogeneous(jblas::vec2 const& p, 
                  jblas::vec2 const& q,
                  jblas::vec3& hm,
                  jblas::mat& HM_p, 
                  jblas::mat& HM_q);
 

             void points3plane(jblas::vec3 const& a, jblas::vec3 const& b, jblas::vec3 const& c, jblas::vec6& plane);

            
      jblas::vec6 hmPointsToPlucker(jblas::vec4& A, jblas::vec4& B);
            
            void hmPointsToPlucker(jblas::vec4& A, jblas::vec4& B, jblas::vec6& L,jblas::mat& L_a,jblas::mat& L_b);

            jblas::vec4 uvdToHmPoint(jblas::vec3 const& uvd, jblas::vec4& k, double const alpha);

            void uvdToHmPoint(jblas::vec3 const& uvd, jblas::vec4& k, double const alpha, jblas::vec4& hmP, jblas::mat& HmPu);
      
        
      
      /*template <class IntrVec, class PlkVec>
        jblas::vec6 projectPluckerToStereo(IntrVec const& k,
                           double alpha, 
                           PlkVec const& L)
        {
          
          // right camera frame
          jblas::vec6 rightFrame;
          rightFrame.clear();
          rightFrame(0) = -alpha/k(2);
          
          
          jblas::vec6 LR;
          jafar::slam::EulerTools::lineToFrame(rightFrame, L, LR);
          
          jblas::mat33 K;
          pluckerIntrinsicMatrix(k, K);
          
          jblas::vec6 stLine;
          
          jblas::vec3 hm;
          
          hm = projectPlucker(K, L);
          subrange(stLine, 0, 3) = hm;
          
          hm = projectPlucker(K, LR);
          subrange(stLine, 3, 6) = hm;
          
          return stLine;
          
        }*/
      jblas::vec6 projectPluckerToStereo(jblas::vec4 const& k,
                         double alpha, 
                         jblas::vec6 const& L);
      
      
      
      
      //template <class StereoLineObs, class IntrVec, class PlkVec, class JacMat>
      //  void projectPluckerToStereo(IntrVec const&  k, 
      //                double          alpha,
      //                PlkVec const&   L,
      //                StereoLineObs&  stLine,
      //                JacMat&         StLine_l);
      void projectPluckerToStereo(jblas::vec4 const&   k, 
                    double               alpha,
                    jblas::vec6 const&   L,
                    jblas::vec6&         stLine,
                    jblas::mat66&        StLine_l);
      
      
      jblas::vec2 extToRhoTheta(const jblas::vec2& ext1, const jblas::vec2& ext2);
      
      void extToRhoTheta(const jblas::vec2& ext1, const jblas::vec2& ext2, jblas::vec2& rt, jblas::mat& RT_ee);     
      
      
      
      jblas::vec4 uvdsToStereoRhoTheta(jblas::vec6 const& uvduvd);
      
      void uvdsToStereoRhoTheta(jblas::vec6 const& uvduvd, jblas::vec4& rtrt, jblas::mat& RR_uu);
      
      void uvdsToStereoRhoTheta(jblas::vec6 const& uvduvd, jblas::mat& RR_uu);
      
      void homogenousToExtDistances( const jblas::vec3& _hm, const jblas::vec2& ext1, const jblas::vec2& ext2, jblas::vec2& dd, jblas::mat& DD_hm, jblas::mat& DD_ext1, jblas::mat& DD_ext2 );
      
      void homogenousToPointDistance( const jblas::vec3& _hm, const jblas::vec2& _ext, double& d, jblas::mat_range& D_hm, jblas::mat_range& D_ext );
      
      void homogenousToExtDistances( const jblas::vec3& _hm, const jblas::vec2& ext1, const jblas::vec2& ext2, jblas::vec2& dd, jblas::mat& DD_hm);
      
      void homogenousToPointDistance( const jblas::vec3& _hm, const jblas::vec2& _ext, double& d, jblas::mat_range& D_hm );

      jblas::vec6 points3DToPointVecLine(const jblas::vec3& p, const jblas::vec3& q);

      void points3DToPointVecLine(const jblas::vec3& p, const jblas::vec3& q, jblas::vec6& m, jblas::mat66& M_l);

      jblas::vec2 intersectPointVecLines(const jblas::vec6& l1, const jblas::vec6& l2);

      double cosVecsAngle(const jblas::vec3& v1, const jblas::vec3& v2);

    } 
  }
  
}

#endif