utils.cpp

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#include "utils.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/random.hpp>

boost::mt19937 gen;
boost::mt19937 rng_mt19937; 

ImageMatchVec imageMatchCoords(const vector<cv::KeyPoint> & tkpts, const vector<cv::KeyPoint> & qkpts,
        const std::vector<cv::DMatch> &matches){
    ALIGNED<ImageMatch>::vector spherical_matches;
    for(unsigned int i = 0 ; i < matches.size() ; i ++){
        cv::KeyPoint qkp = qkpts[matches[i].queryIdx];
        cv::KeyPoint tkp = tkpts[matches[i].trainIdx];
        Eigen::Vector3d qpt = (PanoImCoord::fromPixel(Vector2d(qkp.pt.x, qkp.pt.y))).getHomogenous();
        Eigen::Vector3d tpt = (PanoImCoord::fromPixel(Vector2d(tkp.pt.x, tkp.pt.y))).getHomogenous();
        spherical_matches.push_back(std::pair<Vector3d, Vector3d>(tpt, qpt));
    }
    return spherical_matches;
}


void points3dSTLToPointMatrix(const Points3dSTL & pts, PointMatrix & ptmat){
    for(Points3dSTL::size_type i = 0; i<pts.size(); i++){
        ptmat.block<3,1>(0, i) = pts[i];
    }
}

ImageMatchVec buildImageMatchesFrom3dPoints(
        const SE3 & T_wc1, const SE3 & T_wc2, const Points3dSTL & points){
    ImageMatchVec matches;
    for (unsigned int i = 0; i < points.size(); i++){
        matches.push_back(ImageMatch(T_wc1*points[i], T_wc2*points[i]));
    }
    return matches;
}

Points3dSTL projectPoints(SE3 T_sd, const Points3dSTL & points){
    Points3dSTL res;
    for(Points3dSTL::const_iterator it = points.begin(); it != points.end(); it++){
        Vector3d pt = *it;
        res.push_back(T_sd*pt);
    }
    return res;
}


double calculateVolume( const Vector3d & upper_left_deep, const Vector3d & bottom_right_shallow){
     Vector3d tmp = bottom_right_shallow - upper_left_deep;
     return abs(tmp(0)*tmp(1)*tmp(2));
}



void generateRandomScene(
        const Vector3d & upper_left_deep,
        const Vector3d & bottom_right_shallow,
        double target_num_vec,
        Points3dSTL & points){
    vector<boost::uniform_real<> > uniform;
    for(int i=0; i<3;i++){
        double a = upper_left_deep(i);
        double b = bottom_right_shallow(i);
        if(a < b){
            uniform.push_back(boost::uniform_real<>(a, b));
        } else{
            uniform.push_back(boost::uniform_real<>(b, a));
        }
    }
    for(int j=0; j < target_num_vec; j++){
        Vector3d vec;
        for(int i=0; i < 3 ; i++){
            vec(i) = uniform[i](gen);
        }
        points.push_back(vec);
    }
}


void generateRandomTrajectory(
        const Vector3d & upper_left_deep,
        const Vector3d & bottom_right_shallow,
        int num_stops,
        ALIGNED<SE3>::vector & transforms){ //T_wcx transforms for each pose
    Points3dSTL poses;
    Points3dSTL orientations;
    generateRandomScene(upper_left_deep, bottom_right_shallow, num_stops, poses);
    generateRandomScene(Vector3d::Zero(), Vector3d(2*M_PI, 2*M_PI, 2*M_PI), num_stops, orientations);
    transforms.push_back(SE3());
    for(int i=0; i<num_stops; i++){
        Vector3d tmp = orientations[i];
        transforms.push_back(SE3(SO3(tmp(0), tmp(1), tmp(2)), poses[i]).inverse());
    }
}


bool isApprox(double num, double target_num, double error){
    Matrix<double, 1, 1> m;
    m(0,0) = num - target_num;
    return m.isZero(error);
}