pose_graph.h

Go to the documentation of this file.

//part of the source code of master thesis of Jiri Divis, source code written by Jiri Divis
#ifndef VSLAM_POSE_GRAPH_H_INCLUDED
#define VSLAM_POSE_GRAPH_H_INCLUDED

#include "utils.h"
#include "tf/transform_listener.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/CompressedImage.h"
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h" 
#include "Sophus/se3.h"
//#include "sfm_utils.h" //XXX moove
#include "feature_matching.h"

#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <assert.h>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <map>
//#include <boost/tr1/functional.hpp> // XXX



namespace boost{



    
    enum vertex_kp_t { vertex_kp = 101 }; 
    enum vertex_desc_t { vertex_desc = 102 }; 

    enum vertex_fix_idx_t { vertex_fix_idx = 103 }; 

    enum vertex_seq_t { vertex_seq = 104 }; 

    enum vertex_transform_t { vertex_transform = 105 }; 
    
    enum vertex_keyframe_flag_t { vertex_keyframe_flag = 106 }; 

    enum vertex_debug_info_t { vertex_debug_info = 107 };

    enum vertex_timestamp_t { vertex_timestamp = 108 }; 

    enum edge_kp_idx_map_t { edge_kp_idx_map= 201 }; 


    enum edge_transform_t { edge_transform= 202 }; 
    
    enum edge_rel_scale_error_t { edge_rel_scale_error= 203 }; 

    enum graph_points_t { graph_points= 301 }; 

    BOOST_INSTALL_PROPERTY(vertex, kp);
    BOOST_INSTALL_PROPERTY(vertex, desc);
    BOOST_INSTALL_PROPERTY(vertex, fix_idx);
    BOOST_INSTALL_PROPERTY(vertex, seq);
    BOOST_INSTALL_PROPERTY(vertex, transform);
    BOOST_INSTALL_PROPERTY(vertex, keyframe_flag);
    BOOST_INSTALL_PROPERTY(vertex, debug_info);
    BOOST_INSTALL_PROPERTY(vertex, timestamp);

    BOOST_INSTALL_PROPERTY(edge, kp_idx_map);
    BOOST_INSTALL_PROPERTY(edge, transform);
    BOOST_INSTALL_PROPERTY(edge, rel_scale_error);

    BOOST_INSTALL_PROPERTY(graph, points);

}


