graph_utils.h

Go to the documentation of this file.

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

namespace vslam{

/*------------------------------------------------------------------------------------------*/


template<class G> int getFeatureIdx(const G& g, int source_idx, 
        typename graph_traits<G>::vertex_descriptor source_vd, 
        typename graph_traits<G>::vertex_descriptor target_vd){
    using namespace boost;
    assert(edge(source_vd, target_vd, g).second);
    typename graph_traits<G>::edge_descriptor ed = edge(source_vd, target_vd, g).first;
    map<int, int>::const_iterator it = 
            get(edge_kp_idx_map, g, ed).find(source_idx);
    if(it != get(edge_kp_idx_map, g, ed).end()){
        return it->second;
    } else{
        return BAD_INDEX;
    }
}

template<class G> int getFeatureIdx(const G& g, FeatureID<G> srcf,
        typename graph_traits<G>::vertex_descriptor target_vd){
    return getFeatureIdx(g, srcf.vidx, srcf.vd, target_vd); 
}

template<typename G> void remove_idx_pair(
        int sidx,
        typename graph_traits<G>::vertex_descriptor src, 
        typename graph_traits<G>::vertex_descriptor dest, 
        G& g){
    using namespace boost;
    typename property_map<G, edge_kp_idx_map_t>::type epmap = get(edge_kp_idx_map, g);
    int didx = epmap[edge(src, dest, g).first][sidx];
    epmap[edge(src, dest, g).first].erase(sidx);
    epmap[edge(dest, src, g).first].erase(didx);
}

template<typename G> void add_idx_pair(
        int src_idx, int dest_idx, 
        typename graph_traits<G>::vertex_descriptor src, 
        typename graph_traits<G>::vertex_descriptor dest, G& g){
    using namespace boost;
    typename property_map<G, edge_kp_idx_map_t>::type epmap = get(edge_kp_idx_map, g);
    assert((size_t)dest_idx < get(vertex_kp, g, dest).size());
    assert((size_t)src_idx < get(vertex_kp, g, src).size());
    epmap[edge(src, dest, g).first][src_idx] = dest_idx;
    epmap[edge(dest, src, g).first][dest_idx] = src_idx;
}

template<typename G> void add_idx_pair_from_dmatch(
        const cv::DMatch& dmatch,
        typename graph_traits<G>::vertex_descriptor tvd, 
        typename graph_traits<G>::vertex_descriptor qvd, 
        G& g){
    add_idx_pair(dmatch.trainIdx, dmatch.queryIdx, tvd, qvd, g);
}

template<typename G> bool filter_index_pair_duplicates(
        const cv::DMatch& dmatch, 
        typename graph_traits<G>::vertex_descriptor src, 
        typename graph_traits<G>::vertex_descriptor dest, G& g){
    using namespace boost;
    bool add = (getFeatureIdx(g, dmatch.trainIdx, src, dest) == BAD_INDEX);
    add = add && (getFeatureIdx(g, dmatch.queryIdx, dest, src) == BAD_INDEX);
    return add;
}

inline vector<cv::DMatch> remapDMatchIdxs(
        const vector<cv::DMatch>& dmatches, const vector<int>& tidx, const vector<int>& qidx){
    vector<cv::DMatch> result;
    result.reserve(dmatches.size());
    for(vector<cv::DMatch>::const_iterator it=dmatches.begin(); it!= dmatches.end(); ++it){
        cv::DMatch tmp = *it;
        tmp.trainIdx = tidx[it->trainIdx];
        tmp.queryIdx = qidx[it->queryIdx];
        result.push_back(tmp);
    }
    return result;
}

inline cv::DMatch dmatchFromPair(const pair<int, int>& p){
    return cv::DMatch(p.first, p.second, 0.1);
}

inline pair<int, int> pairFromDMatch(const cv::DMatch& dmatch){
    return make_pair(dmatch.trainIdx, dmatch.queryIdx);
}

template<template<class, class> class CT, class G> CT<int, std::allocator<int> > 
common_feature_tracks(
        G&g, 
        typename graph_traits<G>::vertex_descriptor vad, 
        typename graph_traits<G>::vertex_descriptor vbd, 
        typename graph_traits<G>::vertex_descriptor vcd){
    typename property_map<G, edge_kp_idx_map_t>::type ekpimap = get(edge_kp_idx_map, g);
    typedef typename graph_traits<G>::edge_descriptor EdgeD;
    EdgeD edba = edge(vbd, vad, g).first;
    EdgeD edbc = edge(vbd, vcd, g).first;
    CT<int, std::allocator<int> > common_b_idx = switchCont<CT>(getCommon(
            transformF<int>(mapToList(ekpimap[edba]), getFirst),
            transformF<int>(mapToList(ekpimap[edbc]), getFirst)));
    return common_b_idx;
}
/*------------------------------------------------------------------------------------------*/

/*------------------------------------------------------------------------------------------*/


template<class G> ImageMatch computeMatch(const G&g, FeatureID<G> source, FeatureID<G> target){
    using namespace boost;
    typename property_map<G, vertex_kp_t>::const_type kp_pmap = get(vertex_kp, g);
    cv::KeyPoint kp;
    kp = kp_pmap[source.vd][source.vidx];
    PanoImCoord c1 = PanoImCoord::fromPixel(Vector2d(kp.pt.x, kp.pt.y));
    kp = kp_pmap[target.vd][target.vidx];
    PanoImCoord c2 = PanoImCoord::fromPixel(Vector2d(kp.pt.x, kp.pt.y));
    return ImageMatch(c1.getHomogenous(), c2.getHomogenous());
}

template<class G> ImageMatch computeMatch(const G&g, int source_idx,
        typename graph_traits<G>::vertex_descriptor source_vd, 
        typename graph_traits<G>::vertex_descriptor target_vd){
    using namespace boost;
    int target_idx = getFeatureIdx(g, source_idx, source_vd, target_vd);
    return computeMatch(g, FeatureID<G>(source_vd, source_idx), FeatureID<G>(target_vd, target_idx));
}



template<typename G> ImageMatchVec computeMatches(
        typename graph_traits<G>::vertex_descriptor src, 
        typename graph_traits<G>::vertex_descriptor dest, 
        const G& g){
    using namespace boost;
    ImageMatchVec matches;
    typename property_map<G, vertex_kp_t>::const_type vkpmap = get(vertex_kp, g);
    for(vector<cv::KeyPoint>::size_type i=0; i<vkpmap[src].size(); i++){
        if(getFeatureIdx(g, i, src, dest)!=BAD_INDEX){
            matches.push_back(computeMatch(g, i, src, dest));
        }
    }
    return matches;
}

/*------------------------------------------------------------------------------------------*/

/*------------------------------------------------------------------------------------------*/


template<class PoseGraphT> SE3 getEdgeTransform(
        const PoseGraphT& g, 
        typename graph_traits<PoseGraphT>::vertex_descriptor vd1, 
        typename graph_traits<PoseGraphT>::vertex_descriptor vd2){
    return  get(vertex_transform, g, vd2) * get(vertex_transform, g, vd1).inverse();
}

template<class G> void setEdgeTransform(
        const SE3& T_tvd_qvd, 
        typename graph_traits<G>::vertex_descriptor tvd,
        typename graph_traits<G>::vertex_descriptor qvd,
        G& g){
    get(edge_transform, g, edge(tvd, qvd, g).first) = T_tvd_qvd;
    get(edge_transform, g, edge(qvd, tvd, g).first) = T_tvd_qvd.inverse();
    get(vertex_transform, g, qvd) = T_tvd_qvd * get(vertex_transform, g, tvd);
}
/*------------------------------------------------------------------------------------------*/

/*------------------------------------------------------------------------------------------*/


inline double vectorAngle(const Vector3d& vec1, const Vector3d& vec2){
    double d = (vec1.normalized() - vec2.normalized()).norm();
    double angle = asin(d/2.0) * 2.0;
    return angle;
}

template<class PoseGraphT> double disparityAngle( 
        const PoseGraphT& g, FeatureID<PoseGraphT> f1, FeatureID<PoseGraphT> f2){
    SE3 T_wv1 = get(vertex_transform, g, f1.vd);
    SE3 T_wv2 = get(vertex_transform, g, f2.vd);
    SE3 T_v1v2 = T_wv2 * T_wv1.inverse();

    ImageMatch m = computeMatch(g, f1, f2);
    m.first = (T_v1v2.rotation_matrix() * m.first).eval();
    return vectorAngle(m.first, m.second);
}


template<class PoseGraphT> double minEdgeDisparityAngle(
        PoseGraphT& g,
        typename graph_traits<PoseGraphT>::vertex_descriptor srcv,
        typename graph_traits<PoseGraphT>::vertex_descriptor destv){
    //vector<pair<FeatureID<PoseGraphT>, FeatureID<PoseGraphT> > > feature_pairs;
    vector<double> disparity_angles;
    const map<int, int> & src_dst_map = get(edge_kp_idx_map, g, edge(srcv, destv, g).first);

    for(map<int, int>::const_iterator it=src_dst_map.begin(); it!=src_dst_map.end(); ++ it){
        double angle =disparityAngle(g, FeatureID<PoseGraphT>(srcv, it->first), 
                FeatureID<PoseGraphT>(destv, it->second));
        disparity_angles.push_back(angle);
    }
    std::sort(disparity_angles.begin(), disparity_angles.end());
    return disparity_angles[disparity_angles.size() - disparity_angles.size() / 10];
}
/*------------------------------------------------------------------------------------------*/

/*------------------------------------------------------------------------------------------*/

template<class G> Vector3d triangulatePointWrtWorld(const G& g, FeatureID<G> f1, FeatureID<G> f2){
    SE3 T_v1v2 = getEdgeTransform(g, f1.vd, f2.vd);
    ImageMatchVec matches;
    matches.push_back(computeMatch(g, f1, f2));
    Points3dSTL tr_pts;
    computeStructure(T_v1v2, matches, tr_pts);
    return get(vertex_transform, g, f1.vd).inverse() * tr_pts[0];
}

template<class G> Vector3d triangulatePointWrtWorldPos(const G& g, FeatureID<G> f1, FeatureID<G> f2){
    SE3 T_v1v2 = getEdgeTransform(g, f1.vd, f2.vd);
    ImageMatchVec matches;
    matches.push_back(computeMatch(g, f1, f2));
    Points3dSTL tr_pts;
    computeStructureForcePositiveDepths(T_v1v2, matches, tr_pts);
    return get(vertex_transform, g, f1.vd).inverse() * tr_pts[0];
}

inline bool isEstimateInFrontOfCamera(const LandmarkObservation& obs, const Vector3d& estimate){ 
    double angle = vectorAngle(
            obs.T_cw.inverse() * estimate, PanoImCoord::fromPixel(obs.pixel).getHomogenous());
    //std::cout << "Angle: " << angle << endl;
    if(angle < 0.5 * M_PI) return true;
    else return false;
}
/*------------------------------------------------------------------------------------------*/


template<class G> void landmarkObservationsBySize(
        const G& g, 
        Points<G>& points, 
        typename graph_traits<G>::vertex_descriptor vd, 
        vector<vector<int> > & all_result,
        vector<vector<int> > & kf_result){
    const vector<cv::KeyPoint> & keypoints = get(vertex_kp, g, vd);
    const vector<int> & fix_map = get(vertex_fix_idx, g, vd);
    
    int maxlen=8;
    all_result.assign(maxlen+1, vector<int>());
    kf_result.assign(maxlen+1, vector<int>());

    for(int kp_idx=0; kp_idx< (int)keypoints.size(); kp_idx++){
        if(fix_map[kp_idx]!=BAD_INDEX && points.getPoint(fix_map[kp_idx]).initialized){
            int sz = points.getPoint(fix_map[kp_idx]).seen_from.size();
            for(sz=std::min(sz, maxlen); sz>0; sz--){
                all_result[sz].push_back(fix_map[kp_idx]);
            }
            int count=0;
            typedef pair<typename Point<G>::VertexD, int> T;
            BOOST_FOREACH(T fc, points.getPoint(fix_map[kp_idx]).seen_from){
                if(get(boost::vertex_keyframe_flag, g, fc.first)){
                    count++;
                }
            }
            for(count=std::min(count, maxlen); count>0; count--){
                kf_result[count].push_back(fix_map[kp_idx]);
            }
        }
    }
}


template<class G> Vector2d getFeaturePixel(const G& g, const FeatureID<G> f){
    cv::KeyPoint kp = get(vertex_kp, g, f.vd)[f.vidx];
    return Vector2d(kp.pt.x, kp.pt.y);
}

template<class G> 
double maxFeatureDistance(const G& g, const Points<G>& points, typename Point<G>::VertexD v){
    const vector<int>& fix_idxs = get(vertex_fix_idx, g,  v);
    double max=0;
    for(size_t idx1=0; idx1 < fix_idxs.size(); idx1++){
        for(size_t idx2=0; idx2 < fix_idxs.size(); idx2++){
            if(fix_idxs[idx1]!=BAD_INDEX && fix_idxs[idx2]!=BAD_INDEX){
                if(points.getPoint(fix_idxs[idx1]).initialized && points.getPoint(idx2).initialized){
                    double val = (getFeaturePixel(g, FeatureID<G>(v, idx1)) - 
                            getFeaturePixel(g, FeatureID<G>(v, idx2))).norm();
                    if(val > max){
                        max=val;
                    }
                }
            }
        }
    }
    return max;
}

template<class G> std::string printVertexId(
        const G& g, typename graph_traits<G>::vertex_descriptor vd){
    std::ostringstream oss;
    oss << get(vertex_seq, g, vd);
    return oss.str();
}






}//namespace vslam



#endif