utils.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_UTILS_H_INCLUDED
#define VSLAM_UTILS_H_INCLUDED

#define ROSCONSOLE_MIN_SEVERITY ROSCONSOLE_SEVERITY_DEBUG

//turn of eigen alignment stuff.
#define EIGEN_DONT_ALIGN
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <vector>
#include <list>
#include <set>
#include <assert.h>
#include "ros/ros.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/tr1/memory.hpp>
#include <boost/tr1/cmath.hpp>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/filesystem.hpp>
#include "Sophus/se3.h"
#include "Sophus/so3.h"

#include "nav_msgs/Odometry.h"

extern boost::mt19937 rng_mt19937; 

const double PI = std::atan(1.0)*4; //TODO remove

template <typename T1, typename T2 = T1>
struct ALIGNED {
  typedef std::vector<T1, Eigen::aligned_allocator<T1> > vector;
  typedef std::list<T1, Eigen::aligned_allocator<T1> > list;
  typedef std::set<T1, Eigen::aligned_allocator<T1> > set;
  typedef std::map<T1, T2> map; //XXX
};

typedef ALIGNED<Eigen::Vector3d>::vector Points3dSTL;
typedef ALIGNED<Eigen::Vector2d>::vector Points2dSTL;
typedef ALIGNED<std::pair<int, Eigen::Vector3d> >::vector IdxPoint3dVec;
typedef ALIGNED<std::pair<int, Eigen::Vector2d> >::vector IdxPoint2dVec;
typedef std::pair<Eigen::Vector3d, Eigen::Vector3d> ImageMatch;
typedef ALIGNED<ImageMatch>::vector ImageMatchVec;

using Eigen::Vector2f;
using Eigen::Vector2d; using Eigen::Vector3d; using Eigen::VectorXd;
using Eigen::Matrix3d; using Eigen::Matrix4d; using Eigen::MatrixXd; using Eigen::Matrix;
using std::tr1::shared_ptr;
using std::cout; using std::cerr; using std::endl; using std::pair; using std::make_pair;
using std::list; using std::map; using std::vector; 
using Sophus::SE3; using Sophus::SO3;
//using std::tr1::isfinite;

namespace vslam{
    extern std::string image_mask_file;
    extern std::string image_sample_file;
    extern boost::filesystem::path debug_output_path;
}

class PanoramaParams{
    public:
        static const int width = 1600;
        static const int height = 800;
};



/*1st index is 0*/
template<class T> T linspace_idx(T first, T last, int count, T idx){
    if(idx > count - 1.0){
        T tmp = ((T)count - 1) + 0.0000001;
        idx = (idx / tmp - (T)std::tr1::round(idx / tmp)) * tmp;
    }
    return first + ((last-first) / (T)((T)count - 1)) * idx;
}

template<class T> T inv_linspace_idx(T first, T last, int count, T val){
    return (val - first) / ((last-first) / (T)(count - 1));
}

template<class Iterator> void printContainer(Iterator b, Iterator e, const char * separator){
    while(b!=e){
        cout << *b << separator;
        ++b;
    }
}

template<class ImageParamsObj> class ImageCoordinate{
    private:
        bool initialized;
        double azimuth;
        double elevation;

    public:
        typedef ImageParamsObj Params;

        ImageCoordinate() : initialized(false) {}

        //pixel coordinate (x,y)=(0,0) is at the upper-left corner of the image. 
        //(x,y)=(width-1,height-1) is bottom right corner
        static ImageCoordinate<ImageParamsObj> fromPixel(Eigen::Vector2d pixel){
            ImageCoordinate<ImageParamsObj> obj;
            obj.azimuth = linspace_idx(
                    (double)0,
                    (double)(ImageParamsObj::width - 1) / (double)ImageParamsObj::width * (-2.0)*M_PI, 
                    ImageParamsObj::width,
                    pixel(0) - (double)(((int)ImageParamsObj::width) / 2)); 
            obj.elevation = -linspace_idx(-M_PI / 2.0, M_PI / 2.0, ImageParamsObj::height, pixel(1));
            obj.initialized = true;
            return obj;
        }

        //(azimuth,elevation)=(0,0) corresponds to (0,0,1) i.e. z-ray.
        static ImageCoordinate<ImageParamsObj> fromSpherical(Eigen::Vector2d spherical){
            ImageCoordinate<ImageParamsObj> obj;
            obj.azimuth = spherical(0);
            obj.elevation = spherical(1);
            obj.initialized = true;
            return obj;
        }

        static ImageCoordinate<ImageParamsObj> fromHomogenous(Eigen::Vector3d homogenous){
            ImageCoordinate<ImageParamsObj> obj;
            homogenous.normalize();
            double x = homogenous(0);
            double y = homogenous(1);
            double z = homogenous(2);

            double r_xz = std::sqrt(x*x + z*z);
            obj.azimuth = std::atan2(-x, z);
            obj.elevation = std::atan2(y, r_xz);
            obj.initialized = true;
            return obj;
        }

        Eigen::Vector2d getPixel() const{
            assert(initialized);
            Eigen::Vector2d t;
            t(0) = inv_linspace_idx(
                    (double)0, 
                    (double)(ImageParamsObj::width - 1) / (double)ImageParamsObj::width * (-2.0)*M_PI,
                    ImageParamsObj::width, 
                    azimuth) + (double)(((int)ImageParamsObj::width) / 2);
            t(1) = inv_linspace_idx(-M_PI / 2.0, M_PI / 2.0, ImageParamsObj::height, -elevation);
            return t;
        }

        Eigen::Vector2d getSpherical() const{
            assert(initialized);
            return Eigen::Vector2d(azimuth, elevation);
        }

        Eigen::Vector3d getHomogenous() const{
            assert(initialized);
            Eigen::Vector3d direction;
            direction(0) = -std::sin( azimuth ) * std::cos( elevation );
            direction(1) = std::sin( elevation );
            direction(2) = std::cos( azimuth ) * std::cos( elevation );
            return direction;
        }

        void print(){
            {
                Vector2d c = getPixel();
                cout << "pixel coordinates: " << "x: " << c(0) << "y: " << c(1) << endl;
            }
            cout << "spherical coordinates: " <<
                    "azimuth/PI: " << azimuth / M_PI << "elevation/PI: " << elevation / M_PI << endl;
            {
                Vector3d c = getHomogenous();
                cout << "homogenous coordinates: " <<
                        "x: " << c(0) << "y: " << c(1) << "z: " << c(2) << endl;
            }
        }
}; 

