YoshiRi

clear all
close all

%% config
%true depth
% sampling time 
ST = 0.033 %33ms
END = 5.0 % 5sec simulation
STEREO_NOISE_S = 1; % 1px sigma for stereo disparity noise
MONO_NOISE_S = 1; % 1?? sigma for monocular distance estimation noise

YoshiRi

from ctypes import *
import math
import random
import numpy as np
import matplotlib.pyplot as plt
import time # time.time() to get time

class Checktime:
    def __init__(self):
        self.checktime = [0]

YoshiRi

YoshiRi

// Look at https://github.com/vonzhou/opencv/blob/master/image-search/orb.cpp
std::vector<cv::DMatch> ratio_test(std::vector< std::vector<cv::DMatch> > matches12, double ratio, std::vector<cv::KeyPoint> kpl, std::vector<cv::KeyPoint> kpr){
    std::vector<cv::DMatch> good_matches;         
    for(int i = 0; i < matches12.size(); i++){  
        if(abs(kpl[matches12[i][0].queryIdx].pt.y - kpr[matches12[i][0].trainIdx].pt.y ) < 1){ // in the same line
            if(matches12[i][0].distance < ratio * matches12[i][1].distance){
                good_matches.push_back(matches12[i][0]);
            }
        }
    }

YoshiRi

// for image processing
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/ximgproc/disparity_filter.hpp"
#include <stdio.h>
#include <stdlib.h>

YoshiRi

/*   Author: Yoshi Ri @ Univ of Tokyo 
*            2017 Aug 30
*
*    Subscribing stereo camera image and camera info to get rectified image
*/

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>

YoshiRi

import numpy as np
#D1 = [-0.35002811852321858, 0.096534651810849512, 0.0, 0.0, 0.000000]
#D2 = [-0.36311895125549826, 0.10890311353734204, 0.0, 0.0, 0.000000]

K1 = np.array([ 439.65213495954725,0,306.67821191917398,0,436.73222033708959,245.79237261646784,0,0,1]).reshape(3,3)
K2 = np.array([ 437.00346198880374,0,324.72266880648021,0,434.00126234994462,236.08291224903141,0,0,1]).reshape(3,3)

R1 = np.array([1.0,0,0, 0,1.0,0, 0,0,1.0 ]).reshape(3,3)
R2 = np.array([0.9999669439878518,-0.001850056106487306,-0.0079175895321455282, 0.0018719784959910215,0.99999443266408283,0.002762306527137064, 0.007912435030206437,-0.0027770367736432208,0.99996484010121622 ]).reshape(3,3)

YoshiRi

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sat Jun 24 22:45:15 2017

@author: yoshi
"""

import cv2
import numpy as np

YoshiRi

# -*- coding: utf-8 -*-
# largely from here
# http://russeng.hatenablog.jp/entry/2015/06/16/004704

import numpy
import cv2
# get file name efficiently
from glob import glob 

def main():

YoshiRi

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 23 00:05:14 2017

@author: ossyaritoori
"""

#coding: utf-8