absorient.h

Go to the documentation of this file.

#ifndef TAG_ABSORIENT_H_
#define TAG_ABSORIENT_H_

#include <vector>
#ifdef WIN32
#include <tuple>
#else
#include <tr1/tuple>
#endif

#include <TooN/TooN.h>
#include <TooN/sim2.h>
#include <TooN/sim3.h>
#include <TooN/SVD.h>

namespace tag {


namespace Internal {

template <int D>
std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> computeOrientationScale( const std::vector<TooN::Vector<D> > & a, const std::vector<TooN::Vector<D> > & b ){
    TooN::SizeMismatch<D,D>::test(a.front().size(), b.front().size());
    const int DIM = a.front().size();
    const size_t N = std::min(a.size(), b.size());

    // compute cross correlations
    TooN::Matrix<D> s(DIM,DIM); 
    s = TooN::Zeros;
    for( size_t i = 0; i < N; i++){
        s += b[i].as_col() * a[i].as_row();
    }
    s /= N;

    // SVD of cross correlation matrix
    TooN::SVD<D> svd(s);

    // build S for rotation matrix
    TooN::Vector<D> S(DIM);
    S = TooN::Ones;

    const TooN::DefaultPrecision eps = 1e-8;
    const TooN::DefaultPrecision ds = determinant_gaussian_elimination(s);
    if(ds < -eps){
        S[DIM-1] = -1;
    } else if(ds < eps) { // close to 0 let U * VT decide
        const TooN::DefaultPrecision duv = TooN::determinant_gaussian_elimination(svd.get_U()) 
                                        * TooN::determinant_gaussian_elimination(svd.get_VT());
        if(duv <  0)
            S[DIM-1] = -1;
    }

    // compute trace(DS)
    TooN::DefaultPrecision scale = 0;
    for(int i = 0; i < DIM; ++i)
        scale += svd.get_diagonal()[i] * S[i];

    return std::make_pair(svd.get_U() * S.as_diagonal() * svd.get_VT(), scale);
}

}

template <int D>
inline TooN::Matrix<D> computeRotation( const std::vector<TooN::Vector<D> > & a, const std::vector<TooN::Vector<D> > & b ){
    std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> Rs = Internal::computeOrientationScale( a, b );
    return Rs.first;
}

inline TooN::SO3<> computeOrientation( const std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b ){
    std::pair<TooN::Matrix<3>, TooN::DefaultPrecision> result = Internal::computeOrientationScale( a, b );
    return TooN::SO3<>(result.first);
}

inline TooN::SO2<> computeOrientation( const std::vector<TooN::Vector<2> > & a, const std::vector<TooN::Vector<2> > & b ){
    std::pair<TooN::Matrix<2>, TooN::DefaultPrecision> result = Internal::computeOrientationScale( a, b );
    return TooN::SO2<>(result.first);
}

TooN::SO3<> computeOrientation( const TooN::Vector<3> & a1, const TooN::Vector<3> & b1, const TooN::Vector<3> & a2, const TooN::Vector<3> & b2 );

template <int D>
std::pair<TooN::Matrix<D>, TooN::Vector<D> > computeAbsoluteOrientation( const std::vector<TooN::Vector<D> > & a, const std::vector<TooN::Vector<D> > & b){
    TooN::SizeMismatch<D,D>::test(a.front().size(), b.front().size());
    const int DIM = a.front().size();
    const size_t N = std::min(a.size(), b.size());
    
    if(N == 1){    // quick special case
        TooN::Matrix<D> R(DIM, DIM);
        R = TooN::Identity;
        return std::make_pair(R, b.front() - a.front());
    }

    // compute centroids
    // Strictly speaking, it is not necessary to remove the centroids from both sets, 
    // one set is enough. However, we do it nevertheless to improve the conditioning
    // of the rotation/scale estimation.
    TooN::Vector<D> ma = TooN::Zeros(DIM), mb = TooN::Zeros(DIM);
    for(unsigned i = 0; i < N; ++i){
        ma += a[i];
        mb += b[i];
    }
    ma /= N;
    mb /= N;

    // compute shifted locations
    std::vector<TooN::Vector<D> > ap(N), bp(N);
    for( unsigned i = 0; i < N; ++i){
        ap[i] = a[i] - ma;
        bp[i] = b[i] - mb;
    }
    
    // put resulting transformation together
    std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> Rs = Internal::computeOrientationScale( ap, bp );
    return std::make_pair(Rs.first, mb - Rs.first * ma);
}

inline TooN::SE3<> computeAbsoluteOrientation( const std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
    std::pair<TooN::Matrix<3>, TooN::Vector<3> > Rt = computeAbsoluteOrientation<3>( a, b );
    return TooN::SE3<>(Rt.first, Rt.second);
}

inline TooN::SE2<> computeAbsoluteOrientation( const std::vector<TooN::Vector<2> > & a, const std::vector<TooN::Vector<2> > & b){
    std::pair<TooN::Matrix<2>, TooN::Vector<2> > Rt = computeAbsoluteOrientation<2>( a, b );
    return TooN::SE2<>(Rt.first, Rt.second);
}

template <int D>
std::tr1::tuple<TooN::Matrix<D>, TooN::Vector<D>, TooN::DefaultPrecision > computeSimilarity( const std::vector<TooN::Vector<D> > & a, const std::vector<TooN::Vector<D> > & b){
    TooN::SizeMismatch<D,D>::test(a.front().size(), b.front().size());
    const int DIM = a.front().size();
    const size_t N = std::min(a.size(), b.size());
    
    if(N == 1){    // quick special case
        TooN::Matrix<D> R(DIM, DIM);
        R = TooN::Identity;
        return std::tr1::make_tuple(R, b.front() - a.front(), 1);
    }

    // compute centroids
    // Strictly speaking, it is not necessary to remove the centroids from both sets, 
    // one set is enough. However, we do it nevertheless to improve the conditioning
    // of the rotation/scale estimation.
    TooN::Vector<D> ma = TooN::Zeros(DIM), mb = TooN::Zeros(DIM);
    for(unsigned i = 0; i < N; ++i){
        ma += a[i];
        mb += b[i];
    }
    ma /= N;
    mb /= N;

    // compute shifted locations
    std::vector<TooN::Vector<D> > ap(N), bp(N);
    for( unsigned i = 0; i < N; ++i){
        ap[i] = a[i] - ma;
        bp[i] = b[i] - mb;
    }
    
    // put resulting transformation together
    std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> Rs = Internal::computeOrientationScale( ap, bp );
        // compute scale
    TooN::DefaultPrecision sa = 0;
    for( unsigned int i = 0; i < N; ++i){
        sa += TooN::norm_sq(ap[i]);
    }
    sa /= N;
    const TooN::DefaultPrecision scale = Rs.second / sa;
    return std::tr1::make_tuple(Rs.first, mb - Rs.first * (scale * ma), scale);
}

inline TooN::SIM3<> computeSimilarity( const std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
    std::tr1::tuple<TooN::Matrix<3>, TooN::Vector<3>, TooN::DefaultPrecision > Rts = computeSimilarity<3>(a,b);
    return TooN::SIM3<>(TooN::SO3<>(std::tr1::get<0>(Rts)), std::tr1::get<1>(Rts), std::tr1::get<2>(Rts));
}

inline TooN::SIM2<> computeSimilarity( const std::vector<TooN::Vector<2> > & a, const std::vector<TooN::Vector<2> > & b){
    std::tr1::tuple<TooN::Matrix<2>, TooN::Vector<2>, TooN::DefaultPrecision > Rts = computeSimilarity<2>(a,b);
    return TooN::SIM2<>(TooN::SO2<>(std::tr1::get<0>(Rts)), std::tr1::get<1>(Rts), std::tr1::get<2>(Rts));
}

TooN::SO3<> computeMeanOrientation( const std::vector<TooN::SO3<> > & r);

TooN::Matrix<3> quaternionToMatrix( const TooN::Vector<4> & q );

} // namespace tag

#endif // TAG_ABSORIENT_H_