node.h
Go to the documentation of this file.
00001 #ifndef _NODE_H_
00002 #define _NODE_H_
00003 
00004 // Functions for handling nodes (cameras) within the sba system.
00005 
00006 #include <stdio.h>
00007 #include <iostream>
00008 #include <Eigen/Core>
00009 #include <Eigen/Geometry>
00010 
00011 // needed for camera params
00012 #ifndef _CAMPARAMS_H_
00013 #define _CAMPARAMS_H_
00014 
00015 namespace frame_common
00016 {
00017 
00021   struct CamParams
00022   {
00023     double fx, fy, cx, cy, tx;
00024   };
00025 } // end namespace
00026 
00027 #endif // _CAMPARMS_H_
00028 
00029 namespace fc = frame_common;
00030 
00031 // put things into a namespace
00032 namespace sba
00033 {
00034 
00037   typedef Eigen::Vector3d Keypoint;
00038 
00039 
00043   typedef Eigen::Vector4d Point;
00044 
00045 
00063   class Node
00064   {
00065   public:
00066     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00067 
00068     // 6DOF pose as a unit quaternion and translation vector
00069     Eigen::Matrix<double,4,1> trans; 
00070     Eigen::Quaternion<double> qrot;  
00071     
00074     void normRot();
00075     
00077     void normRotLocal();
00078 
00080     Eigen::Matrix<double,3,4> w2n; 
00081     
00083     void setTransform();
00084 
00085     // Covariance matrix, 6x6.  Variables are [trans,rot], with the
00086     // rotational part being the xyz parameters of the unit
00087     // quaternion
00088     //    Eigen::Matrix<double,6,6> covar;
00089 
00092     Eigen::Matrix<double,3,3> Kcam; 
00093     double baseline;                    
00094     
00095     
00098     void setKcam(const fc::CamParams &cp)
00099     {
00100       Kcam.setZero();
00101       Kcam(0,0) = cp.fx;
00102       Kcam(1,1) = cp.fy;
00103       Kcam(0,2) = cp.cx;
00104       Kcam(1,2) = cp.cy;
00105       Kcam(2,2) = 1.0;
00106       baseline = cp.tx;
00107       setProjection();
00108     }
00109 
00111     Eigen::Matrix<double,3,4> w2i;
00112     
00116     void project2im(Eigen::Vector2d &pi, const Point &p) const
00117           { Eigen::Vector3d p1 = w2i * p; pi = p1.head(2)/p1(2); }
00118 
00121     void setProjection()
00122           { w2i = Kcam * w2n; }
00123 
00126     Eigen::Matrix<double,3,3> dRdx, dRdy, dRdz;
00127     
00129     static Eigen::Matrix3d dRidx, dRidy, dRidz; // these are constant
00130     
00132     static void initDr();       // call this to set up constant matrices
00133 
00135     void setDr(bool local = false);
00136     
00138     void setDri();
00139 
00141     bool isFixed;
00142 
00144     Eigen::Matrix<double,4,1> oldtrans;
00145     
00147     Eigen::Quaternion<double> oldqrot;
00148     
00150     void projectMono(const Point& pt, Eigen::Vector3d& proj);
00151     
00153     void projectStereo(const Point& pt, Eigen::Vector3d& proj);
00154   };
00155   
00156   
00157   // Functions to create transformations
00158 
00164   void transformW2F(Eigen::Matrix<double,3,4> &m, 
00165                     const Eigen::Matrix<double,4,1> &trans, 
00166                     const Eigen::Quaternion<double> &qrot);
00167 
00168 
00171   void transformF2W(Eigen::Matrix<double,3,4> &m, 
00172                     const Eigen::Matrix<double,4,1> &trans, 
00173                     const Eigen::Quaternion<double> &qrot);
00174 
00175 
00178   void transformN2N(Eigen::Matrix<double,4,1> &trans, 
00179                     Eigen::Quaternion<double> &qrot,
00180                     Node &nd0, Node &nd1);
00181 
00182 } // sba namespace
00183 
00184 #endif // _NODE_H_


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Aug 24 2016 03:37:37