fourpointpose.h

Go to the documentation of this file.

#ifndef TAG_FOURPOINTPOSE_H_
#define TAG_FOURPOINTPOSE_H_

#include <vector>

#include <TooN/TooN.h>
#include <TooN/se3.h>

namespace tag {


TooN::SE3<> fourPointPose( const std::vector<TooN::Vector<3> > & points, const std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError = 0.14 );

TooN::SE3<> fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > & points, const std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError = 0.14 );

template <int ImagePlaneZ = 1>
struct Point4SE3Estimation {
    TooN::SE3<> T;
    bool valid;
    double angularError;
    static const int hypothesis_size = 4;

    inline Point4SE3Estimation(double ang = 0.14) : valid(false), angularError(ang) {  }

    template<class It> inline bool estimate(It begin, It end) {
        assert(std::distance(begin,end) >= 4);
        valid = true;

        std::vector<TooN::Vector<3> > points(4);
        std::vector<TooN::Vector<3> > pixels(4);
        unsigned int i = 0;
        for(It match = begin; match != end; match++, i++){
            pixels[i] = unproject(match->pixel);
            pixels[i][2] *= ImagePlaneZ;
            points[i] = match->position;
        }
        T = fourPointPoseFromCamera( points, pixels, valid, angularError );
        return valid;
    }

    template<class Obs, class Tol> inline bool isInlier( const Obs& obs, const Tol& tolerance ) const {
        return score(obs) < tolerance;
    }

    template<class Obs> inline double score(const Obs & obs) const {
        if(valid){
            TooN::Vector<3> pos = T * obs.position;
            TooN::Vector<2> diff = project(pos) - obs.pixel / ImagePlaneZ;
            double disp = diff*diff;
            return disp;
        }
        return 100;
    }

    template<class Obs> inline double getSqError(const Obs & obs) const {
        return score(obs);
    }
};

} // namespace tag

#endif /*FOURPOINTPOSE*/