model_estimation.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_MODEL_ESTIMATION_H_INCLUDED
#define VSLAM_MODEL_ESTIMATION_H_INCLUDED

#include "model.h"
#include "error_func.h"
#include "utils.h"
#include <Eigen/Dense>
#include "sfm_utils.h"
#include "lihartley5pt/interface.h"

/*------------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------------*/
struct ImageMatchClass{
    typedef ALIGNED<ImageMatchClass>::vector ImageMatchClassVec;
    const Vector3d im_c1, im_c2;
    ImageMatchClass(const Vector3d& a, const Vector3d& b) : im_c1(a), im_c2(b) { }
    ImageMatch getAsImageMatch(){
        return std::make_pair(im_c1, im_c2);
    }
};


template<class ErrorAccumClassT> 
        class Point3dModelFromLandmarkObservationPairBuilder : public GeneralizedModelBuilder<ErrorAccumClassT>{
    public:
        typedef Model<Vector3d, LandmarkObservation> Point3dModel;

        Point3dModelFromLandmarkObservationPairBuilder(const ErrorAccumClassT& _error_accum) :
                GeneralizedModelBuilder<ErrorAccumClassT>(_error_accum) { }

        virtual shared_ptr<Point3dModel> operator()(const Point3dModel::DataVec & selected){
                assert(selected.size()==getSize());
                LandmarkObservation oa = selected[0];
                LandmarkObservation ob = selected[1];
                Vector3d pt_c1; 
                Vector3d oa_bproj, ob_bproj;
                oa_bproj = PanoImCoord::fromPixel(oa.pixel).getHomogenous();
                ob_bproj = PanoImCoord::fromPixel(ob.pixel).getHomogenous();
                computeStructureForcePositiveDepth( ob.T_cw.inverse() * oa.T_cw, 
                        ImageMatchClass(oa_bproj, ob_bproj).getAsImageMatch(), pt_c1);
                Vector3d pt_w = oa.T_cw * pt_c1;
                return shared_ptr<Point3dModel>(new SingleSolutionModel<ErrorAccumClassT>(
                            getSize(), selected, pt_w, this->error_accum));
            }
        virtual unsigned int getSize() const { return 2; }
};

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

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


class ScaleObservation{
    public:
        Vector2d k_proj_measurement;
        Vector2d n_proj_measurement;
        Vector3d lm_estimate_kk_k;
        SE3 T_wk; 
        SE3 T_kn; 
    //ScaleObservation(const G& g, const FeatureID<G>& fid, const SE3& T_kn){}
};



class ScaleReprojectionError : public DatumErrorFunc<double, ScaleObservation, Vector2d> {
    private:
        LandmarkToPoseReprojectionError lm_reprojection_error;
    public:
        virtual Vector2d operator()(const double& model, const ScaleObservation& datum) const{
            SE3 T_kn(datum.T_kn.unit_quaternion(), model * datum.T_kn.translation());    
            LandmarkObservation lm_obs((T_kn * datum.T_wk).inverse(), datum.n_proj_measurement);
            return lm_reprojection_error(datum.T_wk.inverse() * datum.lm_estimate_kk_k, lm_obs);
        }
};


template<class ErrorAccumClassT> 
    class ScaleModelBuilder : public GeneralizedModelBuilder<ErrorAccumClassT>{
        public:
            typedef typename ErrorAccumClassT::ModelClassT ScaleModel;

            ScaleModelBuilder(const ErrorAccumClassT& _error_accum) : 
                GeneralizedModelBuilder<ErrorAccumClassT>(_error_accum) { }

            virtual shared_ptr<ScaleModel> operator()(const typename ScaleModel::DataVec & selected){
                assert(selected.size()==this->getSize());
                ScaleObservation datum = selected[0];
                Vector3d estimate_kn_k;
                computeStructureForcePositiveDepth(
                        datum.T_kn, 
                        computeMatch(datum.k_proj_measurement, datum.n_proj_measurement),
                        estimate_kn_k);
                return shared_ptr<ScaleModel>(new SingleSolutionModel<ErrorAccumClassT>(
                        getSize(), 
                        selected, 
                        (datum.lm_estimate_kk_k.norm() / estimate_kn_k.norm()),
                        this->error_accum));
            }

            virtual unsigned int getSize() const { return 1; }
};

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

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

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



template<class ContainerT> class DataSelector{
    public:
        virtual shared_ptr<ContainerT> select() = 0;
};


