vo.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_VO_H_INCLUDED
#define VSLAM_VO_H_INCLUDED
#include "utils.h"
#include "pose_graph.h"
#include "vis.h"
#include "data_dump.h"
#include "landmark_manager.h"

#include <vector>
#include <deque>
#include <string>
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <ros/console.h>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include "graph_opt.h"

#include "yaml-cpp/yaml.h"

namespace vslam{

template<class PoseGraphT> class VodomLogger{
    public:
    virtual void doLogging(const PoseGraphT& graph) = 0;
};


template<class PoseGraphT>
class DummyVodomLogger : public VodomLogger<PoseGraphT>{
    public:
    virtual void doLogging(const PoseGraphT& graph){ }
};

template<class PoseGraphT>
class VertexDebugInfoLogger : public VodomLogger<PoseGraphT>{
    public:
        virtual void doLogging(const PoseGraphT& graph){
            YAML::Emitter yaml_out; 
            typedef typename boost::graph_traits<PoseGraphT>::vertex_iterator VIT;
            VIT vb, ve;
            tie(vb, ve) = vertices(graph);
            yaml_out << YAML::BeginSeq;
            for(;vb!=ve; vb++){
                //if(adjacent_vertices(*vb, graph).first != adjacent_vertices(*vb, graph).second){
                    yaml_out << YAML::BeginSeq;
                    yaml_out << get(vertex_timestamp, graph, *vb);
                    yaml_out << get(vertex_debug_info, graph, *vb).double_propmap;
                    yaml_out << YAML::EndSeq;
                //}
            }
            yaml_out << YAML::EndSeq;

            boost::filesystem::path p(debug_output_path.native());
            p /= "vertex_debug_info.yaml";
            std::ofstream f(p.native().c_str());
            f << yaml_out.c_str();
            f.close();
        }
};


template<template<class> class NodeBuilderTT, class PoseGraphT> class VisualOdometry{
    protected:
        shared_ptr<NodeBuilderTT<PoseGraphT> > nbuilder;
        shared_ptr<EdgeBuilder<PoseGraphT> > ebuilder;
        PoseGraphT graph;          
        Points<PoseGraphT> points; 

        typedef typename graph_traits<PoseGraphT>::vertex_descriptor VertexD;
        VertexD vd;  
        VertexD pvd; 
        std::deque<VertexD> past_vertices; 
        size_t target_past_verticies_size;

        YAML::Emitter yaml_out; 
        shared_ptr<VodomLogger<PoseGraphT> > logger;

    public:
        VisualOdometry(
                shared_ptr<NodeBuilderTT<PoseGraphT> > _nbuilder, 
                shared_ptr<EdgeBuilder<PoseGraphT> > _ebuilder,
                size_t _target_past_verticies_size = 2,
                shared_ptr<VodomLogger<PoseGraphT> > _logger= shared_ptr<VodomLogger<PoseGraphT> >()) :
                    nbuilder(_nbuilder), 
                    ebuilder(_ebuilder), 
                    target_past_verticies_size(_target_past_verticies_size) {
            assert(_target_past_verticies_size > 0 && _target_past_verticies_size < 10);
            logger = _logger;
            if(!logger) logger.reset(new DummyVodomLogger<PoseGraphT>());
        }

        shared_ptr<SE3> processPose( 
                typename NodeBuilderTT<PoseGraphT>::InputType input, 
                int pose_id, 
                uint64_t pose_timestamp, 
                std::deque<SE3>& past_poses, 
                bool debug_images=false, 
                bool trajectory_file=false){
            ros::WallTime all_start_time = ros::WallTime::now();
            shared_ptr<VertexD> vdp = this->processPose0(
                    input, pose_id, pose_timestamp, debug_images, trajectory_file);
            if(vdp){
                past_vertices.push_front(*vdp);
                while(past_vertices.size() > target_past_verticies_size){
                    past_vertices.pop_back();
                }
                past_poses.clear();
                BOOST_FOREACH(VertexD v, past_vertices){
                    past_poses.push_back(SE3(get(vertex_transform, this->graph, v)));
                }
                Vector3d t1, t2;
                SE3 T_w_vd=get(vertex_debug_info, graph, past_vertices.front()).gt_T_wc;
                SE3 T_w_pvd=get(vertex_debug_info, graph, past_vertices.back()).gt_T_wc;
                SE3 T_pvd_vd = T_w_vd * T_w_pvd.inverse();
                get(vertex_debug_info, graph, vd).double_propmap["pose_tr_diff_gt"] = 
                        T_pvd_vd.translation().norm();

                SE3 T_w_vd2=get(vertex_transform, graph, past_vertices.front());
                SE3 T_w_pvd2=get(vertex_transform, graph, past_vertices.back());
                SE3 T_pvd_vd2 = T_w_vd2 * T_w_pvd2.inverse();
                get(vertex_debug_info, graph, vd).double_propmap["pose_tr_diff"] = 
                        T_pvd_vd2.translation().norm();

                ros::WallDuration all_duration = ros::WallTime::now() - all_start_time;
                get(vertex_debug_info, this->graph, 
                        *vdp).double_propmap["timings_all_d"]= all_duration.toSec();
                logger->doLogging(graph);
                return shared_ptr<SE3>(new SE3(get(vertex_transform, this->graph, *vdp)));
            } else{
                return shared_ptr<SE3>();
            }
        }

        shared_ptr<SE3> processPose( 
                typename NodeBuilderTT<PoseGraphT>::InputType input, 
                int pose_id, 
                uint64_t pose_timestamp, 
                bool debug_images=false, 
                bool trajectory_file=false){
            std::deque<SE3> past_poses; 
            return processPose(
                    input, pose_id, pose_timestamp, past_poses, debug_images, trajectory_file);
        }

        virtual shared_ptr<VertexD> processPose0(
                typename NodeBuilderTT<PoseGraphT>::InputType input, 
                int pose_id, 
                uint64_t pose_timestamp, 
                bool debug_images=false, 
                bool trajectory_file=false) = 0;


        PoseGraphT& getPoseGraph(){
            return graph;
        }


        virtual pair<pair<VertexD, VertexD>, bool> getLastEdge(){
            return make_pair(make_pair(pvd, vd), true);
        }

    protected:
        static const int MINSEQ = 1000000;

        template<class T> std::string viewIDString(T view, T from, T to){
            std::ostringstream s;
            s << MINSEQ + view << "_view_" << 
                 MINSEQ + from << "to" << 
                 MINSEQ + to << ".jpg"; 
            return s.str();
        }

        void pose2dVisualization(
                std::string prefix,
                typename graph_traits<PoseGraphT>::vertex_descriptor vd1, 
                typename graph_traits<PoseGraphT>::vertex_descriptor vd2, 
                typename NodeBuilderTT<PoseGraphT>::InputType input_vd1,
                typename NodeBuilderTT<PoseGraphT>::InputType input_vd2){
            //SE3 T_vd1vd2 = get(edge_transform, graph, edge(vd1, vd2, graph).first);
            SE3 T_vd1vd2 = getEdgeTransform(graph, vd1, vd2);
            ImageMatchVec matches = computeMatches(vd1, vd2, graph);
            
            cv::Mat image_vd1 = nbuilder->getPoseImage(input_vd1);
            cv::Mat image_vd2 = nbuilder->getPoseImage(input_vd2);
            cv::Mat image_vd1_wkpts = image_vd1.clone();
            cv::Mat image_vd2_wkpts = image_vd2.clone();
            drawMatchedNodeKeypoints(image_vd2, vd2, vd1, graph, image_vd2_wkpts);
            drawMatchedNodeKeypoints(image_vd1, vd1, vd2, graph, image_vd1_wkpts);


            using boost::filesystem::path;
            using boost::lexical_cast;
            {
                cv::Mat kpts_image;
                drawNodeKeypoints(vd2, graph, image_vd2, kpts_image);
                path p(debug_output_path.native());
                p /= prefix;
                p /= "kpts";
                create_directories(p);
                p /= lexical_cast<std::string>(MINSEQ + get(vertex_seq, graph, vd2)) + ".jpg"; 
                cv::imwrite(p.native(), kpts_image);
            }
            
            cv::Mat image_vd1_epilines = image_vd1_wkpts.clone();
            cv::Mat image_vd2_epilines = image_vd2_wkpts.clone();
            drawRaysOnEpipolarPlane(sE3ToE(T_vd1vd2), matches, image_vd1_epilines, 
                    image_vd2_epilines);
            drawLandmarkReprojections(image_vd2_epilines, vd2, graph, points);
            drawLandmarkReprojections(image_vd1_epilines, vd1, graph, points);
            {
                path p(debug_output_path.native());
                p /= prefix;
                p /= "epilines";
                create_directories(p);
                p /= viewIDString( get(vertex_seq, graph, vd2), get(vertex_seq, graph, vd1), 
                        get(vertex_seq, graph, vd2));
                cv::imwrite(p.native(), image_vd2_epilines);

                p.remove_filename();
                p /= viewIDString( get(vertex_seq, graph, vd1), get(vertex_seq, graph, vd2), 
                        get(vertex_seq, graph, vd1));
                cv::imwrite(p.native(), image_vd1_epilines);
            }
            {
                path p(debug_output_path.native());
                p /= prefix;
                p /= "matches_only";
                create_directories(p);
                cv::Mat image_vd1_matches = image_vd1.clone();
                cv::Mat image_vd2_matches = image_vd2.clone();
                drawMatches3(image_vd1_matches, vd1, image_vd2_matches, vd2, graph, points);
                p /= viewIDString( get(vertex_seq, graph, vd1), get(vertex_seq, graph, vd2), 
                        get(vertex_seq, graph, vd1));
                cv::imwrite(p.native(), image_vd1_matches);
                p.remove_filename();
                p /= viewIDString( get(vertex_seq, graph, vd2), get(vertex_seq, graph, vd1), 
                        get(vertex_seq, graph, vd2));
                cv::imwrite(p.native(), image_vd2_matches);
            }
            drawMatches3(image_vd1_wkpts, vd1, image_vd2_wkpts, vd2, graph, points);
            {
                path p(debug_output_path.native());
                p /= prefix;
                p /= "matches_plus_kpts";
                create_directories(p);
                p /= viewIDString( get(vertex_seq, graph, vd1), get(vertex_seq, graph, vd2), 
                        get(vertex_seq, graph, vd1));
                cv::imwrite(p.native(), image_vd1);

                p.remove_filename();
                p /= viewIDString( get(vertex_seq, graph, vd2), get(vertex_seq, graph, vd1), 
                        get(vertex_seq, graph, vd2));
                cv::imwrite(p.native(), image_vd2);
            }


        }
};



#define  VERTEX_WINDOW_LEN 4

template<template<class> class NodeBuilderTT, class PoseGraphT> 
class KeyframeVisualOdometry : public VisualOdometry<NodeBuilderTT, PoseGraphT> {
    protected:
        typedef typename graph_traits<PoseGraphT>::vertex_descriptor VertexD;

