vis.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 "vis.h"
#include "model_estimation.h"
#include <iostream>

//needed also by epipolar matching
double getInPlaneThetaByPhi(const Eigen::Hyperplane<double, 3> & epipolar_plane, double x){
    Eigen::Vector4d c = epipolar_plane.coeffs();
    Vector3d h = PanoImCoord::fromPixel(Vector2d(x, 0.0)).getHomogenous();
    assert(c(3) == 0);
    double y = (-c(0)*h(0) - c(2)*h(2)) / c(1);
    return PanoImCoord::fromHomogenous(Vector3d(h(0), y, h(2))).getPixel()(1);
}


typedef list<std::pair<std::pair<double, double>, std::pair<double, double> > > LineList;

void tmpname(const Eigen::Hyperplane<double,3>& plane, 
        double max_len, double x1, double x2, LineList& lines ){
    double y1, y2;
    y1=getInPlaneThetaByPhi(plane, x1);
    y2=getInPlaneThetaByPhi(plane, x2);
    double length = std::sqrt(std::pow(x1-x2, 2) + std::pow(y2-y1, 2));
    if(length > max_len){
        tmpname(plane, max_len, x1, x1+(x2-x1)/2.0, lines); 
        tmpname(plane, max_len, x1+(x2-x1)/2.0, x2, lines);
    } else{
        lines.push_back(make_pair(make_pair(x1, y1), make_pair(x2, y2)));
    }
}



void tmpname(const Eigen::Hyperplane<double,3>& plane, 
        double max_len, double x1, double x2, bool left_side, LineList& lines ){
    double y1, y2;
    y1=getInPlaneThetaByPhi(plane, x1);
    y2=getInPlaneThetaByPhi(plane, x2);
    double length = std::sqrt(std::pow(x1-x2, 2) + std::pow(y2-y1, 2));
    if(length > max_len){
        if(left_side){
            tmpname(plane, max_len, x1+(x2-x1)/2.0, x2, left_side, lines);
        } else{
            tmpname(plane, max_len, x1, x1+(x2-x1)/2.0, left_side, lines); 
        }
    } else{
        lines.push_back(make_pair(make_pair(x1, y1), make_pair(x2, y2)));
    }
}

void drawLineSegments(LineList& lines, cv::Mat& image){
    LineList::const_iterator it;
    it=lines.begin();
    while(it != lines.end()){
        cv::Point2d pt1 = cv::Point2d(it->first.first, it->first.second);
        cv::Point2d pt2 = cv::Point2d(it->second.first, it->second.second);
        cv::line(image, pt1, pt2, CV_RGB(255,0,0), 1, 4);
        ++it;
    }
}

void drawPixelsOnPlaneThroughProjCentre(Eigen::Hyperplane<double,3> plane, double step,
        cv::Mat & image){
    double a,b;
    a=0;
    b=PanoramaParams::width - 1;
    LineList lines;
    tmpname(plane, step, a, b, lines);
    drawLineSegments(lines, image);
}

void drawPixelsOnPlaneThroughProjCentre(Eigen::Hyperplane<double,3> plane, double step,
        const Vector2d& ic, cv::Mat & image){
    double a,b;
    a=0;
    b=PanoramaParams::width - 1;
    LineList lines;
    tmpname(plane, step, a, ic(0), true, lines);
    tmpname(plane, step, ic(0), b, false, lines);
    drawLineSegments(lines, image);
}

/*
void drawRaysOnEpipolarPlane(
        const Matrix3d & E,
        const ImageMatchVec & matches,
        cv::Mat & img1,
        cv::Mat & img2){
    const double drawstep = 15.0;
    for(ImageMatchVec::const_iterator it=matches.begin(); it!=matches.end(); it++){
        PanoImCoord ic;
        double y;
        Vector3d c2_line = E * it->first;
        Eigen::Hyperplane<double,3> plane=Eigen::Hyperplane<double,3>(c2_line, 0);
        ic = PanoImCoord::fromHomogenous(it->second);
        drawPixelsOnPlaneThroughProjCentre(plane, drawstep, ic, img2);

        Vector3d c1_line = (it->second).transpose() * E;
        plane=Eigen::Hyperplane<double,3>(c1_line, 0);
        ic = PanoImCoord::fromHomogenous(it->first);
        y=getInPlaneThetaByPhi(plane, ic.getPixel()(0));
        cv::Point2d pt1 = cv::Point2d(ic.getPixel()(0), ic.getPixel()(1));
        cv::Point2d pt2 = cv::Point2d(ic.getPixel()(0), y);
        cv::line(img1, pt1, pt2, CV_RGB(0,0,255), 1, 4);
        drawPixelsOnPlaneThroughProjCentre(plane, 
                drawstep, ic, img1);
    }
}*/