typedef ImageCoordinate<PanoramaParams> PanoImCoord;
typedef vector<PanoImCoord> ImageCoordinateVec;


inline Vector2d homogenousToPixel(const Vector3d & hom){
    return PanoImCoord::fromHomogenous(hom).getPixel();
}


template<typename T> class Maybe {
    T * obj;

    Maybe(){
        obj = 0;
    }
    public:

    const static Maybe<T> no;
    static Maybe<T> yes(const T & _obj){
        Maybe m;
        m.obj=new T(_obj);
        return m;
    }

    T & value() const{
        assert(obj != NULL);
        return *obj;
    }

    operator bool(){
        return obj != NULL;
    }

    bool operator==(const Maybe<T> & _obj) const{
        if(obj && _obj){
            return *obj == *(_obj.obj);
        } else{
            return false;
        }
    }

    bool operator!=(const Maybe<T> & _obj) const{
        return !(*this == _obj);
    }

    Maybe(const Maybe<T> & _obj){
        if(_obj == Maybe<T>::no){
            obj = 0;
        } else{
            obj = new T(_obj.value());
        }
    }
};
 //end of Cam module

inline ImageMatch computeMatch(const Vector2d& px_c1, const Vector2d& px_c2){
    PanoImCoord c1 = PanoImCoord::fromPixel(px_c1);
    PanoImCoord c2 = PanoImCoord::fromPixel(px_c2);
    return ImageMatch(c1.getHomogenous(), c2.getHomogenous());
}


template<class T> cv::Point cvPointFromVector2(Eigen::Matrix<T, 2, 1> vec){
    return cv::Point(vec(0), vec(1));
}



ImageMatchVec imageMatchCoords(const vector<cv::KeyPoint> & tkpts, const vector<cv::KeyPoint> & qkpts,
        const std::vector<cv::DMatch> &matches);

typedef Eigen::Matrix<double, 3, Eigen::Dynamic> PointMatrix;

void points3dSTLToPointMatrix(const Points3dSTL & pts, PointMatrix & ptmat);

ImageMatchVec buildImageMatchesFrom3dPoints(
        const SE3 & T_wc1, const SE3 & T_wc2, const Points3dSTL & points);

Points3dSTL projectPoints(SE3 T_sd, const Points3dSTL & points);


void generateRandomScene(
        const Vector3d & upper_left_deep,
        const Vector3d & bottom_right_shallow,
        double target_num_vec,
        Points3dSTL & points);

void generateRandomTrajectory(
        const Vector3d & upper_left_deep,
        const Vector3d & bottom_right_shallow,
        int num_stops,
        ALIGNED<SE3>::vector & transforms);

inline void generateLineTrajectory(
        const Vector3d & start,
        const Vector3d & end,
        int num_stops,
        ALIGNED<SE3>::vector & transforms){ //T_wcx transforms for each pose
    Points3dSTL poses;
    Points3dSTL orientations;
    Vector3d normalized_direction = (end-start).normalized();
    double track_len = (end-start).norm();
    double segment_len = track_len / (double) (num_stops - 1);
    Vector3d tmp=start;
    for(int i=0; i<num_stops; i++){
        transforms.push_back(SE3(SO3(0, 0, 0), -tmp));
        tmp = (tmp + normalized_direction * segment_len).eval();
    }
}