template<class ContainerT> class RandomDataSelector : public DataSelector<ContainerT>{
    const ContainerT data;
    int model_size;
    int num_iterations;
    int iterations;

    public:
        RandomDataSelector(const ContainerT& _data, int _model_size, int _num_iterations) : 
                data(_data), model_size(_model_size) {
            num_iterations=_num_iterations;        
            iterations=0;
        }

        virtual shared_ptr<ContainerT> select(){
            if(iterations > num_iterations) return shared_ptr<ContainerT>();

            std::set<int> selected_idx;
            assert(static_cast<int>(data.size())>=model_size);
            while(static_cast<int>(selected_idx.size()) < model_size){
                boost::uniform_int<> interval(0,data.size() - 1);
                selected_idx.insert(interval(rng_mt19937));
            }
            shared_ptr<ContainerT> selected(new ContainerT());
            for(std::set<int>::iterator it=selected_idx.begin(); it!=selected_idx.end(); it++){
                selected->push_back(data[*it]);
            }
            ++iterations;
            return selected;
        }
};

template<class ContainerT> class PairSelectorFromTwoSets : public DataSelector<ContainerT>{
    private:
        const ContainerT data1, data2;
        typename ContainerT::const_iterator it1, it2;
        bool past_end;

        PairSelectorFromTwoSets(const ContainerT& _data1, const ContainerT& _data2) :
                data1(_data1), data2(_data2){
            it1=data1.begin();
            it2=data2.begin();
            past_end=false;
        }

        virtual shared_ptr<ContainerT> select(){
            if(past_end) return 0;

            shared_ptr<ContainerT> selected_data(new ContainerT());
            selected_data->push_back(*it1);
            selected_data->push_back(*it2);

            it2++;
            if(it2==data2.end()){
                it1++;
                if(it1==data1.end()){
                    past_end=true;
                } else{
                    it2=data2.begin();
                }
            }

            return selected_data;
        }
};


template<class ContainerT> class AllCombinationsSelector : public DataSelector<ContainerT>{
    private:
        const ContainerT data;
        std::deque<typename ContainerT::const_iterator> state;
        bool past_end;
        int model_size;

        typedef typename ContainerT::const_iterator T;

        void next(){
            T it = state.back();
            state.pop_back();
            ++it;
            if(it == data.end()){
                if(state.size()==0){
                    past_end=true;
                } else{
                    next();
                }
                state.push_back(data.begin());
            } else{
                state.push_back(it);
            }
        }
    public:
        AllCombinationsSelector(const ContainerT& _data, int _model_size) :
                data(_data), model_size(_model_size){
            assert(static_cast<int>(data.size()) > model_size);
            for(int i=0; i < model_size;i++){
                typename ContainerT::const_iterator it = data.begin();
                for(int j=0; j<i; j++){
                    it++;
                }
                state.push_back(it);
            }
            past_end=false;
        }

        virtual shared_ptr<ContainerT> select(){
            if(past_end){
                return shared_ptr<ContainerT>();
            } else{
                shared_ptr<ContainerT> selected_data(new ContainerT());
                BOOST_FOREACH(T it, state){
                    selected_data->push_back(*it);
                }
                next();
                return selected_data;
            }
        }
};

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


template<class ErrorAccumClassT> class RanSaCOptions{
    private:
        int iterations;
        shared_ptr<double> inlier_ratio;
        shared_ptr<int> min_inliers;
    public:
        RanSaCOptions(int _iterations, double _inlier_ratio = 0.35) : 
                iterations(_iterations), inlier_ratio(new double(_inlier_ratio)) {
            assert(_inlier_ratio < 1.0);
        }

        RanSaCOptions(int _iterations, int _min_inliers) :
                iterations(_iterations), 
                min_inliers(new int(_min_inliers)) {}

        int getMinInliers(
                shared_ptr<ModelBuilder<ErrorAccumClassT> > mbuilder, int data_size) const{
            int val;
            if(min_inliers){
                val = *min_inliers;
            } else{
                val = static_cast<int>(data_size * *inlier_ratio);
            }
            val = std::max(val, mbuilder->getSize());
            return val;
        }

        int getIterations() const{
            return iterations;
        }
};


template<class ErrorAccumClassT> shared_ptr<typename ErrorAccumClassT::ModelClassT> ranSaC(
        shared_ptr<ModelBuilder<typename ErrorAccumClassT::ModelClassT> > model_builder,
        shared_ptr<DataSelector<typename ErrorAccumClassT::ModelClassT::DataVec> > data_selector,
        const typename ErrorAccumClassT::ModelClassT::DataVec & data){
    typedef typename ErrorAccumClassT::ModelClassT ModelT;
    assert(model_builder);
    assert(data_selector);
    assert(data.size()>= model_builder->getSize());
    shared_ptr<ModelT> model, best_model;
    shared_ptr<typename ModelT::DataVec> selected = data_selector->select();
    while(selected){
        shared_ptr<ModelT> model = model_builder->operator()(*selected);
        if(model){ 
            model->compute(data);
            if (!best_model || (best_model->getInlierCount() < model->getInlierCount())){
                best_model = model;
            }
        }
        selected = data_selector->select();
    }
    return best_model;
}

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

/*------------------------------------------------------------------------------------------------*/
#endif