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_