proj.h
Go to the documentation of this file.
00001 #ifndef _PROJ_H_
00002 #define _PROJ_H_
00003 
00004 #ifndef EIGEN_USE_NEW_STDVECTOR
00005 #define EIGEN_USE_NEW_STDVECTOR
00006 #endif // EIGEN_USE_NEW_STDVECTOR
00007 
00008 #include <Eigen/Core>
00009 #include <Eigen/Geometry>
00010 #include <Eigen/LU>
00011 #include <Eigen/StdVector>
00012 #include <map>
00013 
00014 #include <sparse_bundle_adjustment/node.h>
00015 
00016 namespace sba
00017 {
00018   class JacobProds
00019   {
00020   public:
00021     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00022 
00023     JacobProds() 
00024     {
00025       Hpp.setZero();
00026       Hpc.setZero();
00027       Hcc.setZero();
00028       Bp.setZero();
00029       JcTE.setZero();
00030     }
00031 
00033     Eigen::Matrix<double,3,3> Hpp;
00034       
00036     Eigen::Matrix<double,3,6> Hpc;
00037       
00039     Eigen::Matrix<double,6,6> Hcc;
00040       
00042     Eigen::Matrix<double,3,1> Bp;
00043       
00045     Eigen::Matrix<double,6,1> JcTE;
00046   };
00047 
00048   class Proj; // Forward reference.
00049   
00052   typedef std::map<const int, Proj, std::less<int>, Eigen::aligned_allocator<Proj> > ProjMap;
00053 
00057   class Proj
00058   {
00059     public:
00060 
00061       EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00062 
00066       Proj(int ci, Eigen::Vector3d &q, bool stereo = true);
00067       
00071       Proj(int ci, Eigen::Vector2d &q);
00072       
00075       Proj();
00076       
00078       int ndi;
00079       
00081       Eigen::Vector3d kp;
00082       
00084       Eigen::Vector3d err;
00085       
00087       bool stereo;
00088       
00090      double calcErr(const Node &nd, const Point &pt, double huber = 0.0);
00091       
00094       double getErrSquaredNorm();
00095       
00098       double getErrNorm();
00099       
00101 
00117       void setJacobians(const Node &nd, const Point &pt, JacobProds *jpp);
00118       
00120       JacobProds *jp;
00121       
00124       Eigen::Matrix<double,6,3> Tpc;
00125       
00127       bool isValid;
00128       
00131       const static double qScale = 1.0;
00132       
00134       bool useCovar;
00135       
00137       Eigen::Matrix<double,3,3> covarmat;
00138       
00140       bool pointPlane;
00141       
00143       Eigen::Vector3d plane_normal;
00144       
00146       Eigen::Vector3d plane_point;
00147       
00149       int plane_point_index;
00150       
00152       int plane_node_index;
00153       
00155       Eigen::Vector3d plane_local_normal;
00156 
00162       void setCovariance(const Eigen::Matrix3d &covar);
00163       
00165       void clearCovariance();
00166       
00167       
00168     protected:
00170       void setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp);
00171       
00173       void setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp);
00174       
00176       double calcErrMono_(const Node &nd, const Point &pt, double huber);
00177       
00179       double calcErrStereo_(const Node &nd, const Point &pt, double huber);
00180   };
00181     
00182   class Track
00183   {
00184     public:
00186       Track(Point p);
00187       
00189       Track();
00190       
00193       ProjMap projections;
00194       
00197       Point point;
00198   };
00199   
00200   
00201 } // sba
00202 
00203 #endif // _PROJ_H


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