void drawRaySegment(const Vector3d& line, const Vector3d& point_hom, cv::Mat& img){
    Eigen::Hyperplane<double,3> plane=Eigen::Hyperplane<double,3>(line, 0);
    Vector2d point_pixel = PanoImCoord::fromHomogenous(point_hom).getPixel();
    Vector2d pt_on_plane_pixel = PanoImCoord::fromHomogenous(
            closestPointOnEpipolarPlane(line, point_hom)).getPixel();
    cv::Point2d pt1 = cv::Point2d(point_pixel(0), point_pixel(1));
    cv::Point2d pt2 = cv::Point2d(pt_on_plane_pixel(0), pt_on_plane_pixel(1));
    cv::line(img, pt1, pt2, CV_RGB(255,0,255), 1, 4);
    drawPixelsOnPlaneThroughProjCentre(
            plane, (double)PanoramaParams::width / 100.0, pt_on_plane_pixel, img);
}


void drawRaysOnEpipolarPlane(
        const Matrix3d & E,
        const ImageMatchVec & matches,
        cv::Mat & img1,
        cv::Mat & img2){
    Eigen::Hyperplane<double,3> plane;
    for(ImageMatchVec::const_iterator it=matches.begin(); it!=matches.end(); it++){
        Vector3d c2_line = E * it->first;
        plane=Eigen::Hyperplane<double,3>(c2_line, 0);
        drawRaySegment(c2_line, it->second, img2);

        Vector3d c1_line = (it->second).transpose() * E;
        plane=Eigen::Hyperplane<double,3>(c1_line, 0);
        drawRaySegment(c1_line, it->first, img1);
    }
    Points3dSTL im1points;
    list<double> elevations;
    double deg_in_rad=((M_PI / 2.0) / 90.0);
    elevations.push_back(0);
    elevations.push_back(40*deg_in_rad);
    elevations.push_back(-40*deg_in_rad);
    elevations.push_back(80*deg_in_rad);
    elevations.push_back(-80*deg_in_rad);
    //list<double>::const_iterator it = elevations.begin();
    /*while(it != elevations.end()){
        Vector3d point = PanoImCoord::fromSpherical(Vector2d(M_PI, *it)).getHomogenous();
        Vector3d c2_line = E * point;
        plane=Eigen::Hyperplane<double,3>(c2_line, 0);
        drawPixelsOnPlaneThroughProjCentre(plane, (double)PanoramaParams::width / 100.0, img1);
        Vector3d c1_line = point.transpose() * E;
        plane=Eigen::Hyperplane<double,3>(c1_line, 0);
        drawPixelsOnPlaneThroughProjCentre(plane, (double)PanoramaParams::width / 100.0, img2);
        ++it;
    }*/
}

void drawKeypoints(const vector<cv::KeyPoint> & keypoints, cv::Mat & image){
    for(unsigned int i = 0; i< keypoints.size(); i++){
        float x = keypoints[i].pt.x;
        float y = keypoints[i].pt.y;
        cv::circle(image,cv::Point(x,y),3,cv::Scalar(255,255,255),3);
    }
}

void drawKeypoints(const ImageCoordinateVec & keypoints, cv::Mat & image){
    for(unsigned int i = 0; i< keypoints.size(); i++){
        Vector2d c = keypoints[i].getPixel();
        cv::circle(image,cv::Point(c(0), c(1)),3,cv::Scalar(255,255,255),3);
    }
}