namespace vslam{
using boost::vertex_kp;
using boost::vertex_desc;
using boost::vertex_fix_idx;
using boost::vertex_seq;
using boost::vertex_transform;
using boost::vertex_keyframe_flag;
using boost::vertex_debug_info;
using boost::vertex_timestamp;
using boost::edge_kp_idx_map;
using boost::edge_transform;
using boost::edge_rel_scale_error;

using boost::vertex_kp_t;
using boost::vertex_desc_t;
using boost::vertex_fix_idx_t;
using boost::vertex_seq_t;
using boost::vertex_transform_t;
using boost::vertex_keyframe_flag_t;
using boost::vertex_debug_info_t;
using boost::vertex_timestamp_t;
using boost::edge_kp_idx_map_t;
using boost::edge_transform_t;
using boost::edge_rel_scale_error_t;

using std::list;
using std::map;

using boost::graph_traits; using boost::property_map; using boost::tie;


const int BAD_INDEX = -1; 


template<class G> class FeatureID{
    public:
        typename graph_traits<G>::vertex_descriptor vd;
        int vidx;
        
        FeatureID(typename graph_traits<G>::vertex_descriptor _vd, int _vidx) : 
                vd(_vd), vidx(_vidx) {}
};

template<class G> bool isLandmakFeature(const G& g, const FeatureID<G>& fid){
    get(vertex_fix_idx, g, fid.vd)[0];
    return get(vertex_fix_idx, g, fid.vd)[fid.vidx]!=BAD_INDEX;
}

template<typename G> struct Point;

namespace lmdetails{
    template<class G> class PointsLoc; 
}

template<class G> class Points{
    private:
        lmdetails::PointsLoc<G> points;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        void activateLandmarks(
                const G&g, typename graph_traits<G>::vertex_descriptor v, int keep_num);

        const Point<G> & getPoint(int idx) const;

        void lmEstimateUpd(int lm_id, Vector3d& new_estimate);

        void lmEstimateUpdChecked(int lm_id, Vector3d& new_estimate);

        void deleteLmObs(G&g, const FeatureID<G>& obs);

        int size() const;


        bool fixNewLandmark( FeatureID<G> fa, FeatureID<G> fm, FeatureID<G> fb, G& g);

        bool fixNewObservation( FeatureID<G> fa, FeatureID<G> fb, G& g);
};



class VertexDebugInfo{
    public:
        uint64_t seq;
        uint64_t timestamp;
        bool is_kf; // is keyframe
        //int num_kpts;
        //int num_kpts_nms; // # of kpts after NMS.
        //int num_2view_kpts;
        //int num_2view_kpts_max;
        int num_init_landmarks; // seen landmarks that are initialized
        int num_uinit_landmarks;
        double avg_reprojection_error;
        double max_reprojection_error;
        int num_discontinued_landmarks;
        int num_extended_landmarks;
        SE3 gt_T_wc;
        map<std::string, double> double_propmap;
};




typedef boost::adjacency_list<
        boost::vecS,
        boost::vecS, 
        boost::directedS,
        boost::property<vertex_kp_t, std::vector<cv::KeyPoint>, 
            boost::property<vertex_desc_t, cv::Mat,
                boost::property<vertex_fix_idx_t, vector<int>,
                    boost::property<vertex_seq_t, int,
                        boost::property<boost::vertex_keyframe_flag_t, int,
                            boost::property<boost::vertex_debug_info_t, VertexDebugInfo,
                                boost::property<boost::vertex_timestamp_t, uint64_t,
                                    boost::property<vertex_transform_t, SE3> > > > > > > >,
        boost::property<edge_kp_idx_map_t, map<int,int>,
            boost::property<edge_transform_t, SE3,
                boost::property<edge_rel_scale_error_t, double> > > > PoseGraph;

typedef graph_traits<PoseGraph>::vertex_descriptor PGVertexD;
typedef graph_traits<PoseGraph>::edge_descriptor PGEdgeD;

typedef ALIGNED<pair<int, Vector3d> >::vector IdxPoint3dVec;


template<class G> class PerformanceLogger{
    public:
        typedef typename boost::graph_traits<G>::vertex_descriptor VDT;
    private:
        G& g;
        VDT vd;
        std::string str;
        ros::WallTime start_time;
        bool stop_forced;
    public:
        PerformanceLogger(G& _g, VDT _vd, std::string name_tag) : g(_g), vd(_vd){
            str = "timings_" + name_tag + "_d_x";
            start_time = ros::WallTime::now();
            stop_forced = false;
        }

        ~PerformanceLogger(){
            if(!stop_forced) forceStop();
        }

        void forceStop(){
            assert(!stop_forced);
            ros::WallDuration duration = ros::WallTime::now() - start_time;
            map<std::string, double> & propmap = get(vertex_debug_info, g, vd).double_propmap;
            if(propmap.find(str)!=propmap.end()){
                str += "x";
            }
            propmap[str] = duration.toSec();
            stop_forced=true;
        }
};
/*------------------------------------------------------------------------------------------*/
template<typename G> class EdgeBuilder{
    public:
        typedef typename graph_traits<G>::vertex_descriptor VertexD;
        typedef typename graph_traits<G>::edge_descriptor EdgeD;

    protected:
        static void featureSelection(G& g, VertexD vd, int target_num, vector<int>& keep_idxs, 
                vector<cv::KeyPoint>& sel_keypoints, cv::Mat& sel_descriptors);

    public:
        bool build(const VertexD & src, const VertexD & dst, Points<G> & points, G & g){
            return buildImpl(src, dst, points, g);
        }

        virtual bool buildImpl(
                const VertexD & src, const VertexD & dst, Points<G> & points, G & g) = 0;
};



template<typename G> class MonoFtrClassEdgeBuilder : public EdgeBuilder<G>{
    public:
        typedef typename graph_traits<G>::vertex_descriptor VertexD;
        typedef typename graph_traits<G>::edge_descriptor EdgeD;
    protected:
        std::tr1::shared_ptr<RobustFeatureMatcher> rmatcher;

        static void compute3viewStructure(VertexD vad, VertexD vbd, VertexD vcd, G & g,
        IdxPoint3dVec&, SE3&);

        static void matchScaleFindNewLandmarks(
                VertexD vsrc, VertexD vdest, Points<G> & points, G & g);

        static void computeScale(VertexD contactvd, VertexD newvd, G & g, Points<G>& points, 
                SE3& scaled_T_vbd_vcd);

        bool unguidedMatching(
                const vector<cv::KeyPoint>& sel_tvd_keypoints,
                const vector<cv::KeyPoint>& sel_qvd_keypoints,
                const cv::Mat& sel_tvd_descriptors,
                const cv::Mat& sel_qvd_descriptors,
                SE3& T_tvd_qvd,
                std::vector<cv::DMatch>& global_dmatches_c1c2);

        void guidedMatching( G& g, VertexD c1vd, VertexD c2vd); 
    public:
        MonoFtrClassEdgeBuilder<G>(std::tr1::shared_ptr<RobustFeatureMatcher> _rmatcher) :
                rmatcher(_rmatcher) {}


        virtual bool buildImpl(
                const VertexD & src, const VertexD & dst, Points<G> & points, G & g);
};



inline std::tr1::shared_ptr<RobustFeatureMatcher> constructRMatcher(
        std::tr1::shared_ptr<cv::DescriptorMatcher> _dmatcher,
        std::tr1::shared_ptr<EMatrixModelBuilder> _minbuilder, 
        std::tr1::shared_ptr<EMatrixModelBuilder> _builder = 
        std::tr1::shared_ptr<EMatrixModelBuilder>());
/*------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------*/

class NMSOptions{
    public:
        bool do_nms;
        int target_num_points; 

