graph_builder.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_GRAPH_BUILDER_H_INCLUDED
#define VSLAM_GRAPH_BUILDER_H_INCLUDED
#include "pose_graph.h" 
#include "graph_utils.h"  
#include "landmark_manager.h"
#include "emat_estimation.h"
#include "grid.h" 

#include <algorithm>
#include <tr1/functional>
#include <string>
namespace vslam{

/*------------------------------------------------------------------------------------------*/
//Local matching stuff.
/*------------------------------------------------------------------------------------------*/
template<class PoseGraphT> void findLocalMatchesSE3(
        const PoseGraphT& g, 
        typename graph_traits<PoseGraphT>::vertex_descriptor vd1, 
        typename graph_traits<PoseGraphT>::vertex_descriptor vd2,
        shared_ptr<RobustFeatureMatcher> rmatcher,
        vector<cv::DMatch>& matches){
    assert(matches.size()==0);
    typename graph_traits<PoseGraphT>::vertex_descriptor vd0;
    typename graph_traits<PoseGraphT>::adjacency_iterator vi, vi_end;
    for(tie(vi, vi_end) = adjacent_vertices(vd1 , g); vi!=vi_end; ++vi){
        if(*vi!=vd2){
            vd0=*vi;
            break; //we take first neighbouring edge to determine vd0
        }
    }
    if(vi==vi_end) return;

    typename property_map<PoseGraphT, vertex_kp_t>::const_type kp_pmap = get(vertex_kp, g);
    typename property_map<PoseGraphT, vertex_desc_t>::const_type desc_pmap = get(vertex_desc, g);
    Grid grid2(60,60, kp_pmap[vd2]);

    for(size_t i=0; i<kp_pmap[vd1].size(); i++) {
        int j = getFeatureIdx(g, i, vd1, vd2);
        int h = getFeatureIdx(g, i, vd1, vd0);
        if(j==BAD_INDEX){
            Vector3d pt_c2;
            Vector2d c2Pixel;
            if(h != BAD_INDEX){
                pt_c2 = get(vertex_transform, g, vd2) * triangulatePointWrtWorldPos(
                        g, FeatureID<PoseGraphT>(vd0, h), FeatureID<PoseGraphT>(vd1, i));
                c2Pixel = PanoImCoord::fromHomogenous(pt_c2).getPixel();
            } else{
                pt_c2 = PanoImCoord::fromPixel(
                        Vector2d(kp_pmap[vd1][i].pt.x, kp_pmap[vd1][i].pt.y)).getHomogenous();
                pt_c2 = (getEdgeTransform(g, vd1, vd2).rotation_matrix() * pt_c2).eval();
                c2Pixel = PanoImCoord::fromHomogenous(pt_c2).getPixel();
            }
            vector<int> keypoint_idxs_c2 = grid2.getNearKeypoints(grid2.getCell(c2Pixel));

            vector<cv::KeyPoint> keypoints2_sel=getArrayIdices(kp_pmap[vd2], keypoint_idxs_c2);
            cv::Mat descriptors2_sel=getArrayIdices<cv::Mat>(desc_pmap[vd2], keypoint_idxs_c2);

            vector<cv::DMatch> matches_tmp;
            RobustFeatureMatcherStats stats;
            rmatcher->nonsymetricalMatch(keypoints2_sel, mkContainer<vector>(kp_pmap[vd1][i]),  
                    descriptors2_sel,
                    getArrayIdices<cv::Mat>(desc_pmap[vd1], mkContainer<vector>((int)i)), 
                    stats, matches_tmp);
            if(matches_tmp.size()>=1){
                cv::DMatch tmp = matches_tmp[0];
                tmp.queryIdx=keypoint_idxs_c2[matches_tmp[0].trainIdx];
                tmp.trainIdx=i;
                matches.push_back(tmp);
            }
        }
    }
}

template<class PoseGraphT> void findLocalMatchesSO3(
        const PoseGraphT& g, 
        typename graph_traits<PoseGraphT>::vertex_descriptor tvd, 
        typename graph_traits<PoseGraphT>::vertex_descriptor qvd,
        shared_ptr<RobustFeatureMatcher> rmatcher,
        Vector2d grid_dim,
        bool skip_landmarks,
        vector<cv::DMatch>& matches){
    assert(matches.size()==0);

    typename property_map<PoseGraphT, vertex_kp_t>::const_type kp_pmap = get(vertex_kp, g);
    typename property_map<PoseGraphT, vertex_desc_t>::const_type desc_pmap = get(vertex_desc, g);
    Grid grid2(grid_dim(0), grid_dim(1), kp_pmap[qvd]);

    for(size_t i=0; i<kp_pmap[tvd].size(); i++) {
        int j = getFeatureIdx(g, i, tvd, qvd);
        if(j!=BAD_INDEX) continue;
        if(skip_landmarks && (get(vertex_fix_idx, g, tvd)[i]!=BAD_INDEX)) continue;

        Vector2d ctvd_pixel = Vector2d(kp_pmap[tvd][i].pt.x, kp_pmap[tvd][i].pt.y);
        Vector3d pt_ct = PanoImCoord::fromPixel(ctvd_pixel).getHomogenous();
        Vector3d pt_cq = getEdgeTransform(g, tvd, qvd).rotation_matrix() * pt_ct;
        Vector2d cqvd_pixel = PanoImCoord::fromHomogenous(pt_cq).getPixel();
        vector<int> keypoint_idxs_c2 = grid2.getNearKeypoints(grid2.getCell(cqvd_pixel));

        vector<cv::KeyPoint> keypointsq_sel=getArrayIdices(kp_pmap[qvd], keypoint_idxs_c2);
        cv::Mat descriptorsq_sel=getArrayIdices<cv::Mat>(desc_pmap[qvd], keypoint_idxs_c2);

        vector<cv::DMatch> matches_tmp;
        RobustFeatureMatcherStats stats;
        rmatcher->nonsymetricalMatch(
                keypointsq_sel, 
                mkContainer<vector>(kp_pmap[tvd][i]),  
                descriptorsq_sel,
                getArrayIdices<cv::Mat>(desc_pmap[tvd], mkContainer<vector>((int)i)), 
                stats, 
                matches_tmp);

        if(matches_tmp.size()>=1){
            cv::DMatch tmp = matches_tmp[0];
            tmp.queryIdx=keypoint_idxs_c2[matches_tmp[0].trainIdx];
            tmp.trainIdx=i;
            matches.push_back(tmp);
        }
    }
}



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

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

//see declaration for comment
template<class PoseGraphT> 
bool OneDetectorNodeBuilder<PoseGraphT>::build(const cv::Mat & image, int image_seq_num, 
        uint64_t pose_timestamp, typename graph_traits<PoseGraphT>::vertex_descriptor& vd, PoseGraphT& g){
    using namespace boost;
    ros::WallTime nodebuilder_start_time = ros::WallTime::now();
    assert(!image.empty());
    vd = add_vertex(g);
    get(vertex_seq, g, vd) = image_seq_num;
    get(vertex_timestamp, g, vd) = pose_timestamp;
    vector<cv::KeyPoint> & keypoints = get(vertex_kp, g, vd);
    cv::Mat & descriptors = get(vertex_desc, g, vd);
    if(feature_2d_detector){ //hack for non-common feature detector interface
        (*feature_2d_detector)(image, mask, keypoints, descriptors, false);
    } else{
        detector->detect(image, keypoints, mask);
    }
    int detected_kpts = keypoints.size();
    vector<int> keep_kp_idx;
    if(nms_options.do_nms){
        nonMaximaSuppresion(keypoints, nms_options.target_num_points, keep_kp_idx);
        keypoints=getArrayIdices<cv::KeyPoint>(keypoints, keep_kp_idx);
    }
    if(feature_2d_detector && nms_options.do_nms){
        descriptors=getArrayIdices<cv::Mat>(descriptors, keep_kp_idx);
    } else{
        extractor->compute(image, keypoints, descriptors);
    }

    vector<int> & pt3_idx = get(vertex_fix_idx, g, vd);
    pt3_idx = vector<int>(keypoints.size());
    std::fill_n(pt3_idx.begin(), keypoints.size(), BAD_INDEX);
    get(vertex_debug_info, g, vd).double_propmap["num_kpts"]=detected_kpts;
    get(vertex_debug_info, g, vd).double_propmap["num_kpts_nms"]=keypoints.size();
    ROS_INFO_STREAM_NAMED("PoseGraph", "constructed node for image number " << image_seq_num <<
            ". Detected " << detected_kpts << " keypoints. " <<
            "Out of this: " << keypoints.size() << " were retained adter NMS.");

    ros::WallDuration nodebuilder_duration = ros::WallTime::now() - nodebuilder_start_time;
    get(vertex_debug_info, g, vd).double_propmap["timings_nodebuilder_d"]=
            nodebuilder_duration.toSec();
    return true;
}


template<class PoseGraphT> void OneDetectorNodeBuilder<PoseGraphT>::demolish( PoseGraphT& g, 
        Points<PoseGraphT>& points, typename graph_traits<PoseGraphT>::vertex_descriptor & vd){
    vector<cv::KeyPoint> & keypoints = get(vertex_kp, g, vd);
    vector<int> & fixed_points = get(vertex_fix_idx, g, vd);
    for(size_t i = 0; i<keypoints.size(); i++){
        if(fixed_points[i] != BAD_INDEX){
            //points.getPoint(fixed_points[i]).deleteObservation(g, vd);
            points.deleteLmObs(g, FeatureID<PoseGraphT>(vd, i)); //XXX
        }
    }

    typename graph_traits<PoseGraphT>::adjacency_iterator vi, vi_end;
    for(tie(vi, vi_end) = adjacent_vertices(vd , g); vi!=vi_end; ++vi){
        remove_edge(*vi, vd, g);
        remove_edge(vd, *vi, g);
    }

    get(vertex_debug_info, g, vd).double_propmap["deleted"]= 1.0;
    get(vertex_debug_info, g, vd).double_propmap.clear();
}

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

/*------------------------------------------------------------------------------------------*/
template<class G> void setRelTranslationError(
        double r_min, double r_max, 
        typename graph_traits<G>::vertex_descriptor vs, 
        typename graph_traits<G>::vertex_descriptor vt, 
        G& g){
    double dist = get(edge_transform, g, edge(vs, vt, g).first).translation().norm();
    double error = (dist*r_max - dist*r_min) / (dist*r_max);
    get(edge_rel_scale_error, g, edge(vs, vt, g).first) = error;
    get(edge_rel_scale_error, g, edge(vt, vs, g).first) = error;
}





template<class G> void MonoFtrClassEdgeBuilder<G>::compute3viewStructure( VertexD vad, VertexD vbd, 
        VertexD vcd, G & g, IdxPoint3dVec& common_pts_3d, SE3& scaled_T_vbd_vcd){
    using namespace boost;
    typename property_map<G, edge_kp_idx_map_t>::type ekpimap = get(edge_kp_idx_map, g);
    //typename property_map<G, edge_transform_t>::type etrmap = get(edge_transform, g);
    EdgeD edba = edge(vbd, vad, g).first;
    EdgeD edbc = edge(vbd, vcd, g).first;
    const map<int, int>& ba_idx = ekpimap[edba];
    const map<int, int>& bc_idx = ekpimap[edbc];
    Points3dSTL pts_ab;
    //computeStructure(etrmap[edba], computeMatches(vbd, vad, g), pts_ab);
    computeStructure(getEdgeTransform(g, vbd, vad), computeMatches(vbd, vad, g), pts_ab);
    //XXX filter out matches that do not need to be computed
    vector<int> common_idx = switchCont<vector>(getCommon(
            transformF<int>(mapToList(ba_idx), getFirst),
            transformF<int>(mapToList(bc_idx), getFirst)));
    Points3dSTL pts_ba_common = switchCont<vector>(transformF<Vector3d>(
            getByIdx(indexBy(transformF<int>(mapToList(ba_idx), getFirst), pts_ab), common_idx), 
            getSecond));

    Points3dSTL pts_bc;
    //computeStructure(etrmap[edbc] , computeMatches(vbd, vcd, g), pts_bc);
    computeStructure(getEdgeTransform(g, vbd, vcd) , computeMatches(vbd, vcd, g), pts_bc);
    Points3dSTL pts_bc_common = switchCont<vector>(transformF<Vector3d>(
            getByIdx(indexBy(transformF<int>(mapToList(bc_idx), getFirst), pts_bc), common_idx), 
            getSecond));
    double r_low, r_hi;
    double ab_to_bc_ratio;
    computeScale(pts_ba_common, pts_bc_common, r_low, ab_to_bc_ratio, r_hi);
    setRelTranslationError(r_low, r_hi, vbd, vcd, g);
    Points3dSTL::const_iterator pts_ba_common_it = pts_bc_common.begin();
    Points3dSTL::const_iterator pts_bc_common_it = pts_ba_common.begin();
    BOOST_FOREACH(int i, common_idx){
        common_pts_3d.push_back(std::make_pair(i, *pts_bc_common_it));
        ++pts_ba_common_it;
        ++pts_bc_common_it;
    }
    //scaled_T_vbd_vcd = SE3(etrmap[edbc].rotation_matrix(), 
    //        etrmap[edbc].translation() * ab_to_bc_ratio);
    scaled_T_vbd_vcd = SE3(getEdgeTransform(g, vbd, vcd).rotation_matrix(), 
            getEdgeTransform(g, vbd, vcd).translation() * ab_to_bc_ratio);
    //cout << "AAA" << ab_to_bc_ratio << endl;
    //cout << "AAA" << etrmap[edbc];
    //cout << "AAA" << scaled_T_vbd_vcd;
}


template<class G> void MonoFtrClassEdgeBuilder<G>::matchScaleFindNewLandmarks(
        VertexD vsrc, VertexD vdest, Points<G> & points, G & g){
    using namespace boost;
    typename graph_traits<G>::adjacency_iterator vi, vi_end;
    int new_track_count=0;
    //first, only scale is computed.
    for(tie(vi, vi_end) = adjacent_vertices(vsrc , g); vi!=vi_end; ++vi){
        if(*vi != vdest){
            IdxPoint3dVec common_pts_3d_vsrc; //common features in the three views
            SE3 scaled_T_vsrc_vdest;
            compute3viewStructure(*vi, vsrc, vdest, g, common_pts_3d_vsrc, scaled_T_vsrc_vdest);
            if(get(vertex_keyframe_flag, g, *vi)){
                get(edge_transform, g, edge(vsrc, vdest, g).first) = scaled_T_vsrc_vdest;
                get(edge_transform, g, edge(vdest, vsrc, g).first) = scaled_T_vsrc_vdest.inverse();
                get(vertex_transform, g, vdest) = scaled_T_vsrc_vdest * get(vertex_transform, g, vsrc);
            }
        }
    }

    //second, find new landmarks
    for(tie(vi, vi_end) = adjacent_vertices(vsrc , g); vi!=vi_end; ++vi){
        if(*vi != vdest){
            //this is done only to get common_pts_3d_vsrc
            IdxPoint3dVec common_pts_3d_vsrc; 
            SE3 scaled_T_vsrc_vdest; //dummy
            compute3viewStructure(*vi, vsrc, vdest, g, common_pts_3d_vsrc, scaled_T_vsrc_vdest);
            
            IdxPoint3dVec::const_iterator common_pts_3d_it = common_pts_3d_vsrc.begin();
            while(common_pts_3d_it!=common_pts_3d_vsrc.end()){
                FeatureID<G> srcf = FeatureID<G>(vsrc, common_pts_3d_it->first);
                FeatureID<G> destf = FeatureID<G>(vdest, getFeatureIdx(g, srcf, vdest));
                FeatureID<G> vif = FeatureID<G>(*vi, getFeatureIdx(g, srcf, *vi));

                //new landmark with 3 features
                if(!isLandmakFeature(g, vif) && !isLandmakFeature(g, srcf) && 
                        !isLandmakFeature(g, destf)){
                    if(points.fixNewLandmark(vif, srcf, destf, g)){
                        new_track_count++;
                    }
                }
                ++common_pts_3d_it;;
            }
        }
    }
    std::string num_new_lms_key="num_new_lms";
    map<std::string, double> & pmap = get(vertex_debug_info, g, vsrc).double_propmap;
    if(pmap.find(num_new_lms_key) == pmap.end()){
        pmap[num_new_lms_key]= new_track_count;
    } else{
        get(vertex_debug_info, g, vdest).double_propmap[num_new_lms_key]=new_track_count;
    }
    ROS_INFO_STREAM_NAMED("EdgeBulder", "# of new tracks: " << new_track_count);
    get(vertex_debug_info, g, vsrc).double_propmap["tracks_new"]=new_track_count;
}




template<class G> void MonoFtrClassEdgeBuilder<G>::computeScale(VertexD convd, VertexD newvd, 
        G & g, Points<G>& points, SE3& scaled_T_vbd_vcd){

    scaled_T_vbd_vcd = SE3(scaled_T_vbd_vcd.rotation_matrix(), 
            scaled_T_vbd_vcd.translation().normalized());
    VertexD vad, vbd, vcd;

    //find last keyframe
    bool found_neighbour = false;
    typename graph_traits<G>::adjacency_iterator vi, vi_end;
    for(tie(vi, vi_end) = adjacent_vertices(convd , g); vi!=vi_end; ++vi){
        if(*vi != newvd){
            found_neighbour = true;
            vad = *vi;
            vbd = convd;
            vcd = newvd;
            if(get(vertex_keyframe_flag, g, *vi)) break;
        }
    }

    double total2;
    if(found_neighbour){
        //set up data for model estimation
        vector<int> common_b_idx = common_feature_tracks<vector>(g, vad, vbd, vcd); 
        int count=0;
        ALIGNED<ScaleObservation>::vector data;
        ALIGNED<ScaleObservation>::vector data_lm;
        for(size_t i = 0; i<common_b_idx.size(); i++){
            ScaleObservation datum;
            FeatureID<G> vbd_f(vbd, common_b_idx[i]);
            FeatureID<G> vcd_f(
                    vcd, 
                    (get(edge_kp_idx_map, g, edge(vbd, vcd, g).first).find(common_b_idx[i]))->second);
            datum.k_proj_measurement = getFeaturePixel(g, vbd_f);
            datum.n_proj_measurement = getFeaturePixel(g, vcd_f);
            datum.T_wk = get(vertex_transform, g, vbd);
            datum.T_kn = scaled_T_vbd_vcd;
            int fix_idx_i = get(vertex_fix_idx, g, vbd)[common_b_idx[i]];
            if(fix_idx_i!=BAD_INDEX && points.getPoint(fix_idx_i).initialized){
                datum.lm_estimate_kk_k = 
                        get(vertex_transform, g, vbd) *points.getPoint(fix_idx_i).global;
                data_lm.push_back(datum);
            } else{
                datum.lm_estimate_kk_k = 
                       get(vertex_transform, g, vbd) * triangulatePointWrtWorldPos(g, vbd_f, vcd_f);
                if(count < 1) data_lm.push_back(datum);
                count++;
            }
            data.push_back(datum);
        }

        //do model estimation
        const double reproj_error_tolerance = 5;
        ScaleReprojectionError error_func;
        typedef MDErrorFuncAdaptor<ScaleReprojectionError> AErrorFuncT;
        AErrorFuncT adapted_error_func(error_func);
        typedef RealErrorMaxErrorAccumulator<AErrorFuncT> ErrorAccumT; 
        ErrorAccumT error_accum(adapted_error_func, reproj_error_tolerance);
        shared_ptr<ModelBuilder<ErrorAccumT::ModelClassT> > mbuilder(
                new ScaleModelBuilder<ErrorAccumT>(error_accum));

        ROS_INFO_STREAM_NAMED("EdgeBulder", "# total common features for scale matching: " <<
                data.size() << " " << data_lm.size());
        //shared_ptr<Model<double, ScaleObservation> > model, best_model;

        typedef Model<double, ScaleObservation> ModelT;
        typedef DataSelector<typename ModelT::DataVec> DataSelectorT;
        //shared_ptr<DataSelectorT> selector(new AllCombinationsSelector<typename ModelT::DataVec>(
        //        data, mbuilder->getSize()));
        shared_ptr<DataSelectorT> selector(new RandomDataSelector<typename ModelT::DataVec>(
                data, mbuilder->getSize(), 100));
        shared_ptr<ModelT> best_model = ranSaC<ErrorAccumT>(mbuilder, selector, data);
        assert(best_model);
        total2 = best_model->getModel();
        ROS_INFO_STREAM_NAMED("EdgeBulder", "# of scale-match inliers: " << 
                best_model->getInlierCount() << " " << best_model->getError());
    } else{
        //fallback
        total2=1.0;
    }
    //save the results or the normalized translation
    SE3 tmp(scaled_T_vbd_vcd.unit_quaternion(),  
            total2 *scaled_T_vbd_vcd.translation());
    scaled_T_vbd_vcd = tmp;
    get(edge_transform, g, edge(convd, newvd, g).first) = scaled_T_vbd_vcd;
    get(edge_transform, g, edge(newvd, convd, g).first) = scaled_T_vbd_vcd.inverse();
    get(vertex_transform, g, newvd) = 
            scaled_T_vbd_vcd * get(vertex_transform, g, convd);
}






template<class G> void EdgeBuilder<G>::featureSelection(
        G& g,
        VertexD vd, 
        int target_num, 
        vector<int>& keep_idxs, 
        vector<cv::KeyPoint>& sel_keypoints,
        cv::Mat& sel_descriptors){
    keep_idxs.clear();
    sel_keypoints.clear();

    nonMaximaSuppresion(get(vertex_kp, g, vd), target_num, keep_idxs);
    sel_keypoints=   getArrayIdices(get(vertex_kp, g, vd), keep_idxs);
    sel_descriptors= getArrayIdices<cv::Mat>(get(vertex_desc, g, vd), keep_idxs);

    ROS_INFO_STREAM_NAMED("EdgeBuilder", "Using " << keep_idxs.size() << 
            " features in image " << printVertexId(g, vd) << " for grobal matching.");
}

template<class G> bool MonoFtrClassEdgeBuilder<G>::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){
    std::vector<cv::DMatch> global_dmatches_c1c2_0;

    RobustFeatureMatcherStats stats;
    rmatcher->symaetricalMatch(sel_tvd_keypoints, sel_qvd_keypoints,
                sel_tvd_descriptors, sel_qvd_descriptors, stats, global_dmatches_c1c2_0);
    ROS_INFO_STREAM_NAMED("EdgeBuilder", "# non-symetrical matches: " << stats.ratio_qt);
    ROS_INFO_STREAM_NAMED("EdgeBuilder", "# symetrical matches: " << stats.symetry);
    ImageMatchVec global_matches_c1c2 = 
            imageMatchCoords(sel_tvd_keypoints, sel_qvd_keypoints, global_dmatches_c1c2_0);
    if(!rmatcher->ransacTest(
            global_matches_c1c2, global_dmatches_c1c2_0, stats, global_dmatches_c1c2, T_tvd_qvd)){
        ROS_WARN_NAMED("PoseGraph", "Model estimation failed. RanSac did not return model.");
        return false;
    } else{
        ROS_INFO_STREAM_NAMED("PoseGraph.EdgeBuilder", "# RanSaC-filtered matches: " << stats.ransac);
        return true;
    }
}


