landmark_manager.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_LANDMARK_MANAGER_H_INCLUDED
#define VSLAM_LANDMARK_MANAGER_H_INCLUDED
#include "utils.h"
#include "pose_graph.h" 
#include "graph_utils.h"  
#include "error_func.h"  // Used by RanSaC in feature-landmark association (Landmark Manager).

#include <algorithm>
#include <tr1/functional>


namespace vslam{
/*------------------------------------------------------------------------------------------------*/
template<typename G> struct Point{
    typedef typename graph_traits<G>::vertex_descriptor VertexD;
    typedef pair<VertexD, int> Feature;
    typedef std::vector<Feature> FeatureCont;

    FeatureCont seen_from; 
    Vector3d global;       
    bool initialized;      
    bool usable;           
};





namespace lmdetails{
/*------------------------------------------------------------------------------------------------*/

template<typename G> class PointsLoc{
    protected:
        vector<Point<G> > pts;
        typedef typename graph_traits<G>::vertex_descriptor VertexD;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        void destroy(int idx, G&g){
            getPoint(idx).initialized=false;
            getPoint(idx).global=Vector3d::Zero();
            typedef typename Point<G>::Feature T;
            BOOST_FOREACH(T pf, getPoint(idx).seen_from){
                vector<int> & fixed_points = get(vertex_fix_idx, g, pf.first);
                fixed_points[pf.second]=BAD_INDEX;
            }
            getPoint(idx).seen_from.clear();
        }

        int create(const G& g, const vector<FeatureID<G> >& observations, 
                const Vector3d& estimate);

        void addObservation(const G&g, int idx, FeatureID<G>& f, const Vector3d& estimate);

        Point<G> & getPoint(int idx){
            return pts[idx];
        }

        const Point<G> & getPoint(int idx) const{
            return pts[idx];
        }
        
        int size() const { return pts.size(); }
};



template<class G> int PointsLoc<G>::create(const G& g, const vector<FeatureID<G> >& observations, 
        const Vector3d& estimate){
    Point<G> pt;
    vector<LandmarkObservation> lm_obs;
    BOOST_FOREACH(FeatureID<G> f, observations){
        pt.seen_from.push_back(make_pair(f.vd, f.vidx));
        lm_obs.push_back(LandmarkObservation(get(vertex_transform, g, f.vd).inverse(), 
                    getFeaturePixel(g, f))); 
    }
    pt.initialized=false;
    pt.usable=true;
    BOOST_FOREACH(LandmarkObservation tmp, lm_obs){
        if(!isEstimateInFrontOfCamera(tmp, estimate)) pt.usable=false; 
    }
    FeatureID<G> ff = FeatureID<G>(pt.seen_from.begin()->first, pt.seen_from.begin()->second);
    FeatureID<G> fl = FeatureID<G>(pt.seen_from.rbegin()->first, pt.seen_from.rbegin()->second);
    if(disparityAngle(g, ff, fl) < (M_PI/180.0)*3.0){ 
        pt.usable=false;
    }
    pt.global=estimate;
    pts.push_back(pt);
    return pts.size() - 1;
}

template<class G> void PointsLoc<G>::addObservation(
        const G&g, int idx, FeatureID<G>& f, const Vector3d& estimate){
    typename Point<G>::FeatureCont & seen_from_idx = pts[idx].seen_from;
    seen_from_idx.push_back(make_pair(f.vd, f.vidx));

    pts[idx].global=estimate;
    if(seen_from_idx.size()>=4){
        pts[idx].usable=true;
    } else{
        Point<G> pt = pts[idx];
        FeatureID<G> ff = FeatureID<G>(pt.seen_from.begin()->first, pt.seen_from.begin()->second);
        FeatureID<G> fl = FeatureID<G>(pt.seen_from.rbegin()->first, pt.seen_from.rbegin()->second);
        if(disparityAngle(g, ff, fl) > (M_PI/180.0)*3.0){
            pt.usable=true;
        }
    }
}






/*------------------------------------------------------------------------------------------------*/
template<class G> class LandmarkQualityOrdering{
    protected:
        const G & g;
        const PointsLoc<G> & points;

    public:
        LandmarkQualityOrdering(const G& _g, const PointsLoc<G>& _points) : g(_g), points(_points) {}

        bool operator()(int lm_idx_a, int lm_idx_b){
            return lm_idx_a > lm_idx_b;
        }
};


template<class G> double ftrTrackEndToEndDisparity(const G&g, const Point<G>& pt){
    FeatureID<G> ff = FeatureID<G>(pt.seen_from.begin()->first, pt.seen_from.begin()->second);
    FeatureID<G> fl = FeatureID<G>(pt.seen_from.rbegin()->first, pt.seen_from.rbegin()->second);
    return disparityAngle(g, ff, fl);
}

template<class G> class DisparityLmOrdering : public LandmarkQualityOrdering<G>{
    public:
        DisparityLmOrdering(const G& _g, const PointsLoc<G>& _points) : 
                LandmarkQualityOrdering<G>(_g, _points) {}

        bool operator()(int lm_idx_a, int lm_idx_b){
            if(this->points.getPoint(lm_idx_a).usable){
                if(this->points.getPoint(lm_idx_b).usable){
                    double disparity_a = ftrTrackEndToEndDisparity(
                            this->g, this->points.getPoint(lm_idx_a));
                    double disparity_b = ftrTrackEndToEndDisparity(
                            this->g, this->points.getPoint(lm_idx_b));
                    return disparity_a > disparity_b;
                } else{
                    return true;
                }
            } else{
                if(this->points.getPoint(lm_idx_b).usable){
                    return false;
                } else{
                    return lm_idx_a > lm_idx_b;
                }
            }
        }
};


template<class G> void pruneLandmarks(
        const G&g, PointsLoc<G>& points, typename graph_traits<G>::vertex_descriptor v, int keep_num){
    vector<int> visible_lm_idxs;  
    BOOST_FOREACH(int fix_idx, get(vertex_fix_idx, g, v)){
        if(fix_idx!=BAD_INDEX){
            visible_lm_idxs.push_back(fix_idx);
        }
    }

    std::sort(visible_lm_idxs.begin(), visible_lm_idxs.end(), DisparityLmOrdering<G>(g, points));
    vector<int>::const_iterator idx_it = visible_lm_idxs.begin();
    for(int j=0; (j< keep_num) && idx_it!=visible_lm_idxs.end(); j++){
        if(points.getPoint(*idx_it).usable){
            points.getPoint(*idx_it).initialized = true;
        } else{
            keep_num++;
        }
        ++idx_it;
    }
}
 //end of group LmM__pruning
/*------------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------------*/

#define  VSLAM_REPROJ_ERROR_TOLERANCE 5.0 //XXX



typedef ALIGNED<LandmarkObservation>::vector LandmarkObservationVec;

typedef Model<Vector3d, LandmarkObservation> PointFromLandmarkObservationsModel;

inline bool landmarkObservationSetConsistency(
        const LandmarkObservationVec& observations1,
        const LandmarkObservationVec& observations2, 
        const LandmarkObservationVec& all_observations, 
        Vector3d& global_est){
    typedef LandmarkToPoseReprojectionError T0;
    typedef MDErrorFuncAdaptor<T0> T1;
    typedef RealErrorMaxErrorAccumulator<T1> T2;
    shared_ptr<ModelBuilder<PointFromLandmarkObservationsModel> > mbuilder;
    T0 error_func;
    T1 adapted_error_func = T1(error_func);
    T2 error_acum = T2(adapted_error_func, VSLAM_REPROJ_ERROR_TOLERANCE);
    mbuilder.reset(new Point3dModelFromLandmarkObservationPairBuilder<T2>(error_acum));
    shared_ptr<SingleSolutionModel<T2> > initial_test_model;
    ALIGNED<LandmarkObservation>::vector tmp_vec;
    initial_test_model.reset(new SingleSolutionModel<T2>(0, tmp_vec,global_est,error_acum));
    initial_test_model->compute(all_observations);
    if(initial_test_model->getInlierCount()==(int)all_observations.size()){
        return true;
    }
    shared_ptr<PointFromLandmarkObservationsModel> model, best_model;
    LandmarkObservationVec selected_data;
    BOOST_FOREACH(LandmarkObservation obs1, observations1){
        selected_data.push_back(obs1);
        BOOST_FOREACH(LandmarkObservation obs2, observations2){
            if(!(obs1 == obs2)){
                selected_data.push_back(obs2);
                model=mbuilder->operator()(selected_data);
                model->compute(all_observations);
                if(model->getInlierCount()==(int)all_observations.size()){
                    if(!best_model || model->getError() < best_model->getError()) best_model=model;
                }
                selected_data.pop_back();
            }
        }
        selected_data.pop_back();
    }
    if(best_model){
        global_est = best_model->getModel();
        return true;
    } else{
        return false;
    }
}



template<class G> bool consistencyCheck(
        const G& g, FeatureID<G> f1, FeatureID<G> f2, FeatureID<G> f3, Vector3d& estimate){
    LandmarkObservationVec obs;
    obs.push_back(LandmarkObservation(get(vertex_transform, g, f1.vd).inverse(), 
            getFeaturePixel(g, f1)));
    obs.push_back(LandmarkObservation(get(vertex_transform, g, f2.vd).inverse(), 
            getFeaturePixel(g, f2)));
    obs.push_back(LandmarkObservation(get(vertex_transform, g, f3.vd).inverse(), 
            getFeaturePixel(g, f3)));
    return landmarkObservationSetConsistency(obs, obs, obs, estimate);
}


template<class G>  bool consistencyCheck(
        const G& g, 
        const Points<G>& points, 
        FeatureID<G> lmfid, 
        FeatureID<G> nonlmfid, 
        Vector3d& estimate){
    typename property_map<G, vertex_fix_idx_t>::const_type vfixmap = get(vertex_fix_idx, g);

    if((vfixmap[lmfid.vd][lmfid.vidx]==BAD_INDEX) && 
            (vfixmap[nonlmfid.vd][nonlmfid.vidx]!=BAD_INDEX)){
        assert(false); 
        return false;
    } else{
        typedef pair<typename graph_traits<G>::vertex_descriptor, int> T1;
        Point<G> point=points.getPoint(vfixmap[lmfid.vd][lmfid.vidx]);
        LandmarkObservationVec obs;
        BOOST_FOREACH(T1 pt, point.seen_from){
            obs.push_back(LandmarkObservation(
                    get(vertex_transform, g, pt.first).inverse(), 
                    getFeaturePixel(g, FeatureID<G>(pt.first, pt.second))));
        }
        obs.push_back(LandmarkObservation(
                get(vertex_transform, g, nonlmfid.vd).inverse(), 
                getFeaturePixel(g, nonlmfid)));
        if(landmarkObservationSetConsistency(obs, obs, obs, estimate)){
            assert(vfixmap[nonlmfid.vd][nonlmfid.vidx]==BAD_INDEX);
            return true;
        } else{
            return false;
        }
    }
}  //group LmM__impl__consist ends here.
/*------------------------------------------------------------------------------------------------*/  //group LmM__impl ends here.
/*------------------------------------------------------------------------------------------------*/
}//namespace lmdetails

/*------------------------------------------------------------------------------------------------*/
template<class G> void Points<G>::activateLandmarks(
        const G&g, typename graph_traits<G>::vertex_descriptor v, int keep_num){
    lmdetails::pruneLandmarks(g, points, v, keep_num);
}

template<class G> const Point<G> & Points<G>::getPoint(int idx) const{
    return points.getPoint(idx);
}


template<class G> void Points<G>::lmEstimateUpd(int lm_id, Vector3d& new_estimate){
    points.getPoint(lm_id).global=new_estimate;
}


template<class G> void Points<G>::lmEstimateUpdChecked(int lm_id, Vector3d& new_estimate){
    this->lmEstimateUpd(lm_id, new_estimate);
}


template<class G> void Points<G>::deleteLmObs(G&g, const FeatureID<G>& obs){
    Point<G> pt = points.getPoint(get(vertex_fix_idx, g, obs.vd)[obs.vidx]);
    
    assert(pt.seen_from.size());
    vector<int> & fixed_points = get(vertex_fix_idx, g, obs.vd);

    int to_delete_idx=BAD_INDEX; int to_delete_kp_idx=BAD_INDEX;
    for(size_t i=0; i<pt.seen_from.size(); i++){
        if(pt.seen_from[i].first==obs.vd){
            to_delete_idx=i;
            to_delete_kp_idx=pt.seen_from[i].second;
        }
    }
    assert(to_delete_idx!=BAD_INDEX && to_delete_kp_idx!=BAD_INDEX);

    if((int)(pt.seen_from.size()-1)!=to_delete_idx){
        pt.seen_from[to_delete_idx]=pt.seen_from[pt.seen_from.size()-1];
    }
    fixed_points[to_delete_kp_idx]=BAD_INDEX;
    pt.seen_from.pop_back();

    if(pt.seen_from.size()==1){
        vector<int> & fixed_points2 = get(vertex_fix_idx, g, pt.seen_from[0].first);
        fixed_points2[pt.seen_from[0].second]=BAD_INDEX;
        pt.seen_from.clear();
        pt.initialized=false;
        pt.usable=false;
    }
}


template<class G> int Points<G>::size() const{
    return points.size();
}



template<typename G> bool Points<G>::fixNewLandmark(
        FeatureID<G> fa,
        FeatureID<G> fm,
        FeatureID<G> fb,
        G& g){
    using namespace boost;
    using namespace lmdetails;
    typename property_map<G, vertex_fix_idx_t>::type vm = get(vertex_fix_idx, g);
    bool c1,c2,c3;
    c1=vm[fa.vd][fa.vidx] == BAD_INDEX;
    c2=vm[fm.vd][fm.vidx] == BAD_INDEX;
    c3=vm[fb.vd][fb.vidx] == BAD_INDEX;
    if(!(c1 && c2 && c3)) return false;
    Vector3d estimate;
    if(consistencyCheck(g, fa, fm, fb, estimate)){
        vector<FeatureID<G> > fs;
        fs.push_back(fa); fs.push_back(fm); fs.push_back(fb);
        int tmp = points.create(g, fs, estimate); 
        vm[fm.vd][fm.vidx] = tmp;
        vm[fa.vd][fa.vidx] = tmp;
        vm[fb.vd][fb.vidx] = tmp;
        return true;
    } else{
        return false;
    }
}

template<typename G> bool Points<G>::fixNewObservation(
        FeatureID<G> fa, FeatureID<G> fb, G& g){
    using namespace boost;
    using namespace lmdetails;
    typename property_map<G, vertex_fix_idx_t>::type vm = get(vertex_fix_idx, g);

    if(vm[fa.vd][fa.vidx] == BAD_INDEX){
        if(vm[fb.vd][fb.vidx] == BAD_INDEX){
            assert(false);
        } else{
            return this->fixNewObservation(fb, fa, g);
        }
    } else{
        Vector3d estimate;
        if(points.getPoint(vm[fa.vd][fa.vidx]).initialized){ 
            estimate= points.getPoint(
                    vm[fa.vd][fa.vidx]).global; //set old estimate.
        } else{
            estimate = Vector3d(1.0, 1.0, 1.0); 
        }
        if(consistencyCheck<G>(g, *this, fa, fb, estimate)){
            points.addObservation(g, vm[fa.vd][fa.vidx], fb, estimate);
            vm[fb.vd][fb.vidx] = vm[fa.vd][fa.vidx];
            return true;
        } else{
            return false;
        }
    }
    assert(false);
    return false;
}


template<class G> void findNewLandmarks(
        typename graph_traits<G>::vertex_descriptor convd, 
        typename graph_traits<G>::vertex_descriptor newvd, 
        Points<G> & points, 
        G & g){
    using namespace boost;
    typename graph_traits<G>::adjacency_iterator vi, vi_end;
    int new_track_count=0;
    //find new landmarks
    for(tie(vi, vi_end) = adjacent_vertices(convd , g); vi!=vi_end; ++vi){
        if(*vi != newvd){
            vector<int> common_ftr_convd = common_feature_tracks<vector>(g, *vi, convd, newvd);
            vector<int>::const_iterator common_ftr_convd_it = common_ftr_convd.begin();
            while(common_ftr_convd_it!=common_ftr_convd.end()){
                FeatureID<G> convdf = FeatureID<G>(convd, *common_ftr_convd_it);
                FeatureID<G> newvdf = FeatureID<G>(newvd, getFeatureIdx(g, convdf, newvd));
                FeatureID<G> vif = FeatureID<G>(*vi, getFeatureIdx(g, convdf, *vi));

                //new landmark with 3 features
                if(!isLandmakFeature(g, vif) && !isLandmakFeature(g, convdf) && 
                        !isLandmakFeature(g, newvdf)){
                    if(points.fixNewLandmark(vif, convdf, newvdf, g)){
                        new_track_count++;
                    }
                }
                ++common_ftr_convd_it;
            }
        }
    }
    std::string num_new_lms_key="num_new_lms";
    map<std::string, double> & pmap = get(vertex_debug_info, g, convd).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, newvd).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, convd).double_propmap["tracks_new"]=new_track_count;
}