        NMSOptions() : do_nms(false) {}
        NMSOptions(int _target_num_points) : do_nms(true), target_num_points(_target_num_points) {}
};

template<class PoseGraphT> class OneDetectorNodeBuilder {
    protected:
         shared_ptr<cv::DescriptorExtractor> extractor;
         shared_ptr<cv::FeatureDetector> detector;
         shared_ptr<cv::ORB> feature_2d_detector;
         cv::Mat mask;
         NMSOptions nms_options;

        OneDetectorNodeBuilder(NMSOptions _nms_options = NMSOptions()) : nms_options(_nms_options) {
            cv::Mat mask0 = cv::imread(image_mask_file);
            cv::cvtColor(mask0, mask, CV_RGB2GRAY);
        }
    public:
        typedef cv::Mat InputType;

        cv::Mat getPoseImage(const InputType& in){
            return in.clone();
        }

        bool build(const cv::Mat & image, int image_seq_num, uint64_t pose_timestamp,
                typename graph_traits<PoseGraphT>::vertex_descriptor & vd, PoseGraphT& g);

        void demolish(PoseGraphT& g, Points<PoseGraphT>& points, 
                typename graph_traits<PoseGraphT>::vertex_descriptor & vd);
};

template<class PoseGraphT>
class CustomOneDetectorNodeBuilder : public OneDetectorNodeBuilder<PoseGraphT>{
    public:
        typedef cv::Mat InputType;
        CustomOneDetectorNodeBuilder(
                std::tr1::shared_ptr<cv::FeatureDetector> _detector,
                std::tr1::shared_ptr<cv::DescriptorExtractor> _extractor){
            this->detector=_detector;
            this->extractor=_extractor;
        }
};

template<class PoseGraphT>
class CustomOneDetectorNodeBuilderFeature2D : public OneDetectorNodeBuilder<PoseGraphT>{
    public:
        typedef cv::Mat InputType;
        CustomOneDetectorNodeBuilderFeature2D(
                std::tr1::shared_ptr<cv::ORB> _feature_2d_detector) : 
                OneDetectorNodeBuilder<PoseGraphT>(NMSOptions(4000)){
            this->feature_2d_detector=_feature_2d_detector;
        }
};

} //namespace vslam

/*------------------------------------------------------------------------------------------*/
//boost stuff for running dijkstra algorithm on the pose-graph.
/*------------------------------------------------------------------------------------------*/

#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <iostream>
#include <cmath>
#include <tr1/functional>
namespace vslam{
    template<class PoseGraphT> class AllWeightsOneWeightMap {
        PoseGraphT* dummy;
    };
}

namespace boost{
    template<class PoseGraphT> struct property_traits<vslam::AllWeightsOneWeightMap<PoseGraphT> >{
        typedef int value_type;
        typedef int reference;
        typedef typename graph_traits<PoseGraphT>::edge_descriptor key_type;
        typedef readable_property_map_tag category;
    };
}

namespace vslam{
    template<class PoseGraphT> int get(
            AllWeightsOneWeightMap<PoseGraphT> pmap, 
            typename boost::property_traits<AllWeightsOneWeightMap<PoseGraphT> >::key_type vd){
        return 1;
    };
}



#include "graph_builder.h"
#endif