        bool initial_pose_registred;
        bool has_pvd;
        VertexD current_keyframe;
        vector<VertexD> window_vertices;
        int window_idx;
        int total_vertex_count;

        typename NodeBuilderTT<PoseGraphT>::InputType vd_image, pvd_image, keyframe_image;

        G2ORunner2<PoseGraphT> opt;
    public:
        KeyframeVisualOdometry(
                shared_ptr<NodeBuilderTT<PoseGraphT> > _nbuilder, 
                shared_ptr<EdgeBuilder<PoseGraphT> > _ebuilder,
                size_t _target_past_verticies_size = 1,
                shared_ptr<VodomLogger<PoseGraphT> > _logger= shared_ptr<VodomLogger<PoseGraphT> >()) :
                VisualOdometry<NodeBuilderTT, PoseGraphT>(
                    _nbuilder, _ebuilder, _target_past_verticies_size, _logger), 
                initial_pose_registred(false), has_pvd(false), window_vertices(VERTEX_WINDOW_LEN), 
                window_idx(-1), total_vertex_count(0) { }
        
        virtual bool buildNewKeyframe() = 0;
        
        virtual void newKeyframeBuilt() = 0;

        virtual shared_ptr<VertexD> processPose0(
                typename NodeBuilderTT<PoseGraphT>::InputType input,
                int pose_id, 
                uint64_t pose_timestamp, 
                bool debug_images=false, 
                bool trajectory_file=false){
            VertexD new_pose;
            this->nbuilder->build(input, pose_id, pose_timestamp, new_pose, this->graph);
            get(vertex_keyframe_flag, this->graph, new_pose) = false;

            if(this->initial_pose_registred){// do we have at least one pose in the pose-graph?
                if(!this->ebuilder->build(current_keyframe, new_pose, this->points, this->graph)){
                    this->nbuilder->demolish(this->graph, this->points, new_pose);
                } else{
                    total_vertex_count++;
                    window_idx=(window_idx+1) % VERTEX_WINDOW_LEN;
                    if(total_vertex_count>VERTEX_WINDOW_LEN){
                        if(!get(vertex_keyframe_flag, this->graph, window_vertices[window_idx])){
                            this->nbuilder->demolish(
                                    this->graph, this->points, window_vertices[window_idx]);
                        }
                    }
                    window_vertices[window_idx]=new_pose;
                    this->points.activateLandmarks(this->graph, new_pose, 50);//XXX
                    this->pvd=this->vd;
                    this->vd=new_pose;
                    pvd_image=vd_image;
                    vd_image = input;

                    vector<vector<int> > all_result, kf_result;
                    landmarkObservationsBySize(
                            this->graph, this->points, this->vd, all_result,  kf_result);
                    get(vertex_debug_info, this->graph, this->vd).double_propmap["lmlen_eq1"]=
                            kf_result[1].size();
                    get(vertex_debug_info, this->graph, this->vd).double_propmap["lmlen_ge3"]=
                            all_result[3].size();
                    for(int i=1; i< (int)all_result.size(); i++){
                        ROS_INFO_STREAM_NAMED("PoseGraph", "# of tracks of length " << 
                                i << "+, kf-only:" << kf_result[i].size()); 
                        ROS_INFO_STREAM_NAMED("PoseGraph", "# of tracks of length " << i 
                                << "+, all:" << all_result[i].size()); 
                    }
                    if(total_vertex_count >= 5){
                        G2OConstActiveDistanceBuilder<PoseGraphT> builder(1000000, 8.0, 3);
                        opt.doOptimalization(this->graph, this->points, builder, this->vd);
                    }
                    get(vertex_debug_info, this->graph, this->vd).double_propmap["max_lm_im_distance"]=
                            maxFeatureDistance(this->graph, this->points, this->vd);
                    this->pose2dVisualization(
                        "all", current_keyframe, this->vd, pvd_image, vd_image);
                    if(this->buildNewKeyframe()){
                        this->pose2dVisualization(
                            "kf", current_keyframe, this->vd, keyframe_image, vd_image);
                        this->points.activateLandmarks(this->graph, current_keyframe, 100);//XXX
                        current_keyframe=this->vd;
                        keyframe_image=vd_image;
                        get(vertex_keyframe_flag, this->graph, current_keyframe) = true;
                        this->newKeyframeBuilt();
                    }
                }
            } else{
                total_vertex_count++;
                window_idx=(window_idx+1) % VERTEX_WINDOW_LEN;
                this->vd=new_pose;
                vd_image = input;
                window_idx=0;
                window_vertices[0]=new_pose;

                this->initial_pose_registred=true;
                current_keyframe=this->vd;
                keyframe_image=vd_image;
                get(vertex_keyframe_flag, this->graph, current_keyframe) = true;
            }
            if(trajectory_file) drawGraph(this->graph);
            return shared_ptr<VertexD>(new VertexD(this->vd));
        }
};


template<template<class> class NodeBuilderTT, class PoseGraphT> 
class SimpleVisualOdometry : public KeyframeVisualOdometry<NodeBuilderTT, PoseGraphT> {

