error_func.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis

#ifndef VSLAM_ERROR_FUNC_H_INCLUDED
#define VSLAM_ERROR_FUNC_H_INCLUDED

#include "utils.h"
#include "sfm_utils.h"
#include <Eigen/Dense>
#include <boost/tr1/cmath.hpp>


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


struct LandmarkObservation{
    SE3 T_cw; 
    Vector2d pixel; 

    LandmarkObservation(const SE3& _T_cw, const Vector2d& _pixel) : T_cw(_T_cw), pixel(_pixel) { } 
    bool operator==(const LandmarkObservation& other){
        return T_cw.matrix().isApprox(other.T_cw.matrix()) && pixel.isApprox(other.pixel);
    }
};


template<class ModelT, class DatumT, class ErrorT> class DatumErrorFunc{
    public:
        typedef Model<ModelT, DatumT> ModelClassT;
        typedef ModelT ModelRep;
        typedef DatumT Datum;
        typedef ErrorT Error;
        //returns HUGE_VAL iff the error cannot be determined.
        virtual ErrorT operator()(const ModelT& model, const DatumT& datum) const = 0;
};


template<class ModelT, class DatumT> 
        class DatumRealErrorFunc : public DatumErrorFunc<ModelT, DatumT, double> { };



template<class DatumErrorFuncT> class MDErrorFuncAdaptor : public 
        DatumRealErrorFunc<typename DatumErrorFuncT::ModelRep, typename DatumErrorFuncT::Datum>{
    protected:

        DatumErrorFuncT ef;
    public:
        MDErrorFuncAdaptor(const DatumErrorFuncT& _error_func) : ef(_error_func) {}

        virtual double operator()(
                const typename DatumErrorFuncT::ModelRep& model, 
                const typename DatumErrorFuncT::Datum& datum) const{
            return ef(model, datum).norm();
        }
};

template<class ModelT, class DatumT> class DummyErrorFunc : public DatumRealErrorFunc<ModelT, DatumT>{
    public:
        virtual double operator()(const ModelT & E, const DatumT& datum) const{
            return 0;
        }
};

template<class DatumErrorFuncT, class ErrorAccumulatorT> class ErrorAccumulator{
    protected:
        DatumErrorFuncT error_func;
    public:
        typedef typename DatumErrorFuncT::ModelClassT::ModelRep ModelRep;
        typedef typename DatumErrorFuncT::ModelClassT::Datum DatumT;
        typedef typename DatumErrorFuncT::Error ErrorT;
        typedef Model<ModelRep, DatumT> ModelClassT;

        ErrorAccumulator(const DatumErrorFuncT& _error_func) : error_func(_error_func) { }

        ErrorT computeError(const ModelRep& model, const DatumT& datum){
            return error_func(model, datum);
        }

        bool inlierTest(const ErrorT& e) { assert(false); return false;}

        ErrorAccumulatorT errorAccumulator(ErrorAccumulatorT ae, ErrorT e) {assert(false);}
};

template<class DatumErrorFuncT> 
        class RealErrorPlusErrorAccumulator : public ErrorAccumulator<DatumErrorFuncT, double>{
    protected:
        double treshold;
    public:
        RealErrorPlusErrorAccumulator(DatumErrorFuncT _error_func, double _treshold) : 
                ErrorAccumulator<DatumErrorFuncT, double>(_error_func), treshold(_treshold) { }

        bool isInlier(const double& e){ return e < treshold; }

        double errorAccumulator(double ae, double e){ return ae + e; }
};

template<class DatumErrorFuncT> 
        class RealErrorMaxErrorAccumulator : public ErrorAccumulator<DatumErrorFuncT, double>{
    protected:
        double treshold;
    public:
        RealErrorMaxErrorAccumulator(DatumErrorFuncT _error_func, double _treshold) : 
                ErrorAccumulator<DatumErrorFuncT, double>(_error_func), treshold(_treshold) { }

        bool isInlier(const double& e){ return e < treshold; }

        double errorAccumulator(double ae, double e){ return std::max(ae, e); }
};
/*------------------------------------------------------------------------------------------*/

class LandmarkToPoseReprojectionError : public DatumErrorFunc<Vector3d, LandmarkObservation, Vector2d>{
    public:
        virtual Vector2d operator()(const Vector3d& model, const LandmarkObservation& datum) const{
            Vector2d epx, mpx, wh, error;
            epx = PanoImCoord::fromHomogenous(datum.T_cw.inverse()*model).getPixel();
            mpx = datum.pixel;

            wh(0)=PanoImCoord::Params::width;
            wh(1)=PanoImCoord::Params::height;
            for(int i=0; i<2;i++){
                error(i) = 
                    std::min(
                            std::abs(epx(i) - mpx(i)), 
                            std::min(
                                    std::abs((epx(i) + wh(0)) - mpx(i)),
                                    std::abs((epx(i) - wh(0)) - mpx(i))));
                    //std::min(std::abs(epx(i) - mpx(i)), std::abs(std::abs(epx(i) - mpx(i)) - wh(i)));
            }
            return error;
        }
};



class DatumErrorFromSE3Tranform : public DatumRealErrorFunc<Matrix3d, ImageMatch>{
     ImageMatchVec selected;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        DatumErrorFromSE3Tranform(const ImageMatchVec & _selected) : selected(_selected) {}

        virtual double operator()(const Matrix3d & E, const ImageMatch & image_match) const{
            SE3 T_c1c2;
            if(extractTransformFromE(selected, E, T_c1c2)){;
                Vector3d d1 = T_c1c2 * image_match.first;
                d1.normalize();
                Vector3d d2 = image_match.second;
                d2.normalize();
                return (d1-d2).norm();
            } else{
                return HUGE_VAL;
            }
        }
};


class AngleError : public DatumRealErrorFunc<Matrix3d, ImageMatch>{
    protected:
        double computeAbsoluteAngle(
                const Vector3d & hom_line, const Vector3d & hom_point) const{
            Vector3d pt_on_plane = closestPointOnEpipolarPlane(hom_line, hom_point);
            //assert(!pt_on_plane.isZero());
            double tmp = atan((hom_point - pt_on_plane).norm() / pt_on_plane.norm());
            if(!std::isfinite(tmp)) tmp = HUGE_VAL;
            return tmp;
        }
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        virtual double operator()(const Matrix3d & E, const ImageMatch & image_match) const{
            double error = computeAbsoluteAngle(E * image_match.first, image_match.second);
            error += computeAbsoluteAngle(image_match.second.transpose() * E, image_match.first);
            return error / 2.0;
        }
};





#endif