emat_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_EMAT_ESTIMATION_H_INCLUDED
#define VSLAM_EMAT_ESTIMATION_H_INCLUDED

#include "utils.h"
#include "model_estimation.h"


template<class ErrorAccumClassT> class EightPointModelBuilder : 
        public GeneralizedModelBuilder<ErrorAccumClassT> {
    protected:
        double error_treshold;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

        shared_ptr<EMatrixModel> operator()(const ImageMatchVec& selected){
            return shared_ptr<EMatrixModel>(new SingleSolutionModel<ErrorAccumClassT >(
                        8, selected, computeE(selected), this->error_accum));
        }

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



template<class ErrorAccumClassT> 
class LiHartley5ptFrom6ptsBuilder : public GeneralizedModelBuilder<ErrorAccumClassT>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        LiHartley5ptFrom6ptsBuilder(const ErrorAccumClassT& _error_accum) :
                GeneralizedModelBuilder<ErrorAccumClassT>(_error_accum) { }

        //uses 5-point algorithm and 6th point to filter uniqe solution. 
        virtual shared_ptr<EMatrixModel> operator()(const ImageMatchVec & selected){
            ImageMatchVec selected5 = selected;
            selected5.pop_back();
            ALIGNED<Matrix3d>::vector Ematrixes = li_hartley_5pt(selected5, false);
            shared_ptr<EMatrixModel> model;
            BOOST_FOREACH(Matrix3d E, Ematrixes){
                shared_ptr<SingleSolutionModel<ErrorAccumClassT> > m(new SingleSolutionModel<
                        ErrorAccumClassT>(getSize(), selected, E, this->error_accum));
                SE3 estimate;
                if(extractTransformFromE(selected, m->getModel(), estimate)){
                    model = m;
                }
            }
            return model;
        }

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





template<class ErrorAccumT> class LiHartleyMulti5ptBuilder : 
        public GeneralizedModelBuilder<ErrorAccumT>{
    protected:
        ErrorAccumT error_accum;
    public:
        LiHartleyMulti5ptBuilder(const ErrorAccumT& _error_accum) :
                GeneralizedModelBuilder<ErrorAccumT>(_error_accum){}

        virtual shared_ptr<EMatrixModel> operator()(const ImageMatchVec & selected){
            ALIGNED<Matrix3d>::vector Ematrixes;
            Ematrixes = li_hartley_5pt(selected, false);

            return shared_ptr<EMatrixModel>(
                    new MultiSolutionModel<ErrorAccumT, Eigen::aligned_allocator>(
                            (int)getSize(),
                            selected,
                            Ematrixes, 
                            error_accum)); 
        }

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


template<class ErrorAccumT> class DummyModelBuilder : public GeneralizedModelBuilder<ErrorAccumT>{
    protected:
        ALIGNED<Matrix3d>::vector Ematrixes;
        int i;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        DummyModelBuilder(
                const ErrorAccumT& _error_accum, 
                const ALIGNED<Matrix3d>::vector &_Ematrixes) :
                GeneralizedModelBuilder<ErrorAccumT>(_error_accum), Ematrixes(_Ematrixes), i(-1) {}
        
        shared_ptr<EMatrixModel> operator()(const ImageMatchVec & selected){
            i++;
            assert(i<(int)Ematrixes.size());
            return shared_ptr<EMatrixModel>(new SingleSolutionModel<ErrorAccumT>(
                        1,  selected, Ematrixes[i], this->error_accum));
        }

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






class DummyModelBuilderStd : public DummyModelBuilder<
        RealErrorMaxErrorAccumulator<AngleError> >{
    private:
        typedef RealErrorMaxErrorAccumulator<AngleError> ErrorAccumT;
    public:

        DummyModelBuilderStd(
                const ALIGNED<Matrix3d>::vector &_Ematrixes, 
                double _error_treshold = M_PI / 45.0) :
                DummyModelBuilder<ErrorAccumT>(
                ErrorAccumT(AngleError(), _error_treshold), _Ematrixes) {}
};

class EightPointModelBuilderStd : public EightPointModelBuilder<
        RealErrorMaxErrorAccumulator<DummyErrorFunc<Matrix3d, ImageMatch> > >{
    private:
        typedef DummyErrorFunc<Matrix3d, ImageMatch> ErrorFuncT;
        typedef RealErrorMaxErrorAccumulator<DummyErrorFunc<Matrix3d, ImageMatch> > ErrorAccumT;
    public:

        EightPointModelBuilderStd() :
                EightPointModelBuilder<ErrorAccumT>(ErrorAccumT( ErrorFuncT(), error_treshold)) {}
};

class LiHartley5ptFrom6ptsBuilderStd : public LiHartley5ptFrom6ptsBuilder<
        RealErrorMaxErrorAccumulator<AngleError> >{
    private:
        typedef RealErrorMaxErrorAccumulator<AngleError> ErrorAccumT;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        LiHartley5ptFrom6ptsBuilderStd( double _error_treshold = M_PI / 45.0) :
            LiHartley5ptFrom6ptsBuilder<ErrorAccumT>(ErrorAccumT(AngleError(), _error_treshold)) {}
};




#endif