template< class T> class SingleVariableWrapper{
    protected:
        T value_var;
    public:
        T val() const { return value_var; };
};

class SpeedAsNaturalNumber : public SingleVariableWrapper<int>{
    public:
        SpeedAsNaturalNumber(int speed){
            value_var = speed;
            assert(speed >=0);
        }
};

class TimeAsNaturalNumber : public SingleVariableWrapper<int>{
    public:
        TimeAsNaturalNumber(int time){
            value_var=time;
            assert(time >=0);
        }
};

struct SpeedSegment{
    SE3 T_c1c2;
    SpeedAsNaturalNumber speed_mul;
    TimeAsNaturalNumber time_mul;

    SpeedSegment(const SE3& _T_c1c2, SpeedAsNaturalNumber _speed_mul, TimeAsNaturalNumber _time_mul) :
            T_c1c2(_T_c1c2), speed_mul(_speed_mul), time_mul(_time_mul) {}
};

inline void constructTrajectoryFromSpeedSegments(
        std::vector<SpeedSegment> speed_segments,
        ALIGNED<SE3>::vector& transforms){
    transforms.push_back(SE3());
    SE3 prevT_wc = SE3();
    BOOST_FOREACH(SpeedSegment segment, speed_segments){
        SE3 T_c1ct = SE3();
        for(int i=0; i < segment.speed_mul.val(); i++){
            T_c1ct = T_c1ct * (T_c1ct.inverse() * segment.T_c1c2 * T_c1ct);
        }
        for(int i=0; i < segment.time_mul.val(); i++){
            SE3 T_wc = prevT_wc * (prevT_wc.inverse() * T_c1ct * prevT_wc);
            transforms.push_back(T_wc);
            prevT_wc = T_wc;
        }
    }
}

bool isApprox(double num, double target_num, double error);


inline std::string seqencedName(int seq, std::string name){
    std::ostringstream s;
    s << seq << "_" << name;
    return s.str();
}

inline Vector3d coordFrameConventionLocalToROS(const Vector3d& vec){
    return Vector3d(vec(2), -vec(0), vec(1));
    //return vec;
}

inline SE3 coordFrameConventionLocalToROS(const SE3& T){
    //coordinate switch done using change of basis from internal to external coordinates(matrix C).
    Eigen::Matrix4d C = Eigen::Matrix4d::Zero();
    C(0,2)=1; C(1,0)=-1; C(2,1)=1; C(3,3)=1;
    Eigen::Matrix4d M = C * T.matrix() * C.inverse();
    return SE3(M.block(0,0,3,3), M.block(0,3,3,1));
    //return T;
}

/*------------------------------------------------------------------------------------------*/
//Defunct attempt to automaticly specify allocator for Eigen types.
/*------------------------------------------------------------------------------------------*/
template<class T> struct EigenAllocTraits{
    typedef std::allocator<T> alloc;
};

template<> struct EigenAllocTraits<Vector3d>{
    typedef Eigen::aligned_allocator<Vector3d> alloc; 
};

/*------------------------------------------------------------------------------------------*/
//Stuff for working with ordered index-value pairs
/*------------------------------------------------------------------------------------------*/

template<class T1, class T2> T1 getFirst(const pair<T1, T2>& pair){
    return pair.first;
}

template<class T1, class T2> T2 getSecond(const pair<T1, T2>& pair){
    return pair.second;
}


template< class TR, template<class, class> class C, template<class> class A, class T> 
        C<TR, typename EigenAllocTraits<TR>::alloc > 
        transformF(const C<T, A<T> > & container, TR (*f)(const T&)){
    C<TR, typename EigenAllocTraits<TR>::alloc > result;
    std::transform(container.begin(), container.end(), std::back_inserter(result), f);
    return result;
}


template<class C1, class C2> C1 getCommon(const C1 & idx1, const C2 & idx2){ 
    C1 result;
    typename C1::const_iterator i1 = idx1.begin();
    typename C2::const_iterator i2 = idx2.begin();
    while(i1 != idx1.end() && i2 != idx2.end()){
        if(*i2 < *i1) { ++i2; continue; }
        if(*i1 < *i2) { ++i1; continue; }
        result.push_back(*i1);
        ++i1; ++i2;
    }
    return result;
}


template<template<class, class> class C1, template<class, class> class C2, 
        template<class> class Alloc1, template<class> class Alloc2, class TK, class TV>
list<pair<TK, TV>, Alloc2<pair<TK, TV> > > 
        indexBy(const C1<TK, Alloc1<TK> >& idx, const C2<TV, Alloc2<TV> >& points){
    list<pair<TK, TV>, Alloc2<pair<TK, TV> > > result;
    std::transform(idx.begin(), idx.end(), points.begin(), back_inserter(result), 
            std::make_pair<TK, TV>);
    return result;
}


