vertex_landmark.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_POINT_H_
#define VO_G2O_TYPE_VERTEX_POINT_H_

#include <g2o/core/base_vertex.h>
#include <iostream>


namespace vslam{
namespace g2o_types{
    using Eigen::Vector3d;

    template<class PoseGraphT> class VertexPointXYZ : public g2o::BaseVertex<3, Vector3d>
    {
        protected:
            int pointds_idx; 
            Points<PoseGraphT>* points; 
            bool initialized;

        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

            bool save_state; 

            VertexPointXYZ(int _points_idx, Points<PoseGraphT> & _points) : 
                    pointds_idx(_points_idx), initialized(true) {
                points=&_points;        
            }

            VertexPointXYZ() : points(NULL), initialized(false) {}

            virtual void setToOriginImpl() {
                _estimate = Vector3d::Zero();
            }


            virtual bool read(std::istream& is) { assert(false); return true; }
            virtual bool write(std::ostream& os) const { 
                os << _estimate.transpose();
                return os.good();
            }

            virtual void oplusImpl(const double* update) {
                Eigen::Map<const Vector3d> increment(update);
                _estimate += increment;
            }


            virtual ~VertexPointXYZ(){
                //if((points->getPoint(pointds_idx).global-_estimate).norm() > 1.0){
                if(save_state){
                    assert(initialized);
                    points->lmEstimateUpd(pointds_idx, _estimate);
                }
                //}
            }
    };
}
}//namespace vslam
#endif