template<class ErrorAccumT> typename ErrorAccumT::ErrorT outlierRemoval(
        const Matrix3d& E,
        const ErrorAccumT& ea, 
        const ImageMatchVec& matches_c1c2, 
        const vector<cv::DMatch>& dmatches_c1c2,
        vector<cv::DMatch>& filtered_matches_c1c2){ 
    ALIGNED<Matrix3d>::vector Ematrices;
    Ematrices.push_back(E);
    shared_ptr<EMatrixModel> model = 
            DummyModelBuilder<ErrorAccumT>( ea, Ematrices)(ImageMatchVec());
    model->compute(matches_c1c2);
    MaskVec::const_iterator itIn= model->getInlierMask().begin();
    std::vector<cv::DMatch>::const_iterator itM = dmatches_c1c2.begin();
    while(itIn!= model->getInlierMask().end()) {
        if (*itIn) { // it is a valid match
            filtered_matches_c1c2.push_back(*itM);
        }
        ++itIn, ++itM;
    }
    return model->getError();
}


template<class G> void MonoFtrClassEdgeBuilder<G>::guidedMatching(
        G& g,
        VertexD qvd, 
        VertexD tvd){ 
    ROS_INFO_NAMED("EdgeBuilder", "Starting local matching."); 
    PerformanceLogger<G> pl(g, qvd, "local_matching");
    typename property_map<G, vertex_kp_t>::type vkp_pmap = get(vertex_kp, g);
    vector<cv::DMatch> local_dmatches0, local_dmatches;
    vector<int> retain_idxs;
    findLocalMatchesSO3(g, tvd, qvd, rmatcher, Vector2d(60, 60), false, local_dmatches);
    if(local_dmatches.size() > 0){
        for(size_t i = 0 ; i< local_dmatches.size();i++){
            if(filter_index_pair_duplicates(local_dmatches[i], tvd, qvd, g)){
                retain_idxs.push_back(i);
            }
        }
        local_dmatches=getArrayIdices(local_dmatches, retain_idxs);
        ROS_INFO_STREAM_NAMED("PoseGraph.EdgeBuilder", 
                "# non-symetrical matches (excl global): " << local_dmatches.size());
        ImageMatchVec matches_c1c2 = 
                imageMatchCoords(vkp_pmap[tvd], vkp_pmap[qvd], local_dmatches);
        RealErrorMaxErrorAccumulator<AngleError> ea(AngleError(), ((M_PI / 180.0)*1.0)); 
        SE3 T_tvd_qvd = getEdgeTransform(g, tvd, qvd);
        outlierRemoval(
                sE3ToE(T_tvd_qvd), 
                ea, 
                matches_c1c2, 
                local_dmatches, 
                local_dmatches0);
        std::for_each(local_dmatches0.begin(), local_dmatches0.end(), std::tr1::bind(
                add_idx_pair_from_dmatch<G>, std::tr1::placeholders::_1, tvd, qvd, 
                std::tr1::ref(g)));
        int tmp = get(edge_kp_idx_map, g, edge(qvd, tvd, g).first).size();
        ROS_INFO_STREAM_NAMED("EdgeBuilder", 
                "# matches satisfying 2-view constraint (excl. global): " <<  local_dmatches0.size());
        ROS_INFO_STREAM_NAMED("EdgeBuilder", 
                "# matches satisfying 2-view constraint (incl. global): " <<  tmp);
        get(vertex_debug_info, g, qvd).double_propmap["num_2view_match_kpts_tot"]=tmp;
        get(vertex_debug_info, g, qvd).double_propmap["num_2view_match_kpts_loc"]=
                local_dmatches0.size();

    } else{
        ROS_INFO_NAMED("PoseGraph", "Did not find any new mactehs.");
    }
}





