YoshiRi

import pyocr
import cv2
import numpy as np
import pyocr.builders
import matplotlib.pyplot as plt
from PIL import Image, ImageDraw, ImageFont

## 参考：https://qiita.com/mo256man/items/82da5138eeacc420499d
def cv2_putJPText(img, text, org, fontFace='C:\Windows\Fonts\HGRPP1.TTC', fontScale=10, color=(255,0,0), mode=0):
    """

YoshiRi

YoshiRi

YoshiRi

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Euler angle to quaternion 
% input : roll,pitch,yaw [rad]
% output quaternion: 
% you need robotics toolbox
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Q = eul2quat_self(rpy)

roll = rpy(1);

YoshiRi

///////////////////////////////////////////
//  Yoshi Ri @ Univ Tokyo
//  8 April 2018
//////////////////////////////////////////

// for servo motor control
#include <Servo.h>
#include <Encoder.h>
// ROS communication
//#include <ArduinoHardware.h>

YoshiRi

YoshiRi

YoshiRi

% figure のデフォルトフォント設定
set(0, 'defaultAxesFontSize', 12);
set(0, 'defaultTextFontSize', 12);
set(0, 'defaultAxesFontName','Times-Roman');
set(0,'defaultTextFontName','Times-Roman');
% ブロックのデフォルトフォントを変更
set_param(0,'DefaultBlockFontSize',11);
set_param(0,'DefaultBlockFontName','Times-Roman');
% ラインのデフォルトフォントサイズを変更
set_param(0,'DefaultLineFontSize',9);

YoshiRi

%%  test for particle filter
% Problem Setting is here
% https://ja.wikipedia.org/wiki/%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%BC#%E5%BF%9C%E7%94%A8%E4%BE%8B

% settings
dt = 0.001; % time sample
s_sys = 10000; % system noise
s_out = 0.05; % measurement noise
s_rout = 0.1;
% system eq

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/xfeatures2d.hpp"
#include "opencv2/flann.hpp"
#include "opencv2/cudaarithm.hpp"
#include "opencv2/cudafeatures2d.hpp"