model.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_H_INCLUDED
#define VSLAM_MODEL_H_INCLUDED

typedef std::vector<bool> MaskVec; 

template<class ModelT, class DatumT> class Model{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        typedef typename ALIGNED<DatumT>::vector DataVec;
        typedef ModelT ModelRep;
        typedef DatumT Datum;
    protected:
        const unsigned int size;
        const DataVec model_data;
        ModelT model;
        double error;
        double all_data_error;
        vector<bool> inlier_mask;
        typename DataVec::size_type inlier_count;
    public:
        Model(int _size, const DataVec& _model_data, const ModelT& _model) : 
                size(_size), model_data(_model_data), model(_model) {}

        virtual void compute(const DataVec & image_matches) = 0;

        double getError() const{
            assert(inlier_mask.size() > 0);
            return all_data_error;
        }

        double getInlierError() const{
            assert(inlier_mask.size() > 0);
            return error;
        }

        const vector<bool> & getInlierMask() const{
            assert(inlier_mask.size() > 0);
            return inlier_mask;
        }

        int getInlierCount() const{
            assert(inlier_mask.size() > 0);
            return inlier_count;
        }

        DataVec getInliers(const DataVec & all_data){
            assert(inlier_mask.size() > 0);
            DataVec inliers(inlier_count);
            typename DataVec::iterator inliers_it = inliers.begin();
            typename DataVec::const_iterator it = all_data.begin();
            typename MaskVec::const_iterator it_mask = inlier_mask.begin();
            while(it!=all_data.end()){
                if(*it_mask) *inliers_it = *it;
                ++it; ++it_mask;
            }
            return inliers;
        }

        int getSize() const{
            return size;
        }

        const ModelT & getModel() const{
            return model;
        }
        const DataVec& getModelData() const{
            return model_data;
        }

};

typedef Model<Matrix3d, ImageMatch> EMatrixModel;


template<class ErrorAccumulatorT> class SingleSolutionModel : public 
        Model<typename ErrorAccumulatorT::ModelRep, typename ErrorAccumulatorT::DatumT>{
    protected:
        ErrorAccumulatorT error_acc;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef Model<typename ErrorAccumulatorT::ModelRep, typename ErrorAccumulatorT::DatumT> 
                ModelClassT;

        SingleSolutionModel(
                int _size, 
                const typename ModelClassT::DataVec& _model_data, 
                const typename ErrorAccumulatorT::ModelRep& _model, 
                const ErrorAccumulatorT& _error) : 
                Model<typename ErrorAccumulatorT::ModelRep, typename ErrorAccumulatorT::DatumT>(
                        _size, _model_data, _model), error_acc(_error) {}

        virtual void compute(const typename ModelClassT::DataVec & all_data){
            this->inlier_mask.resize(all_data.size());
            this->error=0;
            this->all_data_error=0;
            this->inlier_count=0;
            typename ModelClassT::DataVec::const_iterator all_data_it = all_data.begin();
            vector<bool>::iterator inlier_mask_it = this->inlier_mask.begin();
            while(all_data_it!=all_data.end()){
                assert(inlier_mask_it != this->inlier_mask.end());
                double tmp = error_acc.computeError(this->getModel(), *all_data_it);
                if(error_acc.isInlier(tmp)){
                    *inlier_mask_it = true;
                    this->error = error_acc.errorAccumulator(this->error, tmp);
                    this->inlier_count++;
                } else{
                    *inlier_mask_it = false;
                }
                this->all_data_error = error_acc.errorAccumulator(this->all_data_error, tmp);
                ++all_data_it; ++inlier_mask_it;
            }
            assert(inlier_mask_it == this->inlier_mask.end());
        }
};


template<class ErrorAccumulatorT, template<class> class CAllocTT> class MultiSolutionModel : 
        public Model<typename ErrorAccumulatorT::ModelRep, typename ErrorAccumulatorT::DatumT> {
    public:
        typedef typename ErrorAccumulatorT::ModelRep ModelRepT;
        typedef typename ErrorAccumulatorT::ModelClassT ModelT;
        typedef std::vector<ModelRepT, CAllocTT<ModelRepT> > ModelRepVecT; 

    protected:
        ErrorAccumulatorT error_acc;
        ModelRepVecT models;

        virtual void compute(const typename ModelT::DataVec & all_data){
            shared_ptr<shared_ptr<typename ErrorAccumulatorT::ModelT> > model_tmp, best_model;
            BOOST_FOREACH(ModelRepT mr, models){
                model_tmp.reset(new SingleSolutionModel<ErrorAccumulatorT>(
                        this->model_data, mr, error_acc));
                model_tmp->compute(all_data);
                if(!best_model || (best_model->getInlierCount() < model_tmp->getInlierCount())){
                    best_model = model_tmp;
                }
            }
            assert(best_model != models.end());
            this->model = best_model->getModel();
            this->error = best_model->getError();
            this->all_data_error = best_model->all_data_error;
            this->inlier_mask = best_model->getInlierMask();
            this->inlier_count = best_model->getInlierCount();
        }

    public:
        MultiSolutionModel(
                int _size, 
                const typename ModelT::DataVec& _model_data, 
                const ModelRepVecT & _models, 
                const ErrorAccumulatorT& _error) : 
                Model<ModelRepT, typename ErrorAccumulatorT::DatumT>(
                        _size, _model_data), models(_models), error_acc(_error) {}


};


template<class ModelClassT> class ModelBuilder{
    public:
        virtual shared_ptr<ModelClassT> operator()(
                const typename ModelClassT::DataVec & selected) = 0;

        virtual unsigned int getSize() const = 0;
};

typedef ModelBuilder<EMatrixModel> EMatrixModelBuilder;


template<class ErrorAccumClassT> class GeneralizedModelBuilder : public 
        ModelBuilder<typename ErrorAccumClassT::ModelClassT>{
    protected:
        ErrorAccumClassT error_accum;
    public:
        GeneralizedModelBuilder(const ErrorAccumClassT& _error_accum) : error_accum(_error_accum) { }
        virtual shared_ptr<typename ErrorAccumClassT::ModelClassT> operator()(
                const typename ErrorAccumClassT::ModelClassT::DataVec & selected) = 0;

        virtual unsigned int getSize() const = 0;
};



#endif