//documented in header
template<class G> bool MonoFtrClassEdgeBuilder<G>::buildImpl(
        const VertexD & convd, const VertexD & newvd, Points<G> & points, G & g) {
    using namespace boost;
    ROS_INFO_STREAM_NAMED("PoseGraph", "Constructing edge (" << printVertexId(g, convd) << 
            ", " << printVertexId(g, newvd) << ").");
    PerformanceLogger<G> edge_builder_pl(g, newvd, "edge_builder");

    std::vector<cv::DMatch> global_dmatches_con_new,dmatches_con_new_0; 
    SE3 T_con_new;

    //feature selection
    vector<int> gm_convd_keep_idx, gm_newvd_keep_idx;
    vector<cv::KeyPoint> gm_convd_keypoints, gm_newvd_keypoints;
    cv::Mat gm_newvd_descriptors, gm_convd_descriptors;
    {
        PerformanceLogger<G> pl(g, newvd, "feature_selection");
        EdgeBuilder<G>::featureSelection(
                g, convd, 700, gm_convd_keep_idx, gm_convd_keypoints, gm_convd_descriptors);
        EdgeBuilder<G>::featureSelection(
                g, newvd, 700, gm_newvd_keep_idx, gm_newvd_keypoints, gm_newvd_descriptors);
    }

    //global unguided on selected fetures + model estimation from computed matches.
    {
        PerformanceLogger<G> edge_builder_pl(g, newvd, "global_matching");
        if(!unguidedMatching(gm_convd_keypoints,  gm_newvd_keypoints,
                    gm_convd_descriptors, gm_newvd_descriptors, T_con_new, global_dmatches_con_new)){
            return false;
        }
    }
    {
        bool status;
        status=add_edge(newvd, convd, g).second;
        status=status && add_edge(convd, newvd, g).second;
        assert(status);
    }
    setEdgeTransform(T_con_new, convd, newvd, g);
    dmatches_con_new_0=remapDMatchIdxs(global_dmatches_con_new, gm_convd_keep_idx, gm_newvd_keep_idx);
    std::for_each(dmatches_con_new_0.begin(), dmatches_con_new_0.end(), std::tr1::bind(
            add_idx_pair_from_dmatch<G>, std::tr1::placeholders::_1, convd, newvd, std::tr1::ref(g)));
    
    //guided matching
    guidedMatching(g, newvd, convd);

    if(minEdgeDisparityAngle(g, convd, newvd) < (M_PI/180.0)*1.5){ //XXX
        ROS_WARN_NAMED("PoseGraph", "edge construction failed. Disparity too small.");
        ROS_WARN_STREAM_NAMED("PoseGraph", "disparity:" << 
            minEdgeDisparityAngle(g, convd, newvd) / M_PI * 180.0);
        remove_edge(newvd, convd, g);
        remove_edge(convd, newvd, g);
        return false;
    }
    ROS_WARN_STREAM_NAMED("PoseGraph", "disparity:" << 
            minEdgeDisparityAngle(g, convd, newvd) / M_PI * 180.0);
    

    SE3 scaled_T_vsrc_vdest = get(edge_transform, g, edge(convd, newvd, g).first);
    computeScale(convd, newvd, g, points, scaled_T_vsrc_vdest);

    //update landmarks
    {
        PerformanceLogger<G> pl(g, newvd, "lm_accounting");
        findNewLandmarks(convd, newvd, points, g); //XXX
        //matchScaleFindNewLandmarks(convd, newvd, points, g); //XXX
        extendLandmarks(convd, newvd, points, g);
    }
    ROS_INFO_STREAM_NAMED("PoseGraph", "Done constructing edge");
    return true;
}

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

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){ 
    return std::tr1::shared_ptr<RobustFeatureMatcher>(
        new RobustFeatureMatcher( _dmatcher, _minbuilder, _builder));
}



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





}//namespace vslam
#endif