template<template<class, class> class C1, template<class, class> class C2, 
        class T1, class T2, template<class> class A1, template<class> class A2>
C1<pair<T1, T2>, A1<pair<T1, T2> > > 
        getByIdx(const C1<pair<T1, T2>, A1<pair<T1, T2> > > & pts, const C2<T1, A2<T1> >& idx){
    typedef C1<pair<T1, T2>, A1<pair<T1, T2> > > C1I;
    C1I result;
    typename C1I::const_iterator pi = pts.begin();
    BOOST_FOREACH(T1 i, idx){
        while(i != pi->first) ++pi;
        result.push_back(*pi);
    }
    return result;
}


template<template<class, class, class, class> class M, class V, class K, class Cmp, 
    template<class> class Alloc> 
list<pair<K, V>, Alloc<pair<K, V> > > mapToList(const M<K, V, Cmp, Alloc<pair< const K, V> > >& map){
    list<pair<K, V>, Alloc<pair<K, V> > > result(map.begin(), map.end());
    return result;
}


template<template<class, class> class CT, template<class, class> class CS, class T,
    template<class> class Alloc>
CT<T, Alloc<T> > switchCont(const CS<T, Alloc<T> > & container){
    return CT<T, Alloc<T> >(container.begin(), container.end());
}


inline Matrix3d sE3ToE(const SE3 & T){
    return (SO3::hat(T.translation()) * T.rotation_matrix()).eval();
}



template<class T> list<T> makeSeq(T first, T last, T step){
    list<T> result;
    for(T i = first; i <= last; i += step){
        result.push_back(i);
    }
    return result;
}

template<class T> bool truePred(const T& t){ return true; }

template<template<class,class> class C1, template<class, class> class C2, class T, class A>
        list<T> inverse_idxs(const C1<T, A>& idx, const C2<T, A>& all_idx){
    typename C1<T, A>::const_iterator it = idx.begin();
    typename C2<T, A>::const_iterator it_all = all_idx.begin();
    list<T> inv_idx;
    while(it_all != all_idx.end()){
        if(*it_all != *it) inv_idx.push_back(*it_all);
        else ++it;
        ++it_all;
    }
    return inv_idx;
} 


template<template<class,class> class C, class T> 
C<T, std::allocator<T> > mkOneValueCont(const T& value){
    C<T, std::allocator<T> > container;
    std::back_insert_iterator<C<T, std::allocator<T> > > it = back_inserter(container);
    it = value;
    return container;
}

template<template<class,class> class C, class T> 
C<T, Eigen::aligned_allocator<T> > mkOneValueContAA(const T& value){
    C<T, Eigen::aligned_allocator<T> > container;
    //std::back_insert_iterator<C<T, Eigen::aligned_allocator<T> > > it = back_inserter(container);
    //it = value;
    container.push_back(value);
    return container;
}

inline geometry_msgs::Pose getPoseFromTcwInSE3(const SE3& T_cw){
    geometry_msgs::Pose pose;

    Vector3d t = T_cw.translation();
    geometry_msgs::Point point;
    point.x=t(0);
    point.y=t(1);
    point.z=t(2);
    pose.position = point;

    geometry_msgs::Quaternion quat;
    Eigen::Quaterniond q = T_cw.unit_quaternion();
    quat.x=q.x();
    quat.y=q.y();
    quat.z=q.z();
    quat.w=q.w();
    pose.orientation = quat;

    return pose;
}


/*------------------------------------------------------------------------------------------*/
template<class T> vector<T> getArrayIdices(const vector<T>& array, const vector<int>& idxs){
    typename vector<int>::const_iterator it = idxs.begin();
    vector<T> result;
    while(it!=idxs.end()){
        result.push_back(array[*it]);
        ++it;
    }
    return result;
}

template<class T> cv::Mat getArrayIdices(const cv::Mat& mat, const vector<int>& idxs){
    cv::Mat result(0, mat.size().width, mat.type());
    result.reserve(idxs.size());
    typename vector<int>::const_iterator it = idxs.begin();
    while(it!=idxs.end()){
        result.push_back(mat.row(*it));
        ++it;
    }
    return result;
}

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

template<template<class T, class = std::allocator<T> > class C, class T> C<T> 
mkContainer(const T& val){
    C<T> container;
    container.push_back(val);
    return container;
}

inline Vector2d c1PixelToc2Pixel(const SE3& T_c1c2, const Vector2d& pixel1){
    Vector3d v = T_c1c2 * PanoImCoord::fromPixel(pixel1).getHomogenous();
    return PanoImCoord::fromHomogenous(v).getPixel();
}


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



#endif