node.h

Go to the documentation of this file.

#ifndef _NODE_H_
#define _NODE_H_
 
// Functions for handling nodes (cameras) within the sba system.
 
#include <stdio.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
// needed for camera params
#ifndef _CAMPARAMS_H_
#define _CAMPARAMS_H_
 
namespace frame_common
{
 
  struct CamParams
  {
    double fx, fy, cx, cy, tx;
  };
} // end namespace
 
#endif // _CAMPARMS_H_
 
namespace fc = frame_common;
 
// put things into a namespace
namespace sba
{
 
  typedef Eigen::Vector3d Keypoint;
 
 
  typedef Eigen::Vector4d Point;
 
 
  class Node
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
 
    // 6DOF pose as a unit quaternion and translation vector
    Eigen::Matrix<double,4,1> trans; 
    Eigen::Quaternion<double> qrot;  
    
    void normRot();
    
    void normRotLocal();
 
    Eigen::Matrix<double,3,4> w2n; 
    
    void setTransform();
 
    // Covariance matrix, 6x6.  Variables are [trans,rot], with the
    // rotational part being the xyz parameters of the unit
    // quaternion
    //    Eigen::Matrix<double,6,6> covar;
 
    Eigen::Matrix<double,3,3> Kcam; 
    double baseline;                    
    
    
    void setKcam(const fc::CamParams &cp)
    {
      Kcam.setZero();
      Kcam(0,0) = cp.fx;
      Kcam(1,1) = cp.fy;
      Kcam(0,2) = cp.cx;
      Kcam(1,2) = cp.cy;
      Kcam(2,2) = 1.0;
      baseline = cp.tx;
      setProjection();
    }
 
    Eigen::Matrix<double,3,4> w2i;
    
    void project2im(Eigen::Vector2d &pi, const Point &p) const
          { Eigen::Vector3d p1 = w2i * p; pi = p1.head(2)/p1(2); }
 
    void setProjection()
          { w2i = Kcam * w2n; }
 
    Eigen::Matrix<double,3,3> dRdx, dRdy, dRdz;
    
    static Eigen::Matrix3d dRidx, dRidy, dRidz; // these are constant
    
    static void initDr();       // call this to set up constant matrices
 
    void setDr(bool local = false);
    
    void setDri();
 
    bool isFixed;
 
    Eigen::Matrix<double,4,1> oldtrans;
    
    Eigen::Quaternion<double> oldqrot;
    
    void projectMono(const Point& pt, Eigen::Vector3d& proj);
    
    void projectStereo(const Point& pt, Eigen::Vector3d& proj);
  };
  
  
  // Functions to create transformations
 
  void transformW2F(Eigen::Matrix<double,3,4> &m, 
                    const Eigen::Matrix<double,4,1> &trans, 
                    const Eigen::Quaternion<double> &qrot);
 
 
  void transformF2W(Eigen::Matrix<double,3,4> &m, 
                    const Eigen::Matrix<double,4,1> &trans, 
                    const Eigen::Quaternion<double> &qrot);
 
 
  void transformN2N(Eigen::Matrix<double,4,1> &trans, 
                    Eigen::Quaternion<double> &qrot,
                    Node &nd0, Node &nd1);
 
} // sba namespace
 
#endif // _NODE_H_