helpers.h

Go to the documentation of this file.

#ifndef TAG_HELPERS_H
#define TAG_HELPERS_H

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

namespace tag {


template <class T> inline const typename T::first_type& first_point(const T& t) { return t.first; }
template <class T> inline const typename T::second_type& second_point(const T& t) { return t.second; }
template <class T> inline double noise(const T& t) { return 1.0; }


template <class It> void getProjectiveHomography(It begin, It end, TooN::Matrix<3>& H){
    assert(std::distance(begin,end) >= 4);

    TooN::WLS<8> wls;
    for (It it=begin; it!=end; it++) {
        const TooN::Vector<2>& a = first_point(*it);
        const TooN::Vector<2>& b = second_point(*it);
        const TooN::Vector<8> J1 = TooN::makeVector(a[0], a[1], 1, 0, 0, 0, -b[0]*a[0], -b[0]*a[1]);
        const TooN::Vector<8> J2 = TooN::makeVector(0, 0, 0, a[0], a[1], 1, -b[1]*a[0], -b[1]*a[1]);
        wls.add_mJ(b[0], J1, noise(*it));
        wls.add_mJ(b[1], J2, noise(*it));
    }
    wls.compute();
    TooN::Vector<8> h = wls.get_mu();
    H[0] = h.template slice<0,3>();
    H[1] = h.template slice<3,3>();
    H[2][0] = h[6];
    H[2][1] = h[7];
    H[2][2] = 1;
}


template <class It> TooN::Matrix<3> getProjectiveHomography(It begin, It end){
    TooN::Matrix<3> H;
    getProjectiveHomography(begin, end, H);
    return H;
}

template<class V, class M > inline void getCrossProductMatrix( const V & vec, M & result ){
    assert(vec.size() == 3);
    assert(result.num_cols() == 3 && result.num_rows() == 3);
    result(0,0) = 0; result(0,1) = -vec[2]; result(0,2) = vec[1];
    result(1,0) = vec[2]; result(1,1) = 0; result(1,2) = -vec[0];
    result(2,0) = -vec[1]; result(2,1) = vec[0]; result(2,2) = 0;
}

template<class V> inline TooN::Matrix<3> getCrossProductMatrix( const V & vec ){
    TooN::Matrix<3> result;
    getCrossProductMatrix(vec, result);
    return result;
}

template<class M> inline void getEssentialMatrix(const TooN::SE3<> & transform , M & E){
    //assert(E.num_cols() == 3 && E.num_rows() == 3);
    const TooN::Vector<3> & t = transform.get_translation();
    const TooN::Matrix<3> & r = transform.get_rotation().get_matrix();
    E[0] = t[1] * r[2] - t[2] * r[1];
    E[1] = t[2] * r[0] - t[0] * r[2];
    E[2] = t[0] * r[1] - t[1] * r[0];
}

inline TooN::Matrix<3> getEssentialMatrix(const TooN::SE3<> & transform ){
    TooN::Matrix<3> E;
    getEssentialMatrix(transform, E);
    return E;
}

} // namespace tag

#endif // TAG_HELPERS_H