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
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;
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
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 }
00202
00203 #endif // _PROJ_H