proj.h

Go to the documentation of this file.

#ifndef _PROJ_H_
#define _PROJ_H_

#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif // EIGEN_USE_NEW_STDVECTOR

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/StdVector>
#include <map>

#include <sba/node.h>

namespace sba
{
  class JacobProds
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment

    JacobProds() 
    {
      Hpp.setZero();
      Hpc.setZero();
      Hcc.setZero();
      Bp.setZero();
      JcTE.setZero();
    }

    Eigen::Matrix<double,3,3> Hpp;
      
    Eigen::Matrix<double,3,6> Hpc;
      
    Eigen::Matrix<double,6,6> Hcc;
      
    Eigen::Matrix<double,3,1> Bp;
      
    Eigen::Matrix<double,6,1> JcTE;
  };

  class Proj; // Forward reference.
  
  typedef std::map<const int, Proj, std::less<int>, Eigen::aligned_allocator<Proj> > ProjMap;

  class Proj
  {
    public:

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment

      Proj(int ci, Eigen::Vector3d &q, bool stereo = true);
      
      Proj(int ci, Eigen::Vector2d &q);
      
      Proj();
      
      int ndi;
      
      Eigen::Vector3d kp;
      
      Eigen::Vector3d err;
      
      bool stereo;
      
     double calcErr(const Node &nd, const Point &pt, double huber = 0.0);
      
      double getErrSquaredNorm();
      
      double getErrNorm();
      

      void setJacobians(const Node &nd, const Point &pt, JacobProds *jpp);
      
      JacobProds *jp;
      
      Eigen::Matrix<double,6,3> Tpc;
      
      bool isValid;
      
      const static double qScale = 1.0;
      
      bool useCovar;
      
      Eigen::Matrix<double,3,3> covarmat;
      
      bool pointPlane;
      
      Eigen::Vector3d plane_normal;
      
      Eigen::Vector3d plane_point;
      
      int plane_point_index;
      
      int plane_node_index;
      
      Eigen::Vector3d plane_local_normal;

      void setCovariance(const Eigen::Matrix3d &covar);
      
      void clearCovariance();
      
      
    protected:
      void setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp);
      
      void setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp);
      
      double calcErrMono_(const Node &nd, const Point &pt, double huber);
      
      double calcErrStereo_(const Node &nd, const Point &pt, double huber);
  };
    
  class Track
  {
    public:
      Track(Point p);
      
      Track();
      
      ProjMap projections;
      
      Point point;
  };
  
  
} // sba

#endif // _PROJ_H