template<class G> void extendLandmarks(
        typename graph_traits<G>::vertex_descriptor convd, 
        typename graph_traits<G>::vertex_descriptor newvd, 
        Points<G> & points, 
        G & g){
    int old_track_count=0;
    int deleted=0;
    const map<int, int> & sdmap = get(edge_kp_idx_map, g, edge(convd, newvd, g).first);
    for(map<int, int>::const_iterator it = sdmap.begin(); it != sdmap.end(); ++it){
        FeatureID<G> convdf = FeatureID<G>(convd, it->first);
        FeatureID<G> newvdf = FeatureID<G>(newvd, it->second);
        
        // (convdf, newvdf) extends convdf landmark
        if(isLandmakFeature(g, convdf) && !isLandmakFeature(g, newvdf)){
            if(points.fixNewObservation(convdf, newvdf, g)){
                old_track_count++;
            } else{
                deleted++;
            }
        }
    }
    ROS_INFO_STREAM_NAMED("EdgeBulder", "# of tracks that ceased: " << deleted);
    ROS_INFO_STREAM_NAMED("EdgeBulder", "# of tracks extended: " << old_track_count);
    if(deleted && old_track_count){
        get(vertex_debug_info, g, convd).double_propmap["tracks_ceased"]=deleted;
        get(vertex_debug_info, g, convd).double_propmap["tracks_extended"]=old_track_count;
    }
} //end of group LmM__interface //end of group LmM
/*------------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------------*/
} //end of namespace vslam

#endif