vertex_pose.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VO_G2O_TYPE_VERTEX_POSE_H_
#define VO_G2O_TYPE_VERTEX_POSE_H_


#include <Eigen/Dense>
#include <g2o/core/base_vertex.h>
#include "se3_for_g2o.h"


namespace vslam{

namespace g2o_types{

    template<class PoseGraphT> class VertexSE3 : public g2o::BaseVertex<6, SE3G2O> {
        protected:
            typedef typename graph_traits<PoseGraphT>::vertex_descriptor VertexD;
            PoseGraphT * g;
            VertexD vd;      
            bool initialized;

        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

            bool save_state; 


            VertexSE3(PoseGraphT& _g, VertexD _vd) : vd(_vd), initialized(true) {
                g=&_g;
                save_state=true;
            }

            VertexSE3() : g(NULL), initialized(false) {}

            virtual void setToOriginImpl() {
                _estimate = SE3G2O();
            }


            virtual bool read(std::istream& is) { assert(false); return true; }

            virtual bool write(std::ostream& os) const { 
                Vector6d v = _estimate.toVector();
                os << v.transpose();
                return os.good();
            };

            virtual void oplusImpl(const double* update)
            {
                Eigen::Map<const Vector6d> v(update);
                SE3G2O increment = SE3G2O::exp(v);
                _estimate = increment * _estimate;
            }

            virtual ~VertexSE3(){
                if(save_state){
                    assert(initialized);
                    get(vertex_transform, *g, vd) = _estimate;
                }
            }
    };
}//namespace g2o_types
}//namespace vslam


#endif