    public:
        SimpleVisualOdometry(
                shared_ptr<NodeBuilderTT<PoseGraphT> > _nbuilder, 
                shared_ptr<EdgeBuilder<PoseGraphT> > _ebuilder,
                size_t _target_past_verticies_size,
                shared_ptr<VodomLogger<PoseGraphT> > _logger= shared_ptr<VodomLogger<PoseGraphT> >()) :
                KeyframeVisualOdometry<NodeBuilderTT, PoseGraphT>(
                    _nbuilder, _ebuilder, _target_past_verticies_size, _logger) {}

        virtual bool buildNewKeyframe() {return true;}
        virtual void newKeyframeBuilt() {};
};



template<template<class> class NodeBuilderTT, class PoseGraphT> 
class CovisibilityKeyframeVisualOdometry : public KeyframeVisualOdometry<NodeBuilderTT, PoseGraphT> {
    protected:
        typedef typename graph_traits<PoseGraphT>::vertex_descriptor VertexD;

        double minimum; 
        double ctreshold; 

    public:

        CovisibilityKeyframeVisualOdometry(
                shared_ptr<NodeBuilderTT<PoseGraphT> > _nbuilder, 
                shared_ptr<EdgeBuilder<PoseGraphT> > _ebuilder,
                double _ctreshold = 0.8,
                size_t _target_past_verticies_size = 1,
                shared_ptr<VodomLogger<PoseGraphT> > _logger= shared_ptr<VodomLogger<PoseGraphT> >()) :
                KeyframeVisualOdometry<NodeBuilderTT, PoseGraphT>(_nbuilder, _ebuilder, 
                        _target_past_verticies_size, _logger),
                ctreshold(_ctreshold){
            newKeyframeBuilt();
        }

