00001 //part of the source code of master thesis, source code produced by Jiri Divis 00008 #ifndef VO_G2O_TYPE_VERTEX_POSE_POINT_H_ 00009 #define VO_G2O_TYPE_VERTEX_POSE_POINT_H_ 00010 00011 #include <iostream> 00012 #include <g2o/core/base_binary_edge.h> 00013 #include "vertex_pose.h" 00014 #include "vertex_landmark.h" 00015 #include "../error_func.h" 00016 00017 00018 namespace vslam{ 00019 namespace g2o_types { 00020 using Eigen::Vector2d; 00021 00023 00029 template<class PoseGraphT> class EdgeSE3PointXYZ : public 00030 g2o::BaseBinaryEdge<2, Vector2d, VertexSE3<PoseGraphT>, VertexPointXYZ<PoseGraphT> > { 00031 protected: 00033 LandmarkToPoseReprojectionError reproj_error_func; 00034 00035 public: 00036 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 00037 00038 void computeError() { 00039 const VertexSE3<PoseGraphT>* pose = dynamic_cast<VertexSE3<PoseGraphT>*>(this->vertex(0)); 00040 const VertexPointXYZ<PoseGraphT>* feature = 00041 dynamic_cast<VertexPointXYZ<PoseGraphT>*>(this->vertex(1)); 00042 Vector2d proj, wh; 00043 proj = PanoImCoord::fromHomogenous(pose->estimate()*feature->estimate()).getPixel(); 00044 this->_error=reproj_error_func(feature->estimate(), 00045 LandmarkObservation(pose->estimate().inverse(), this->measurement())); 00046 } 00047 00052 00053 virtual bool read(std::istream& is) { assert(false); return true;}; 00054 virtual bool write(std::ostream& os) const{ 00055 os << this->measurement()[0] << " " << this->measurement()[1] << " "; 00056 return os.good(); 00057 } 00059 00060 }; 00061 } //namespace g2o_types 00062 } //namespace vslam 00063 #endif