        int getEdgeCovisibility(VertexD s, VertexD t){
            return get(edge_kp_idx_map, this->graph, boost::edge(s, t, this->graph).first).size();
        }

        //* @see ctreshold*/
        virtual bool buildNewKeyframe(){
            int covisibility = getEdgeCovisibility(this->current_keyframe, this->vd);
            ROS_INFO_STREAM_NAMED("VisualOdometry", "Covisibility: " << covisibility);
            ROS_INFO_STREAM_NAMED("VisualOdometry", "Cov. Treshold: " << ctreshold*minimum);
            bool result;
            if(minimum<0 || covisibility > minimum){
                minimum=covisibility;
                result=false;
            } else{
                if(covisibility < ctreshold*minimum) result = true;
                else result=false;
            }
            return result;
        }
        
        virtual void newKeyframeBuilt(){
            minimum=-1;
        }

};


template<template<class> class NodeBuilderTT, class PoseGraphT> 
class ScaleErrorKeyframeVisualOdometry : public KeyframeVisualOdometry<NodeBuilderTT, PoseGraphT> {
    protected:
        typedef typename graph_traits<PoseGraphT>::vertex_descriptor VertexD;

        bool initial_pose_registred;
        VertexD current_keyframe;
        int k; 
        double error;
        double prev_error;
        int count;
        int asc_count;
    public:
        ScaleErrorKeyframeVisualOdometry(
                shared_ptr<NodeBuilderTT<PoseGraphT> > _nbuilder, 
                shared_ptr<EdgeBuilder<PoseGraphT> > _ebuilder,
                size_t _target_past_verticies_size,
                shared_ptr<VodomLogger<PoseGraphT> > _logger= shared_ptr<VodomLogger<PoseGraphT> >()) :
                VisualOdometry<NodeBuilderTT, PoseGraphT>(
                    _nbuilder, _ebuilder, _target_past_verticies_size, _logger),
                initial_pose_registred(false), k(2) {
            newKeyframeBuilt();    
        }
        
        virtual bool buildNewKeyframe(){
            prev_error=error;
            error = get(edge_rel_scale_error, this->graph, 
                    edge(current_keyframe, this->vd, this->graph).first);
            ROS_INFO_STREAM_NAMED("VisualOdometry", "relative error: " << error);

            if(count){
                if(prev_error < error) asc_count++;
                else asc_count=0;
            }
            count++;
            if(asc_count>=k) return true;
            else return false;
        }

        virtual void newKeyframeBuilt(){
            count=0;
            asc_count=0;
        }

};


/*
template<class VOT> class VodomScaleFixer : public VOT{
    public:
    virtual shared_ptr<typename VOT::VertexD> processPose0(
        typename VOT::template NodeBuilderTT<VOT::PoseGraphT>::InputType input,
        int pose_id, 
        uint64_t pose_timestamp, 
        bool debug_images=false, 
        bool trajectory_file=false){
            VOT::processPose0(input, pose_id, pose_timestamp, debug_images, trajectory_file);
            doLogging(this->graph);
        }
};
*/






}//namespace vslam

#endif