├── .gitignore ├── .spyderproject ├── KF.py ├── Main.py ├── README.md ├── Util.py ├── _old_code_.txt ├── _run_Main.cmd ├── data ├── CSVtoOutput.py ├── GetImageData.py ├── GetImageDescriptorData.py ├── GetImagePlot.py ├── GetImagePlotSingle.py ├── GetOutputData.py ├── GetOutputDataTemp.py ├── GetSensorData.py ├── _run_GetOutputData.cmd ├── _run_GetSensorData.cmd ├── input │ ├── IMUデータ間隔.xlsx │ └── plot.jpg ├── output │ ├── lab04 │ │ ├── 20160114_141755_position - 観測モデルによる違い.xlsx │ │ ├── inverse+共面条件 │ │ │ └── 20160114_141755_position - 比較結果.xlsx │ │ ├── inverse │ │ │ └── 20160116_151422_position - 比較結果.xlsx │ │ ├── lab04.csv │ │ ├── lab04.txt │ │ ├── 共面条件 │ │ │ └── 20160117_162704_position.xlsx │ │ ├── 部屋内実験_結果.xlsx │ │ └── 部屋内実験_結果_精度評価.xlsx │ ├── passage02 │ │ ├── 20160114_221145_position - 観測モデルによる違い.xlsx │ │ ├── inverse+共面条件 │ │ │ ├── 20160114_221145_position - 比較結果.xlsx │ │ │ └── 20160114_221145_position - 比較結果（17番まで）.xlsx │ │ ├── inverse │ │ │ ├── 20160115_224917_position - 比較結果.xlsx │ │ │ └── 20160115_224917_position - 比較結果（17番目まで）.xlsx │ │ ├── passage02 - フル.txt │ │ ├── passage02.csv │ │ ├── passage02.txt │ │ ├── 共面条件 │ │ │ └── 20160115_224917_position.xlsx │ │ ├── 建物内実験_結果.xlsx │ │ └── 建物内実験_結果_精度評価.xlsx │ └── 実行時間の計測 │ │ ├── IMUデータ観測時 │ │ ├── IMUデータ観測時_カルマンフィルタ.txt │ │ ├── IMUデータ観測時_カルマンフィルタ.xlsx │ │ ├── IMUデータ観測時_パーティクルフィルタ.txt │ │ └── IMUデータ観測時_パーティクルフィルタ.xlsx │ │ └── 画像データ更新時 │ │ ├── InverseDepth+共面条件.txt │ │ ├── InverseDepth+共面条件.xlsx │ │ ├── InverseDepth.txt │ │ ├── InverseDepth.xlsx │ │ ├── 共面条件.txt │ │ └── 共面条件.xlsx ├── plot3d.py ├── run_GetImageData.cmd ├── run_GetImageDescriptorData.cmd ├── run_GetImagePlot.cmd └── run_GetOutputDataTemp.cmd ├── descriptor.py ├── image.py ├── image_RBPF.py ├── image_coplanarity.py ├── keypoint.py ├── keypoint_pair.py ├── landmark.py ├── landmarkObservation.py ├── particle.py ├── particle_filter.py ├── particle_filter_IMU.py ├── particle_filter_IMU2.py ├── particle_filter_RBPF.py ├── particle_filter_coplanarity.py ├── particle_filter_normal.py ├── sensor.py ├── state.py ├── state_IMU_KF.py ├── state_IMU_PF.py ├── state_RBPF.py ├── state_coplanarity.py └── test.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc -------------------------------------------------------------------------------- /.spyderproject: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/.spyderproject -------------------------------------------------------------------------------- /KF.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | import copy 4 | import numpy as np 5 | 6 | 7 | def execEKF1Update(z, h, mu0, Sigma0, H, R): 8 | '''Linear Kalman Filter 9 | 10 | - 観測方程式 11 | z = H * x + v, v ~ N(0,R) 12 | 13 | Parameters 14 | ========== 15 | - z : 観測列 16 | - mu0 : 初期状態推定値 17 | - Sigma0 : 初期誤差共分散行列 18 | - H, R : カルマンフィルタの係数 19 | 20 | Returns 21 | ======= 22 | - M : 状態推定値列 23 | ''' 24 | 25 | mu = copy.deepcopy(mu0) # 初期状態推定値 26 | Sigma = copy.deepcopy(Sigma0) # 初期誤差共分散行列 27 | 28 | # 更新 29 | S = H.dot(Sigma.dot(H.T)) + R 30 | Sinv = np.linalg.inv(S) 31 | K = Sigma.dot(H.T.dot(Sinv)) 32 | mu = mu + K.dot(z - h) 33 | Sigma = Sigma - K.dot(H.dot(Sigma)) 34 | 35 | return (mu, Sigma, S, Sinv) 36 | 37 | 38 | def execKF1Simple(Y, mu0, Sigma0, A, C, Q, R): 39 | '''Linear Kalman Filter 40 | 41 | - 状態方程式 42 | x = A * x_ + w, w ~ N(0,Q) 43 | - 観測方程式 44 | y = C * x + v, v ~ N(0,R) 45 | 46 | Parameters 47 | ========== 48 | - Y : 観測列 49 | - mu0 : 初期状態推定値 50 | - Sigma0 : 初期誤差共分散行列 51 | - A, C, Q, R : カルマンフィルタの係数 52 | 53 | Returns 54 | ======= 55 | - M : 状態推定値列 56 | ''' 57 | 58 | mu = copy.deepcopy(mu0) # 初期状態推定値 59 | Sigma = copy.deepcopy(Sigma0) # 初期誤差共分散行列 60 | 61 | # 推定 62 | mu_ = A.dot(mu) 63 | Sigma_ = Q + A.dot(Sigma.dot(A.T)) 64 | 65 | # 更新 66 | yi = Y - C.dot(mu_) 67 | S = C.dot(Sigma_.dot(C.T)) + R 68 | K = Sigma_.dot(C.T.dot(np.linalg.inv(S))) 69 | mu = mu_ + K.dot(yi) 70 | Sigma = Sigma_ - K.dot(C.dot(Sigma_)) 71 | 72 | return (mu, Sigma) 73 | 74 | 75 | def execKF1(Y, U, mu0, Sigma0, A, B, C, Q, R): 76 | '''Linear Kalman Filter 77 | 78 | - 状態方程式 79 | x = A * x_ + B * u + w, w ~ N(0,Q) 80 | - 観測方程式 81 | y = C * x + v, v ~ N(0,R) 82 | 83 | Parameters 84 | ========== 85 | - Y : 観測列 86 | - U : 入力列 87 | - mu0 : 初期状態推定値 88 | - Sigma0 : 初期誤差共分散行列 89 | - A, B, C, Q, R : カルマンフィルタの係数 90 | 91 | Returns 92 | ======= 93 | - M : 状態推定値列 94 | ''' 95 | 96 | mu = mu0 # 初期状態推定値 97 | Sigma = Sigma0 # 初期誤差共分散行列 98 | 99 | # 推定 100 | mu_ = A.dot(mu) + B.dot(U) 101 | Sigma_ = Q + A.dot(Sigma.dot(A.T)) 102 | 103 | # 更新 104 | yi = Y - C.dot(mu_) 105 | S = C.dot(Sigma_.dot(C.T)) + R 106 | K = Sigma_.dot(C.T.dot(np.linalg.inv(S))) 107 | mu = mu_ + K.dot(yi) 108 | Sigma = Sigma_ - K.dot(C.dot(Sigma_)) 109 | 110 | return (mu, Sigma) 111 | 112 | 113 | def execKF(T, Y, U, mu0, Sigma0, A, B, C, Q, R): 114 | '''Linear Kalman Filter 115 | 116 | - 状態方程式 117 | x = A * x_ + B * u + w, w ~ N(0,Q) 118 | - 観測方程式 119 | y = C * x + v, v ~ N(0,R) 120 | 121 | Parameters 122 | ========== 123 | - T : ステップ数 124 | - Y : 観測列 125 | - U : 入力列 126 | - mu0 : 初期状態推定値 127 | - Sigma0 : 初期誤差共分散行列 128 | - A, B, C, Q, R : カルマンフィルタの係数 129 | 130 | Returns 131 | ======= 132 | - M : 状態推定値列 133 | ''' 134 | 135 | mu = mu0 # 初期状態推定値 136 | Sigma = Sigma0 # 初期誤差共分散行列 137 | 138 | M = [mu] # 状態推定値列 139 | 140 | for i in range(T): 141 | # 推定 142 | mu_ = A * mu + B * U[i] 143 | Sigma_ = Q + A * Sigma * A.T 144 | 145 | # 更新 146 | yi = Y[i+1] - C * mu_ 147 | S = C * Sigma_ * C.T + R 148 | K = Sigma_ * C.T * S.I 149 | mu = mu_ + K * yi 150 | Sigma = Sigma_ - K * C * Sigma_ 151 | M.append(mu) 152 | 153 | return M 154 | -------------------------------------------------------------------------------- /Main.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | Main.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | The process starts from this program. 9 | This program receives data from Android application via MQTT, and then send the data to Sensor class or Image class. 10 | Data are: 11 | IMU sensor data MQTT topic = SLAM/input/all -> Sensor class 12 | Camera image data MQTT topic = SLAM/input/camera -> Image class 13 | 14 | Sensor class processes IMU sensor data, and send results to State class. 15 | Image class processes Camera image data, and send results to State class. 16 | 17 | State class estimates state (location of the device, etc.) in according to the model you selected below. 18 | 19 | FYI: 20 | MQTT is one of the lightweight messaging protocols. 21 | If you want to run this program, you must prepare your own server and install MQTT broker, and create "server.conf" on the parent directory of this file. "server.conf" is the file like "ipaddress&port&username&password". 22 | 23 | """ 24 | 25 | import paho.mqtt.client as mqtt 26 | from sensor import Sensor 27 | from state import State 28 | from image import Image 29 | #from landmarkObservation import LandmarkObservation 30 | import warnings 31 | 32 | 33 | # Main method 34 | def main(): 35 | global state, sensor, image 36 | warnings.filterwarnings('error') 37 | 38 | # ============================== # 39 | # Select model here! # 40 | # ============================== # 41 | model = "RBPF" 42 | # ===== Model options (state vector type & estimation model) ===== # 43 | # - Coplanarity (IMU with Kalman Filter & Camera with Particle Filter. Observation model is coplanarity. State vector is device state only) 44 | # - RBPF (FastSLAM. IMU with Particle Filter & Camera with Extended Kalman Filter. Observation model is inverse depth. State vector are device and landmark state. Estimated by Rao-Blackwellized particle filter) 45 | # - IMUKF (IMU with Kalman Filter) 46 | # - IMUPF (IMU with Particle Filter, IMU data is observation) 47 | # - IMUPF2 (IMU with Particle Filter, IMU data is control) 48 | # ============================== # 49 | # Select model here! # 50 | # ============================== # 51 | 52 | print("=======================================") 53 | print(" SLAM with Camera and IMU") 54 | print(" "+model+" model is selected.") 55 | print("=======================================") 56 | 57 | state = State().getStateClass(model) 58 | sensor = Sensor(state) 59 | 60 | if(model == "Coplanarity" or model == "RBPF"): 61 | image = Image().getImageClass(model) 62 | image.setState(state) 63 | 64 | #print("Loading theano ... please wait") 65 | #observation = LandmarkObservation() 66 | #if(model == "RBPF"): 67 | # state.setObservationModel(observation) 68 | 69 | #Get estimated state vector from state class and publish to the server (MQTT broker). 70 | def publish_state(): 71 | global state, sensor, image 72 | 73 | if(len(sensor.accel) == 0): 74 | return 75 | 76 | x,v,a,o = state.getState() # Get estimated state vector 77 | 78 | # Publish to the server (MQTT broker) 79 | client.publish("SLAM/output/all",str(x[0])+"&"+str(x[1])+"&"+str(x[2])+"&"+str(o[0])+"&"+str(o[1])+"&"+str(o[2])) 80 | client.publish("SLAM/output/accel",str(a[0])+"&"+str(a[1])+"&"+str(a[2])) 81 | client.publish("SLAM/output/velocity",str(v[0])+"&"+str(v[1])+"&"+str(v[2])) 82 | #client.publish("SLAM/output/temp",str(a[0])+"&"+str(a[1])+"&"+str(a[2])+"&"+str(a_temp[0])+"&"+str(a_temp[1])+"&"+str(a_temp[2])) 83 | #client.publish("SLAM/output/temp",str(o[0])+"&"+str(o[1])+"&"+str(o[2])+"&"+str(o_temp[0])+"&"+str(o_temp[1])+"&"+str(o_temp[2])) 84 | 85 | 86 | #This method is called when mqtt is connected. 87 | def on_connect(client, userdata, flags, rc): 88 | print('[Main.py] Connected with result code '+str(rc)) 89 | client.subscribe("SLAM/input/#") 90 | 91 | 92 | #This method is called when message is arrived. 93 | def on_message(client, userdata, msg): 94 | global state, sensor, image 95 | 96 | 97 | #Process data in senser.py or image.py 98 | if(str(msg.topic) == "SLAM/input/camera"): #image.py 99 | if(model == "Coplanarity" or model == "RBPF"): 100 | #print("+"), 101 | data_ = str(msg.payload).split('$') # time$data&data&... 102 | time = data_[0] 103 | data = data_[1].split('&') # data&data&... 104 | image.processData(time,data) #Process data 105 | publish_state() 106 | #print "|", 107 | else: 108 | publish_state() 109 | 110 | elif(str(msg.topic) == "SLAM/input/all"): #sensor.py 111 | #print "*", 112 | data = str(msg.payload).split('&') # data&data&... 113 | sensor.processData(data) #Process data 114 | publish_state() 115 | #print ",", 116 | 117 | elif(str(msg.topic) == "SLAM/input/stop"): #Stop 118 | print("[Main.py] stop") 119 | client.publish("SLAM/output/stop","true") 120 | 121 | state.init() 122 | sensor.init() 123 | 124 | 125 | #Read server conf file 126 | f = open('../server.conf', 'r') 127 | for line in f: 128 | serverconf = line 129 | f.close() 130 | 131 | #Mqtt connection args 132 | conf = serverconf.split('&') 133 | host = conf[0] 134 | port = int(conf[1]) 135 | username = conf[2] 136 | password = conf[3] 137 | 138 | #Mqtt connect 139 | client = mqtt.Client(client_id="SLAMpython", clean_session=True, protocol=mqtt.MQTTv311) 140 | client.on_connect = on_connect 141 | client.on_message = on_message 142 | client.username_pw_set(username, password=password) 143 | client.connect(host, port=port, keepalive=60) 144 | client.loop_forever() 145 | 146 | 147 | #Main method 148 | if __name__ == '__main__': 149 | main() 150 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SLAM with Camera and IMU for Python 2 | 画像センサとIMUを用いたSLAMのためのPythonプログラム（日本語説明は後半） 3 | 4 | >SLAM = Simultaneous Localization and Mapping 5 | 6 | ## Overview 7 | 8 | [![](http://img.youtube.com/vi/IDZ5fxp_XdY/0.jpg)](https://www.youtube.com/watch?v=IDZ5fxp_XdY) 9 | 10 | ![](https://github.com/knagara/miscellaneous/blob/master/Overview.png) 11 | 12 | - Android App (See -> [SLAMwithCameraIMUforAndroid](https://github.com/knagara/SLAMwithCameraIMUforAndroid)) 13 | - SLAM program (This page) 14 | - MQTT Broker (See -> [MQTT](http://mqtt.org/), [mosquitto](http://mosquitto.org/), [Apollo](https://activemq.apache.org/apollo/)) 15 | 16 | ## How to use 17 | 1.Setup Android App (See -> [SLAMwithCameraIMUforAndroid](https://github.com/knagara/SLAMwithCameraIMUforAndroid)) 18 | 19 | 2.Setup MQTT Broker (See -> [MQTT](http://mqtt.org/), [mosquitto](http://mosquitto.org/), [Apollo](https://activemq.apache.org/apollo/)) 20 | 21 | 3.Install MQTT package of Python 22 | ~~~ 23 | >pip install paho-mqtt 24 | ~~~ 25 | 4.Create a single line text file named 'server.conf' on the parent directory of Main.py. 26 | ~~~ 27 | server.conf 28 | [Format] ipaddress&port&username&password 29 | [Example] 160.16.xxx.xxx&61613&admin&password 30 | ~~~ 31 | 5.Run Main.py 32 | ~~~ 33 | >python Main.py 34 | ~~~ 35 | 6.Start Android App 36 | 37 | 7.Now the program is receiving sensor data and estimate smartphone location, also publishing it to MQTT broker. 38 | 39 | 8.If you want to save estimated data as CSV, run GetOutputData.py 40 | ~~~ 41 | >python ./data/GetOutputData.py 42 | ~~~ 43 | 44 | ## Important Files 45 | 46 | |File name|Explanation| 47 | |:--|:--| 48 | |Main.py|The entry point of SLAM program
Receive sensor data
Publish estimated location| 49 | |image_RBPF.py|Parse sensor data of camera| 50 | |landmark.py|Landmark (Keypoint in 3D space) class
Initialize landmark parameters
Observation model| 51 | |particle.py|Particle class| 52 | |particle_filter_RBPF.py|Particle filter| 53 | |sensor.py|Parse sensor data of IMU| 54 | |state_RBPF.py|Manage state variable| 55 | |data/GetOutputData.py|Receive estimated data and save them as CSV| 56 | 57 | ## Data Flow 58 | 59 | ![](https://github.com/knagara/miscellaneous/blob/master/dataflow_.png) 60 | 61 |

62 | 63 | ## 概要 64 | 65 | [![](http://img.youtube.com/vi/IDZ5fxp_XdY/0.jpg)](https://www.youtube.com/watch?v=IDZ5fxp_XdY) 66 | 67 | ![](https://github.com/knagara/miscellaneous/blob/master/Overview.png) 68 | 69 | - Androidアプリ（ここを参照 -> [SLAMwithCameraIMUforAndroid](https://github.com/knagara/SLAMwithCameraIMUforAndroid)） 70 | - SLAMプログラム（このページ） 71 | - MQTTブローカー（ここを参照 -> [MQTTについて詳しく知る](https://sango.shiguredo.jp/mqtt), [MQTTについてのまとめ](http://tdoc.info/blog/2014/01/27/mqtt.html), [MQTT Broker比較](http://acro-engineer.hatenablog.com/entry/2015/12/09/120500)） 72 | 73 | ## 使い方 74 | 1.Androidアプリをセットアップ（ここを参照 -> [SLAMwithCameraIMUforAndroid](https://github.com/knagara/SLAMwithCameraIMUforAndroid)） 75 | 76 | 2.MQTTブローカーをセットアップ（ここを参照 -> [sango](https://sango.shiguredo.jp/)） 77 | 78 | 3.PythonのMQTTパッケージをインストール 79 | ~~~ 80 | >pip install paho-mqtt 81 | ~~~ 82 | 4.server.confという名前のファイルをMain.pyの親ディレクトリに作成する 83 | ~~~ 84 | server.conf 85 | [フォーマット] ipaddress&port&username&password 86 | [例] 160.16.xxx.xxx&61613&admin&password 87 | ~~~ 88 | 5.Main.pyを起動 89 | ~~~ 90 | >python Main.py 91 | ~~~ 92 | 6.Androidアプリを起動 93 | 94 | 7.センサデータを受信し、位置の推定が始まります。推定結果はMQTTブローカーに送信されます。 95 | 96 | 8.推定結果をCSVファイルに保存したい場合は、GetOutputData.pyを起動 97 | ~~~ 98 | >python ./data/GetOutputData.py 99 | ~~~ 100 | 101 | ## 重要なファイル 102 | 103 | |File name|Explanation| 104 | |:--|:--| 105 | |Main.py|SLAMプログラムのエントリポイント
センサデータの受信
推定結果の送信| 106 | |image_RBPF.py|画像センサデータのパース| 107 | |landmark.py|ランドマーク（3D空間中の特徴点）クラス
ランドマークのパラメータの初期化
観測モデル| 108 | |particle.py|パーティクルクラス| 109 | |particle_filter_RBPF.py|パーティクルフィルタ| 110 | |sensor.py|IMUセンサデータのパース| 111 | |state_RBPF.py|状態変数の管理| 112 | |data/GetOutputData.py|推定結果を受信してCSVで保存| 113 | 114 | ## データフロー 115 | 116 | ![](https://github.com/knagara/miscellaneous/blob/master/dataflow_.png) 117 | -------------------------------------------------------------------------------- /Util.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from math import * 4 | import cv2 as cv 5 | import numpy as np 6 | 7 | def isDeviceMoving(a): 8 | if(abs(a) < 0.005): 9 | return False 10 | else: 11 | return True 12 | 13 | def lowPassFilter(value,newValue,alpha): 14 | value = value * alpha + newValue * (1 - alpha) 15 | return value 16 | 17 | """ 18 | High-pass filter 19 | - newValues 新しい値 20 | - lowValue 前回の低周波領域値が渡され、今回の低周波領域値が格納される配列 21 | - value ハイパスフィルタ適用後の値が格納される配列 22 | """ 23 | def highPassFilter(newValues, lowValue, alpha): 24 | lowValue = alpha * lowValue + (1 - alpha) * newValues 25 | value = newValues - lowValue 26 | return value, lowValue 27 | 28 | def matrixGyro2Euler(x,y): 29 | #回転行列 30 | R = np.array([ 31 | [1, sin(x)*tan(y), cos(x)*tan(y)], 32 | [0, cos(x), -sin(x)], 33 | [0, sin(x)/cos(y), cos(x)/cos(y)] 34 | ]) 35 | return R 36 | 37 | def rotationMatrixX(r): 38 | C = cos(r) 39 | S = sin(r) 40 | #回転行列 41 | R = np.array([ 42 | [1, 0, 0], 43 | [0, C, -S], 44 | [0, S, C] 45 | ]) 46 | return R 47 | 48 | def rotationMatrixY(r): 49 | C = cos(r) 50 | S = sin(r) 51 | #回転行列 52 | R = np.array([ 53 | [C, 0, S], 54 | [0, 1, 0], 55 | [-S, 0, C] 56 | ]) 57 | return R 58 | 59 | def rotationMatrixZ(r): 60 | C = cos(r) 61 | S = sin(r) 62 | #回転行列 63 | R = np.array([ 64 | [C, -S, 0], 65 | [S, C, 0], 66 | [0, 0, 1] 67 | ]) 68 | return R 69 | -------------------------------------------------------------------------------- /_old_code_.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | def predictionAndUpdateOneParticle(self, X, dt, dt2, keypoints, step, P): 10 | 11 | weight = 0.0 12 | weights = [] 13 | count_of_known_keypoints = 0 14 | x_diff_sum = np.array([0.0, 0.0, 0.0]) 15 | x_diffs = [] 16 | 17 | # 姿勢予測 prediction of position 18 | X_ = Particle() 19 | X_.landmarks = X.landmarks 20 | X_.x = X.x + dt*X.v + dt2*X.a 21 | X_.v = X.v + dt*X.a 22 | X_.a = X.a 23 | X_.o = X.o 24 | 25 | for keypoint in keypoints: 26 | ############################# 27 | start_time_ = time.clock() 28 | ############################# 29 | # previous landmark id 30 | prevLandmarkId = (step-1)*10000 + keypoint.prevIndex 31 | # new landmark id 32 | landmarkId = step*10000 + keypoint.index 33 | # The landmark is already observed or not? 34 | if(X.landmarks.has_key(prevLandmarkId) == False): 35 | # Fisrt observation 36 | # Initialize landmark and append to particle 37 | landmark = Landmark() 38 | landmark.init(X, keypoint, P, self.focus) 39 | X.landmarks[landmarkId] = landmark 40 | else: 41 | # Already observed 42 | count_of_known_keypoints += 1 43 | X.landmarks[landmarkId] = X.landmarks[prevLandmarkId] 44 | del X.landmarks[prevLandmarkId] 45 | 46 | # Actual observation z 47 | z = np.array([keypoint.x, keypoint.y]) 48 | 49 | # 計測予測 prediction of observation 50 | # Calc h(x), Hx, Hm (Jacobian matrix of h with respect to X and Landmark) 51 | z__, Hx, Hm = X.landmarks[landmarkId].calcObservation(X_, self.focus) 52 | 53 | # 姿勢のカルマンフィルタ Kalman filter of position 54 | S = Hm.dot(X.landmarks[landmarkId].sigma.dot(Hm.T)) + self.R 55 | L = S + Hx.dot(self.Q.dot(Hx.T)) 56 | Sinv = np.linalg.inv(S) 57 | Linv = np.linalg.inv(L) 58 | sigmax = np.linalg.inv( Hx.T.dot(Sinv.dot(Hx)) + np.linalg.inv(self.Q) ) 59 | mux = sigmax.dot(Hx.T.dot(Sinv.dot(z - z__))) 60 | 61 | # 姿勢のサンプリング sampling of position 62 | x_diff = np.random.multivariate_normal(mux, sigmax) 63 | x_diffs.append(x_diff) 64 | x_diff_sum += x_diff 65 | 66 | # 計測再予測 reprediction of observation 67 | z_ = X.landmarks[landmarkId].h(X_.x + x_diff, X.o, self.focus) 68 | 69 | # ランドマークの予測 prediction of landmark 70 | K = X.landmarks[landmarkId].sigma.dot(Hm.T.dot(Sinv)) 71 | X.landmarks[landmarkId].mu += K.dot(z - z_) 72 | X.landmarks[landmarkId].sigma = X.landmarks[landmarkId].sigma - K.dot(Hm.dot(X.landmarks[landmarkId].sigma)) 73 | 74 | # 重み計算 calc weight 75 | w = (1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * L) ))) * np.exp( -0.5 * ( (z-z_).T.dot(Linv.dot(z-z_)) ) ) 76 | weights.append(w) 77 | 78 | ############################### 79 | end_time_ = time.clock() 80 | if(self.count == 0): 81 | #print(x_diff) 82 | #print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_) 83 | pass 84 | ############################### 85 | 86 | if(count_of_known_keypoints > 0): 87 | x_diff = x_diff_sum/float(count_of_known_keypoints) 88 | X_.x += x_diff 89 | X_.v += (2.0*x_diff)/dt 90 | 91 | weight /= float(count_of_known_keypoints) 92 | weight *= 1000 93 | 94 | ############################### 95 | print("weight "+str(weight)) 96 | #if(self.count == 0): 97 | #print("weight "+str(weight)) 98 | ########################### 99 | self.count+=1 100 | ########################### 101 | 102 | return X_, weight 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | ############################### 111 | end_time_ = time.clock() 112 | if(self.count == 0): 113 | print("z "), 114 | print(z) 115 | print("z_ "), 116 | print(z_) 117 | print("z__ "), 118 | print(z__) 119 | #print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_) 120 | pass 121 | ############################### 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | def calcObservation(self, X, focus): 131 | """ 132 | Calc h and H (Jacobian matrix of h) 133 | 134 | Observation function 135 | z = h(x) + v 136 | 137 | h(x) = [h1(x), h2(x)].T 138 | h1(x) = f*hx/hz - cx 139 | h2(x) = f*hy/hz - cy 140 | """ 141 | 142 | # often used variables 143 | # xi, yi, zi, xt, yt, zt, p (Inverse depth) 144 | xi = self.mu[0] 145 | yi = self.mu[1] 146 | zi = self.mu[2] 147 | xt = X.x[0] 148 | yt = X.x[1] 149 | zt = X.x[2] 150 | p = self.mu[5] 151 | # sin, cos 152 | sinTheta = sin(self.mu[3]) 153 | cosTheta = cos(self.mu[3]) 154 | sinPhi = sin(self.mu[4]) 155 | cosPhi = cos(self.mu[4]) 156 | 157 | # Rotation matrix (Global coordinates -> Local coordinates) 158 | rotXinv = Util.rotationMatrixX(-X.o[0]) 159 | rotYinv = Util.rotationMatrixY(-X.o[1]) 160 | rotZinv = Util.rotationMatrixZ(-X.o[2]) 161 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 162 | 163 | # hG = [hx, hy, hz].T in the global coordinates 164 | hG = np.array([p * (xi - xt) + cosPhi * sinTheta, 165 | p * (yi - yt) - sinPhi, 166 | p * (zi - zt) + cosPhi * cosTheta]) 167 | 168 | # hL = h Local = [hx, hy, hz].T in the local coordinates 169 | hL = np.dot(R, hG) 170 | hx = hL[0] 171 | hy = hL[1] 172 | hz = hL[2] 173 | 174 | # h1 = - f*hx/hz, h2 = - f*hy/hz , and Device coordinates -> Camera coordinates 175 | h1 = - (focus * hx / hz) 176 | h2 = focus * hy / hz 177 | 178 | # derivative 179 | R11 = R[0][0] 180 | R12 = R[0][1] 181 | R13 = R[0][2] 182 | R21 = R[1][0] 183 | R22 = R[1][1] 184 | R23 = R[1][2] 185 | R31 = R[2][0] 186 | R32 = R[2][1] 187 | R33 = R[2][2] 188 | 189 | dhxxi = p * R11 190 | dhyxi = p * R21 191 | dhzxi = p * R31 192 | 193 | dhxyi = p * R12 194 | dhyyi = p * R22 195 | dhzyi = p * R32 196 | 197 | dhxzi = p * R13 198 | dhyzi = p * R23 199 | dhzzi = p * R33 200 | 201 | dhxTheta = R11 * cosPhi * cosTheta - R13 * cosPhi * sinTheta 202 | dhyTheta = R21 * cosPhi * cosTheta - R23 * cosPhi * sinTheta 203 | dhzTheta = R31 * cosPhi * cosTheta - R33 * cosPhi * sinTheta 204 | 205 | dhxPhi = - R11 * sinTheta * sinPhi - R12 * cosPhi - R13 * cosTheta * sinPhi 206 | dhyPhi = - R21 * sinTheta * sinPhi - R22 * cosPhi - R23 * cosTheta * sinPhi 207 | dhzPhi = - R31 * sinTheta * sinPhi - R32 * cosPhi - R33 * cosTheta * sinPhi 208 | 209 | dhxp = R11 * (xi - xt) + R12 * (yi - yt) + R13 * (zi - zt) 210 | dhyp = R21 * (xi - xt) + R22 * (yi - yt) + R23 * (zi - zt) 211 | dhzp = R31 * (xi - xt) + R32 * (yi - yt) + R33 * (zi - zt) 212 | 213 | # Jacobian 214 | f_hz2 = focus / (hz * hz) # focus / (hz)^2 215 | 216 | dh1xi = - f_hz2 * (dhxxi * hz - hx * dhzxi) 217 | dh1yi = - f_hz2 * (dhxyi * hz - hx * dhzyi) 218 | dh1zi = - f_hz2 * (dhxzi * hz - hx * dhzzi) 219 | dh1Theta = - f_hz2 * (dhxTheta * hz - hx * dhzTheta) 220 | dh1Phi = - f_hz2 * (dhxPhi * hz - hx * dhzPhi) 221 | dh1p = - f_hz2 * (dhxp * hz - hx * dhzp) 222 | 223 | dh2xi = f_hz2 * (dhyxi * hz - hy * dhzxi) 224 | dh2yi = f_hz2 * (dhyyi * hz - hy * dhzyi) 225 | dh2zi = f_hz2 * (dhyzi * hz - hy * dhzzi) 226 | dh2Theta = f_hz2 * (dhyTheta * hz - hy * dhzTheta) 227 | dh2Phi = f_hz2 * (dhyPhi * hz - hy * dhzPhi) 228 | dh2p = f_hz2 * (dhyp * hz - hy * dhzp) 229 | 230 | H = np.array([[dh1xi, dh1yi, dh1zi, dh1Theta, dh1Phi, dh1p], 231 | [dh2xi, dh2yi, dh2zi, dh2Theta, dh2Phi, dh2p]]) 232 | 233 | return np.array([h1,h2]), H 234 | 235 | 236 | 237 | 238 | 239 | 240 | def pf_step_camera(self, X, dt, keypoints, step, P, M): 241 | """ One Step of Sampling Importance Resampling for Particle Filter 242 | for IMU sensor 243 | Parameters 244 | ---------- 245 | X : 状態 List of state set 246 | dt : 時刻の差分 delta of time 247 | keypoints : 特徴点 keypoints 248 | step : 現在のステップ数 current step 249 | P : デバイス位置の分散共分散行列 Variance-covariance matrix of position 250 | M : パーティクルの数 num of particles 251 | Returns 252 | ------- 253 | X_resampled : 次の状態 List updated state 254 | """ 255 | 256 | # 初期化 init 257 | X_predicted = range(M) 258 | weight = range(M) 259 | X_resampled = range(M) 260 | 261 | 262 | ############################# 263 | start_time_ = time.clock() 264 | ############################# 265 | # 推定と更新 prediction and update 266 | X_predicted, weight = self.predictionAndUpdate(X, dt, keypoints, step, P) 267 | ############################### 268 | end_time_ = time.clock() 269 | #print "update time = %f" %(end_time_-start_time_) 270 | ############################### 271 | 272 | ############################# 273 | start_time_ = time.clock() 274 | ############################# 275 | # 正規化とリサンプリング normalization and resampling 276 | X_resampled = self.normalizationAndResampling(X_predicted, weight, M) 277 | ############################### 278 | end_time_ = time.clock() 279 | #print "resample time = %f" %(end_time_-start_time_) 280 | ############################### 281 | 282 | return X_resampled 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | def likelihood(self, keypoints, step, P, X): 291 | """ Likelihood function 292 | - 尤度関数 293 | p(y|x) ~ exp(-1/2 * (|y-h(x)|.t * sigma * |y-h(x)|) 294 | - 観測モデル 295 | z = h(x) + v 296 | v ~ N(0, sigma) 297 | Parameters 298 | ---------- 299 | keypoints : 観測 Observation 特徴点 keypoints 300 | step : 現在のステップ数 current step 301 | x : 予測　Predicted particle 302 | Returns 303 | ------- 304 | likelihood : 尤度 Likelihood 305 | """ 306 | 307 | rss = 0.0 # Residual sum of squares 308 | likelihood = 0.0 # Likelihood 309 | 310 | for keypoint in keypoints: 311 | # previous landmark id 312 | prevLandmarkId = (step-1)*10000 + keypoint.prevIndex 313 | # new landmark id 314 | landmarkId = step*10000 + keypoint.index 315 | # The landmark is already observed or not? 316 | ############################# 317 | start_time_ = time.clock() 318 | ############################# 319 | if(X.landmarks.has_key(prevLandmarkId) == False): 320 | # Fisrt observation 321 | # Initialize landmark and append to particle 322 | landmark = Landmark() 323 | landmark.init(X, keypoint, P, self.focus) 324 | X.landmarks[landmarkId] = landmark 325 | ############################### 326 | end_time_ = time.clock() ##################### 327 | if(self.count == 0): ############################### 328 | pass 329 | #print ""+str(landmarkId)+" init time = %f" %(end_time_-start_time_) ##################### 330 | ############################### 331 | else: 332 | # Already observed 333 | X.landmarks[landmarkId] = X.landmarks[prevLandmarkId] 334 | del X.landmarks[prevLandmarkId] 335 | # Observation z 336 | z = numpy.array([keypoint.x, keypoint.y]) 337 | # Calc h and H (Jacobian matrix of h) 338 | h, H = X.landmarks[landmarkId].calcObservation(X, self.focus) 339 | # Kalman filter (Landmark update) 340 | X.landmarks[landmarkId].mu, X.landmarks[landmarkId].sigma = KF.execEKF1Update(z, h, X.landmarks[landmarkId].mu, X.landmarks[landmarkId].sigma, H, self.R) 341 | # Calc residual sum of squares 342 | rss += (z-h).T.dot(z-h) 343 | ############################### 344 | end_time_ = time.clock() ##################### 345 | if(self.count == 0): ############################### 346 | pass 347 | #print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_) ##################### 348 | ############################### 349 | 350 | likelihood = numpy.exp( (-0.5*rss) / (self.noise_camera*len(keypoints)) ) 351 | 352 | ############################### 353 | if(self.count == 0): 354 | print("rss "+str(rss)) 355 | ########################### 356 | self.count+=1 357 | ########################### 358 | return likelihood 359 | 360 | 361 | 362 | 363 | 364 | 365 | ############################### 366 | if(self.count == 0): ############################### 367 | print("z "), 368 | print(z) 369 | print("h "), 370 | print(h) 371 | print("Hx"), 372 | print(H.dot(X.landmarks[landmarkId].mu)) 373 | ############################### 374 | 375 | 376 | 377 | 378 | 379 | class Landmark: 380 | 381 | def __init__(self, id_, step_, index_): 382 | self.id = id_ 383 | self.step = step_ 384 | self.index = index_ 385 | self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0]) 386 | self.sigma = np.zeros([]) 387 | 388 | 389 | 390 | 391 | 392 | 393 | def findLandmark(self, step_, index_): 394 | if(step_ == -1): 395 | return "none" 396 | 397 | if(len(self.landmarks) == 0): 398 | return "none" 399 | 400 | for landmark in reversed(self.landmarks): 401 | if(landmark.step < step_): 402 | break 403 | if(landmark.step == step_ and landmark.index == index_): 404 | return landmark 405 | return "none" 406 | 407 | 408 | 409 | 410 | 411 | 412 | def f_IMU(self, X, dt, dt2, accel, ori, w_a): 413 | """ Transition model 414 | - 状態方程式 415 | x_t = f(x_t-1, u) + w 416 | w ~ N(0, sigma) 417 | """ 418 | 419 | #X_new = copy.deepcopy(X) 420 | X_new = Particle() 421 | X_new.landmarks = X.landmarks 422 | 423 | # Transition with noise (only x,v) 424 | #w_mean = numpy.zeros(3) # mean of noise 425 | #w_cov_a = numpy.eye(3) * self.noise_a_sys # covariance matrix of noise (accel) 426 | #w_a = numpy.random.multivariate_normal(w_mean, w_cov_a) # generate random 427 | 428 | X_new.x = X.x + dt*X.v + dt2*X.a + dt2*w_a 429 | X_new.v = X.v + dt*X.a + dt*w_a 430 | X_new.a = accel 431 | X_new.o = ori 432 | 433 | return X_new 434 | 435 | 436 | def f_camera(self, dt, X): 437 | """ Transition model 438 | - 状態方程式 439 | x_t = f(x_t-1, u) + w 440 | w ~ N(0, sigma) 441 | """ 442 | 443 | #X_new = copy.deepcopy(X) 444 | X_new = Particle() 445 | X_new.landmarks = X.landmarks 446 | 447 | dt2 = 0.5 * dt * dt 448 | 449 | # Transition with noise (only x,v) 450 | w_mean = numpy.zeros(3) # mean of noise 451 | w_cov_a = numpy.eye(3) * self.noise_a_sys # covariance matrix of noise (accel) 452 | w_a = numpy.random.multivariate_normal(w_mean, w_cov_a) # generate random 453 | 454 | X_new.x = X.x + dt*X.v + dt2*X.a + dt2*w_a 455 | X_new.v = X.v + dt*X.a + dt*w_a 456 | X_new.a = X.a 457 | X_new.o = X.o 458 | 459 | return X_new 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | def predictionAndUpdateIMU(self, X, dt, accel, ori, M, noise_a_sys): 468 | print("start") 469 | queue = multiprocessing.Queue() 470 | jobs = [] 471 | for Xlist in list(more_itertools.chunked(X, int(M/4))): 472 | job = multiprocessing.Process(target=worker_IMU, args=(queue, Xlist, dt, accel, ori, noise_a_sys)) 473 | jobs.append(job) 474 | job.start() 475 | [j.join() for j in jobs] 476 | return [queue.get() for i in xrange(M)] 477 | 478 | 479 | 480 | 481 | def predictionAndUpdateIMU(self, X, X_predicted, dt, accel, ori, M, noise_a_sys): 482 | pool = mp.Pool(mp.cpu_count()) 483 | results = [pool.apply_async(f_IMU, args=(Xi, dt, accel, ori, noise_a_sys)) for Xi in X] 484 | output = [p.get() for p in results] 485 | pool.close() 486 | pool.join() 487 | return output 488 | 489 | 490 | 491 | 492 | 493 | 494 | start_time_ = time.clock() ##################### 495 | end_time_ = time.clock() ##################### 496 | print "time = %f" %(end_time_-start_time_) ##################### 497 | 498 | 499 | 500 | 501 | #Set new data and Execute all functions 502 | def processData(self,data): 503 | 504 | #if nomatch then nothing to do 505 | if(data[0] == "nomatch"): 506 | #print("nomatch"), 507 | return 508 | 509 | keypoints = [] 510 | for d in data: 511 | if(d != ''): 512 | keypoints.append(KeyPoint(d)) 513 | 514 | 515 | 516 | 517 | ############################## 518 | print("----------------") 519 | for k in keypointPairs: 520 | print(k.x1), 521 | print(k.y1), 522 | print(" -> "), 523 | print(k.x2), 524 | print(k.y2) 525 | ############################## 526 | 527 | 528 | 529 | 530 | ################################## 531 | # print variance of x 532 | print(self.sigma[0][0]), 533 | print(self.sigma[1][1]), 534 | print(self.sigma[2][2]) 535 | dt2 = 0.5 * 0.02 * 0.02 536 | w_mean = np.zeros(3) # mean of noise 537 | w_cov_a = np.eye(3) * self.PFnoise_a_sys # covariance matrix of noise (accel) 538 | w_a = np.random.multivariate_normal(w_mean, w_cov_a) # generate random 539 | print(dt2*w_a) 540 | ################################## 541 | 542 | 543 | 544 | # ----- Set parameters here! ----- # 545 | self.M = 100 # total number of particles パーティクルの数 546 | self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px] 547 | # Kalman Filter 548 | self.noise_a_sys = 0.01 # system noise of acceleration　加速度のシステムノイズ 549 | self.noise_g_sys = 0.01 # system noise of gyro　ジャイロのシステムノイズ 550 | self.noise_a_obs = 0.00000001 # observation noise of acceleration　加速度の観測ノイズ 551 | self.noise_g_obs = 0.00000001 # observation noise of gyro　ジャイロの観測ノイズ 552 | # Particle Filter 553 | self.PFnoise_a_sys = 5.0 # system noise of acceleration　加速度のシステムノイズ 554 | self.PFnoise_g_sys = 5.0 # system noise of gyro　ジャイロのシステムノイズ 555 | self.PFnoise_a_obs = 0.00000001 # observation noise of acceleration　加速度の観測ノイズ 556 | self.PFnoise_g_obs = 0.00000001 # observation noise of gyro　ジャイロの観測ノイズ 557 | self.PFnoise_coplanarity_obs = 0.1 # observation noise of coplanarity 共面条件の観測ノイズ 558 | # ----- Set parameters here! ----- # 559 | 560 | 561 | 562 | 563 | 564 | # count 565 | self.count += 1 566 | print(self.count), 567 | 568 | 569 | 570 | coplanarity_matrix = range(M) ######################## 571 | 572 | ############################ 573 | print(coplanarity_matrix[0][0]), 574 | print(coplanarity_matrix[0][1]), 575 | print(coplanarity_matrix[0][2]) 576 | ############################ 577 | 578 | 579 | 580 | 581 | 582 | 583 | 584 | ################################## 585 | # print variance of x 586 | print("IMU"), 587 | print(self.sigma[0][0]), 588 | print(self.sigma[1][1]), 589 | print(self.sigma[2][2]) 590 | # print variance of a 591 | #print("IMU"), 592 | #print(self.sigma[6][6]), 593 | #print(self.sigma[7][7]), 594 | #print(self.sigma[8][8]) 595 | ################################## 596 | 597 | ################################## 598 | # print variance of x 599 | print("Camera"), 600 | print(self.sigma[0][0]), 601 | print(self.sigma[1][1]), 602 | print(self.sigma[2][2]) 603 | # print variance of a 604 | #print("Camera"), 605 | #print(self.sigma[6][6]), 606 | #print(self.sigma[7][7]), 607 | #print(self.sigma[8][8]) 608 | ################################## 609 | 610 | 611 | 612 | 613 | 614 | ################### 615 | print("====================") 616 | for X_ in self.X: 617 | print(X_.x[0]), 618 | print(X_.x[1]), 619 | print(X_.x[2]) 620 | print("====================") 621 | ################### 622 | 623 | ################### 624 | print("------------------------") 625 | for X_ in self.X: 626 | print(X_.x[0]), 627 | print(X_.x[1]), 628 | print(X_.x[2]) 629 | print("------------------------") 630 | ################### 631 | -------------------------------------------------------------------------------- /_run_Main.cmd: -------------------------------------------------------------------------------- 1 | python Main.py -------------------------------------------------------------------------------- /data/CSVtoOutput.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | import csv 4 | 5 | filename = 'passage02' 6 | 7 | csvfile = './output/'+filename+'.csv' 8 | f = open(csvfile, "r") 9 | reader = csv.reader(f) 10 | header = next(reader) 11 | 12 | i = 0 13 | x = [] 14 | y = [] 15 | z = [] 16 | 17 | for row in reader: 18 | if(i==0): 19 | x = row 20 | elif(i==1): 21 | y = row 22 | elif(i==2): 23 | z = row 24 | else: 25 | pass 26 | i+=1 27 | 28 | f.close() 29 | 30 | f = open('./output/'+filename+'.txt', 'w') 31 | 32 | for i in range(len(x)): 33 | f.write('SLAM/output/all%'+x[i].strip()+'&'+y[i].strip()+'&'+z[i].strip()+'\n') 34 | 35 | f.write('SLAM/input/stop%true') 36 | f.close() -------------------------------------------------------------------------------- /data/GetImageData.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetImageData.py 5 | Subscribe image data and save them as CSV file. 6 | """ 7 | 8 | import sys 9 | import numpy as np 10 | import paho.mqtt.client as mqtt 11 | 12 | 13 | def init(): 14 | global isFirst 15 | global time 16 | global output 17 | 18 | #global variables 19 | isFirst = True 20 | time = 0 21 | output =[] 22 | 23 | 24 | #This method is called when mqtt is connected. 25 | def on_connect(client, userdata, flags, rc): 26 | print('[GetImageData] Connected with result code '+str(rc)) 27 | client.subscribe("SLAM/input/camera") 28 | client.subscribe("SLAM/input/stop") 29 | 30 | 31 | #Append new data array to the array. 32 | def appendData(time_,data): 33 | global isFirst 34 | global time 35 | global output 36 | 37 | timerow = np.array([long(time_),0,0,0,0,0]) 38 | output.append(timerow) 39 | 40 | #if nomatch then nothing to do 41 | if(data[0] == "nomatch"): 42 | nomatchrow = np.array([-9999,0,0,0,0,0]) 43 | output.append(nomatchrow) 44 | return 45 | 46 | for data_ in data: 47 | if(data_ != ''): 48 | d = data_.split(':') 49 | row = np.array([int(d[0]),float(d[2]),float(d[3]),int(d[1]),float(d[4]),float(d[5])]) 50 | output.append(row) 51 | 52 | 53 | 54 | 55 | #This method is called when message is arrived. 56 | def on_message(client, userdata, msg): 57 | global isFirst 58 | global time 59 | global output 60 | 61 | #print(msg.topic + ' ' + str(msg.payload)) 62 | 63 | #Append data to the array 64 | if(str(msg.topic) == "SLAM/input/camera"): 65 | data_ = str(msg.payload).split('$') # time$data&data&... 66 | time = data_[0] 67 | data = data_[1].split('&') # data&data&... 68 | appendData(time,data) 69 | elif(str(msg.topic) == "SLAM/input/stop"): 70 | np.savetxt('./input/image.csv', output, delimiter=',') 71 | print("[GetImageData] stop") 72 | init() 73 | 74 | 75 | #Main method 76 | if __name__ == '__main__': 77 | 78 | #global variables 79 | isFirst = True 80 | time = 0 81 | output = [] 82 | 83 | #Read server conf file 84 | f = open('../../server.conf', 'r') 85 | for line in f: 86 | serverconf = line 87 | f.close() 88 | 89 | #Mqtt 90 | conf = serverconf.split('&') 91 | host = conf[0] 92 | port = int(conf[1]) 93 | username = conf[2] 94 | password = conf[3] 95 | 96 | #Mqtt connect 97 | client = mqtt.Client(client_id="GetImageData", clean_session=True, protocol=mqtt.MQTTv311) 98 | client.on_connect = on_connect 99 | client.on_message = on_message 100 | client.username_pw_set(username, password=password) 101 | client.connect(host, port=port, keepalive=60) 102 | client.loop_forever() 103 | -------------------------------------------------------------------------------- /data/GetImageDescriptorData.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetImageDescriptorData.py 5 | Subscribe image data and save them as CSV file. 6 | """ 7 | 8 | import sys 9 | import numpy as np 10 | import paho.mqtt.client as mqtt 11 | 12 | 13 | def init(): 14 | global isFirst 15 | global time 16 | global output 17 | 18 | #global variables 19 | isFirst = True 20 | time = 0 21 | output =[] 22 | 23 | 24 | #This method is called when mqtt is connected. 25 | def on_connect(client, userdata, flags, rc): 26 | print('[GetImageDescriptor] Connected with result code '+str(rc)) 27 | client.subscribe("SLAM/input/camera") 28 | client.subscribe("SLAM/input/stop") 29 | 30 | 31 | #Append new data array to the array. 32 | def appendData(time_,data): 33 | global isFirst 34 | global time 35 | global output 36 | 37 | timerow = np.array([long(time_),0,0,0,0,0,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0]) 38 | output.append(timerow) 39 | 40 | #if nomatch then nothing to do 41 | if(data[0] == "nomatch"): 42 | nomatchrow = np.array([-9999,0,0,0,0,0,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0]) 43 | output.append(nomatchrow) 44 | return 45 | 46 | for data_ in data: 47 | if(data_ != ''): 48 | d = data_.split(':') 49 | row = np.array([int(d[0]),float(d[2]),float(d[3]),int(d[1]),float(d[4]),float(d[5]),float(d[6]), float(d[7]), float(d[8]), float(d[9]), float(d[10]), float(d[11]), float(d[12]), float(d[13]), float(d[14]), float(d[15]), float(d[16]), float(d[17]), float(d[18]), float(d[19]), float(d[20]), float(d[21]), float(d[22]), float(d[23]), float(d[24]), float(d[25]), float(d[26]), float(d[27]), float(d[28]), float(d[29]), float(d[30]), float(d[31]), float(d[32]), float(d[33]), float(d[34]), float(d[35]), float(d[36]), float(d[37]), float(d[38]), float(d[39]), float(d[40]), float(d[41]), float(d[42]), float(d[43]), float(d[44]), float(d[45]), float(d[46]), float(d[47]), float(d[48]), float(d[49]), float(d[50]), float(d[51]), float(d[52]), float(d[53]), float(d[54]), float(d[55]), float(d[56]), float(d[57]), float(d[58]), float(d[59]), float(d[60]), float(d[61]), float(d[62]), float(d[63]), float(d[64]), float(d[65]), float(d[66]), float(d[67]), float(d[68]), float(d[69])]) 50 | output.append(row) 51 | 52 | 53 | 54 | 55 | #This method is called when message is arrived. 56 | def on_message(client, userdata, msg): 57 | global isFirst 58 | global time 59 | global output 60 | 61 | #print(msg.topic + ' ' + str(msg.payload)) 62 | 63 | #Append data to the array 64 | if(str(msg.topic) == "SLAM/input/camera"): 65 | data_ = str(msg.payload).split('$') # time$data&data&... 66 | time = data_[0] 67 | data = data_[1].split('&') # data&data&... 68 | appendData(time,data) 69 | elif(str(msg.topic) == "SLAM/input/stop"): 70 | np.savetxt('./input/image_descriptor.csv', output, delimiter=',') 71 | print("[GetImageDescriptor] stop") 72 | init() 73 | 74 | 75 | #Main method 76 | if __name__ == '__main__': 77 | 78 | #global variables 79 | isFirst = True 80 | time = 0 81 | output = [] 82 | 83 | #Read server conf file 84 | f = open('../../server.conf', 'r') 85 | for line in f: 86 | serverconf = line 87 | f.close() 88 | 89 | #Mqtt 90 | conf = serverconf.split('&') 91 | host = conf[0] 92 | port = int(conf[1]) 93 | username = conf[2] 94 | password = conf[3] 95 | 96 | #Mqtt connect 97 | client = mqtt.Client(client_id="GetImageDescriptor", clean_session=True, protocol=mqtt.MQTTv311) 98 | client.on_connect = on_connect 99 | client.on_message = on_message 100 | client.username_pw_set(username, password=password) 101 | client.connect(host, port=port, keepalive=60) 102 | client.loop_forever() 103 | -------------------------------------------------------------------------------- /data/GetImagePlot.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetImageData.py 5 | Subscribe image data and plot on sequential image. 6 | """ 7 | 8 | import sys 9 | import cv2 10 | import numpy as np 11 | import paho.mqtt.client as mqtt 12 | 13 | 14 | def init(): 15 | global isFirst 16 | global time 17 | global output 18 | global count 19 | 20 | #global variables 21 | isFirst = True 22 | time = 0 23 | output =[] 24 | count = 0 25 | 26 | 27 | #This method is called when mqtt is connected. 28 | def on_connect(client, userdata, flags, rc): 29 | print('[GetImagePlot] Connected with result code '+str(rc)) 30 | client.subscribe("SLAM/input/camera") 31 | client.subscribe("SLAM/input/stop") 32 | 33 | 34 | #Append new data array to the array. 35 | def appendData(time_,data): 36 | global isFirst 37 | global time 38 | global output 39 | global count 40 | 41 | count += 1 42 | 43 | timerow = np.array([-10000,0,0,0,0,0]) 44 | output.append(timerow) 45 | 46 | #if nomatch then nothing to do 47 | if(data[0] == "nomatch"): 48 | nomatchrow = np.array([-9999,0,0,0,0,0]) 49 | output.append(nomatchrow) 50 | return 51 | 52 | for data_ in data: 53 | if(data_ != ''): 54 | d = data_.split(':') 55 | row = np.array([int(d[0]),int(d[1]),float(d[2]),float(d[3]),float(d[4]),float(d[5])]) 56 | output.append(row) 57 | 58 | 59 | def plotImage(): 60 | global output, count 61 | 62 | rows = 1920 63 | cols = 1080 * (count+1) 64 | img = np.zeros((rows, cols, 3), np.uint8) 65 | 66 | white = (255, 255, 255) 67 | green = (0, 255, 0) 68 | blue = (255, 200, 0) 69 | yellow = (0, 255, 255) 70 | 71 | font = cv2.FONT_HERSHEY_PLAIN 72 | font_size = 0.7 73 | 74 | for i in range(count): 75 | cv2.line(img, ((i+1)*1080, 0), ((i+1)*1080, 1920), white, 1) 76 | 77 | i = -1 78 | for d in output: 79 | if(d[0] == -10000): 80 | i += 1 81 | elif(d[0] == -9999): 82 | cv2.putText(img, "no match", (100 + i*1080,800), font, 10, yellow) 83 | else: 84 | if(i%2 == 0): 85 | color = green 86 | else: 87 | color = blue 88 | 89 | x1 = int(d[2]) + i*1080 90 | y1 = int(d[3]) 91 | x2 = int(d[4]) + (i+1)*1080 92 | y2 = int(d[5]) 93 | cv2.line(img, (x1,y1), (x2,y2), color, 1) 94 | cv2.line(img, (x1,y1), (x1,y1), yellow, 1) 95 | cv2.line(img, (x2,y2), (x2,y2), yellow, 1) 96 | cv2.putText(img, str(int(d[0])), (x1+2,y1), font, font_size, color) 97 | cv2.putText(img, str(int(d[1])), (x2-15,y2), font, font_size, color) 98 | 99 | #hight = img.shape[0] 100 | #width = img.shape[1] 101 | #img_half = cv2.resize(img,(hight/2,width/2)) 102 | #cv2.imwrite('./input/plot.jpg', img_half) 103 | 104 | cv2.imwrite('./input/plot.jpg', img) 105 | 106 | 107 | 108 | 109 | #This method is called when message is arrived. 110 | def on_message(client, userdata, msg): 111 | global isFirst 112 | global time 113 | global output 114 | 115 | #print(msg.topic + ' ' + str(msg.payload)) 116 | 117 | #Append data to the array 118 | if(str(msg.topic) == "SLAM/input/camera"): 119 | data_ = str(msg.payload).split('$') # time$data&data&... 120 | time = data_[0] 121 | data = data_[1].split('&') # data&data&... 122 | appendData(time,data) 123 | elif(str(msg.topic) == "SLAM/input/stop"): 124 | #np.savetxt('./input/image.csv', output, delimiter=',') 125 | plotImage() 126 | print("[GetImagePlot] stop") 127 | init() 128 | 129 | 130 | #Main method 131 | if __name__ == '__main__': 132 | 133 | #global variables 134 | isFirst = True 135 | time = 0 136 | output = [] 137 | count = 0 138 | 139 | #Read server conf file 140 | f = open('../../server.conf', 'r') 141 | for line in f: 142 | serverconf = line 143 | f.close() 144 | 145 | #Mqtt 146 | conf = serverconf.split('&') 147 | host = conf[0] 148 | port = int(conf[1]) 149 | username = conf[2] 150 | password = conf[3] 151 | 152 | #Mqtt connect 153 | client = mqtt.Client(client_id="GetImagePlot", clean_session=True, protocol=mqtt.MQTTv311) 154 | client.on_connect = on_connect 155 | client.on_message = on_message 156 | client.username_pw_set(username, password=password) 157 | client.connect(host, port=port, keepalive=60) 158 | client.loop_forever() 159 | -------------------------------------------------------------------------------- /data/GetImagePlotSingle.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetImageDataSingle.py 5 | Subscribe image data and plot on single image. 6 | """ 7 | 8 | import sys 9 | import cv2 10 | import numpy as np 11 | import paho.mqtt.client as mqtt 12 | 13 | 14 | def init(): 15 | global isFirst 16 | global time 17 | global output 18 | global count 19 | 20 | #global variables 21 | isFirst = True 22 | time = 0 23 | output =[] 24 | count = 0 25 | 26 | 27 | #This method is called when mqtt is connected. 28 | def on_connect(client, userdata, flags, rc): 29 | print('[GetImagePlotSingle] Connected with result code '+str(rc)) 30 | client.subscribe("SLAM/input/camera") 31 | client.subscribe("SLAM/input/stop") 32 | 33 | 34 | #Append new data array to the array. 35 | def appendData(time_,data): 36 | global isFirst 37 | global time 38 | global output 39 | global count 40 | 41 | count += 1 42 | 43 | timerow = np.array([-10000,0,0,0,0,0]) 44 | output.append(timerow) 45 | 46 | #if nomatch then nothing to do 47 | if(data[0] == "nomatch"): 48 | nomatchrow = np.array([-9999,0,0,0,0,0]) 49 | output.append(nomatchrow) 50 | return 51 | 52 | for data_ in data: 53 | if(data_ != ''): 54 | d = data_.split(':') 55 | row = np.array([int(d[0]),int(d[1]),float(d[2]),float(d[3]),float(d[4]),float(d[5])]) 56 | output.append(row) 57 | 58 | 59 | def plotImage(): 60 | global output, count 61 | 62 | rows = 1280 63 | cols = 720 64 | #rows = 1920 65 | #cols = 1280 66 | img = np.zeros((rows, cols, 3), np.uint8) 67 | 68 | white = (255, 255, 255) 69 | green = (0, 255, 0) 70 | blue = (255, 200, 0) 71 | yellow = (0, 255, 255) 72 | 73 | font = cv2.FONT_HERSHEY_PLAIN 74 | font_size = 0.7 75 | 76 | for d in output: 77 | if(d[0] == -10000): 78 | pass 79 | elif(d[0] == -9999): 80 | #cv2.putText(img, "no match", (100 + i*1080,800), font, 10, yellow) 81 | pass 82 | else: 83 | color = blue 84 | 85 | x1 = int(d[2]) 86 | y1 = int(d[3]) 87 | x2 = int(d[4]) 88 | y2 = int(d[5]) 89 | #cv2.line(img, (x1,y1), (x2,y2), color, 1) 90 | cv2.line(img, (x1,y1), (x1,y1), yellow, 10) 91 | cv2.line(img, (x2,y2), (x2,y2), yellow, 10) 92 | #cv2.putText(img, str(int(d[0])), (x1+2,y1), font, font_size, color) 93 | #cv2.putText(img, str(int(d[1])), (x2-15,y2), font, font_size, color) 94 | 95 | #hight = img.shape[0] 96 | #width = img.shape[1] 97 | #img_half = cv2.resize(img,(hight/2,width/2)) 98 | #cv2.imwrite('./input/plot.jpg', img_half) 99 | 100 | cv2.imwrite('./input/plot_single.jpg', img) 101 | 102 | 103 | 104 | 105 | #This method is called when message is arrived. 106 | def on_message(client, userdata, msg): 107 | global isFirst 108 | global time 109 | global output 110 | 111 | #print(msg.topic + ' ' + str(msg.payload)) 112 | 113 | #Append data to the array 114 | if(str(msg.topic) == "SLAM/input/camera"): 115 | data_ = str(msg.payload).split('$') # time$data&data&... 116 | time = data_[0] 117 | data = data_[1].split('&') # data&data&... 118 | appendData(time,data) 119 | elif(str(msg.topic) == "SLAM/input/stop"): 120 | #np.savetxt('./input/image.csv', output, delimiter=',') 121 | plotImage() 122 | print("[GetImagePlotSingle] stop") 123 | init() 124 | 125 | 126 | #Main method 127 | if __name__ == '__main__': 128 | 129 | #global variables 130 | isFirst = True 131 | time = 0 132 | output = [] 133 | count = 0 134 | 135 | #Read server conf file 136 | f = open('../../server.conf', 'r') 137 | for line in f: 138 | serverconf = line 139 | f.close() 140 | 141 | #Mqtt 142 | conf = serverconf.split('&') 143 | host = conf[0] 144 | port = int(conf[1]) 145 | username = conf[2] 146 | password = conf[3] 147 | 148 | #Mqtt connect 149 | client = mqtt.Client(client_id="GetImagePlotSingle", clean_session=True, protocol=mqtt.MQTTv311) 150 | client.on_connect = on_connect 151 | client.on_message = on_message 152 | client.username_pw_set(username, password=password) 153 | client.connect(host, port=port, keepalive=60) 154 | client.loop_forever() 155 | -------------------------------------------------------------------------------- /data/GetOutputData.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetOutputData.py 5 | Subscribe output and save them as CSV file. 6 | """ 7 | 8 | import sys 9 | import numpy as np 10 | import paho.mqtt.client as mqtt 11 | import datetime 12 | 13 | def init(): 14 | global isFirst, time, isFirstV, timeV, isFirstAll, timeAll 15 | global accel, velocity, gravity, gyro, magnet, ori, position, orientation 16 | 17 | isFirst = True 18 | time = 0 19 | isFirstV = True 20 | timeV = 0 21 | isFirstAll = True 22 | timeAll = 0 23 | accel = np.array([]) 24 | velocity = np.array([]) 25 | gravity = np.array([]) 26 | gyro = np.array([]) 27 | magnet = np.array([]) 28 | ori = np.array([]) 29 | position = np.array([]) 30 | orientation = np.array([]) 31 | 32 | 33 | #This method is called when mqtt is connected. 34 | def on_connect(client, userdata, flags, rc): 35 | print('[GetOutputData] Connected with result code '+str(rc)) 36 | client.subscribe("SLAM/output/#") 37 | 38 | 39 | #Append new data array to the array. 40 | def appendData(data): 41 | global isFirst 42 | global time 43 | global accel, velocity, gravity, gyro, magnet, ori, position, orientation 44 | 45 | time = time + 1 46 | accel0 = np.array([time,float(data[0]),float(data[1]),float(data[2])]) 47 | 48 | if(isFirst): 49 | accel = accel0 50 | isFirst = False 51 | else: 52 | accel = np.c_[accel, accel0] 53 | 54 | 55 | #Append new data array to the array. 56 | def appendDataVelocity(data): 57 | global isFirstV 58 | global timeV 59 | global accel, velocity, gravity, gyro, magnet, ori, position, orientation 60 | 61 | timeV = timeV + 1 62 | velocity0 = np.array([timeV,float(data[0]),float(data[1]),float(data[2])]) 63 | 64 | if(isFirstV): 65 | velocity = velocity0 66 | isFirstV = False 67 | else: 68 | velocity = np.c_[velocity, velocity0] 69 | 70 | 71 | #Append new data array to the array. 72 | def appendDataAll(data): 73 | global isFirstAll 74 | global timeAll 75 | global accel, velocity, gravity, gyro, magnet, ori, position, orientation 76 | 77 | timeAll = timeAll + 1 78 | position0 = np.array([timeAll,float(data[0]),float(data[1]),float(data[2])]) 79 | orientation0 = np.array([timeAll,float(data[3]),float(data[4]),float(data[5])]) 80 | #orientation0 = np.array([timeAll,float(data[3]),float(data[4]),float(data[5]),float(data[6]),float(data[7]),float(data[8])]) 81 | 82 | if(isFirstAll): 83 | position = position0 84 | orientation = orientation0 85 | isFirstAll = False 86 | else: 87 | position = np.c_[position, position0] 88 | orientation = np.c_[orientation, orientation0] 89 | 90 | 91 | #This method is called when message is arrived. 92 | def on_message(client, userdata, msg): 93 | global isFirst 94 | global time 95 | global accel, velocity, gravity, gyro, magnet, ori, position, orientation 96 | 97 | #print(msg.topic + ' ' + str(msg.payload)) 98 | 99 | data = str(msg.payload).split('&') 100 | #Append data to the array 101 | if(str(msg.topic) == "SLAM/output/accel"): 102 | appendData(data) 103 | if(str(msg.topic) == "SLAM/output/velocity"): 104 | appendDataVelocity(data) 105 | elif(str(msg.topic) == "SLAM/output/all"): 106 | appendDataAll(data) 107 | elif(str(msg.topic) == "SLAM/output/stop"): 108 | date = datetime.datetime.now() 109 | datestr = date.strftime("%Y%m%d_%H%M%S_") 110 | np.savetxt('./output/'+datestr+'accel.csv', accel, delimiter=',') 111 | np.savetxt('./output/'+datestr+'velocity.csv', velocity, delimiter=',') 112 | np.savetxt('./output/'+datestr+'position.csv', position, delimiter=',') 113 | np.savetxt('./output/'+datestr+'orientation.csv', orientation, delimiter=',') 114 | print("[GetOutputData] stop") 115 | init() 116 | 117 | 118 | #Main method 119 | if __name__ == '__main__': 120 | 121 | #global variables 122 | isFirst = True 123 | time = 0 124 | isFirstV = True 125 | timeV = 0 126 | isFirstAll = True 127 | timeAll = 0 128 | accel = np.array([]) 129 | velocity = np.array([]) 130 | gravity = np.array([]) 131 | gyro = np.array([]) 132 | magnet = np.array([]) 133 | ori = np.array([]) 134 | position = np.array([]) 135 | orientation = np.array([]) 136 | 137 | #Read server conf file 138 | f = open('../../server.conf', 'r') 139 | for line in f: 140 | serverconf = line 141 | f.close() 142 | 143 | #Mqtt 144 | conf = serverconf.split('&') 145 | host = conf[0] 146 | port = int(conf[1]) 147 | username = conf[2] 148 | password = conf[3] 149 | 150 | #Mqtt connect 151 | client = mqtt.Client(client_id="GetOutputData", clean_session=True, protocol=mqtt.MQTTv311) 152 | client.on_connect = on_connect 153 | client.on_message = on_message 154 | client.username_pw_set(username, password=password) 155 | client.connect(host, port=port, keepalive=60) 156 | client.loop_forever() 157 | -------------------------------------------------------------------------------- /data/GetOutputDataTemp.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetOutputDataTemp.py 5 | Subscribe output and save them as CSV file. 6 | """ 7 | 8 | import sys 9 | import numpy as np 10 | import paho.mqtt.client as mqtt 11 | 12 | def init(): 13 | global isFirst, time 14 | global output 15 | 16 | isFirst = True 17 | time = 0 18 | output = np.array([]) 19 | 20 | 21 | #This method is called when mqtt is connected. 22 | def on_connect(client, userdata, flags, rc): 23 | print('[GetOutputDataTemp] Connected with result code '+str(rc)) 24 | client.subscribe("SLAM/output/#") 25 | 26 | 27 | #Append new data array to the array. 28 | def appendData(data): 29 | global isFirst 30 | global time 31 | global output 32 | 33 | time = time + 1 34 | #output0 = np.array([time,float(data[0]),float(data[1]),float(data[2])]) 35 | output0 = np.array([time,float(data[0]),float(data[1]),float(data[2]),float(data[3]),float(data[4]),float(data[5])]) 36 | #output0 = np.array([time,float(data[0]),float(data[1]),float(data[2]),float(data[3]),float(data[4]),float(data[5]),float(data[6]),float(data[7]),float(data[8])]) 37 | 38 | if(isFirst): 39 | output = output0 40 | isFirst = False 41 | else: 42 | output = np.c_[output, output0] 43 | 44 | 45 | #This method is called when message is arrived. 46 | def on_message(client, userdata, msg): 47 | global isFirst 48 | global time 49 | global output 50 | 51 | #print(msg.topic + ' ' + str(msg.payload)) 52 | 53 | data = str(msg.payload).split('&') 54 | #Append data to the array 55 | if(str(msg.topic) == "SLAM/output/temp"): 56 | appendData(data) 57 | elif(str(msg.topic) == "SLAM/output/stop"): 58 | np.savetxt('./output/temp.csv', output, delimiter=',') 59 | print("[GetOutputDataTemp] stop") 60 | init() 61 | 62 | 63 | #Main method 64 | if __name__ == '__main__': 65 | 66 | #global variables 67 | isFirst = True 68 | time = 0 69 | output = np.array([]) 70 | 71 | #Read server conf file 72 | f = open('../../server.conf', 'r') 73 | for line in f: 74 | serverconf = line 75 | f.close() 76 | 77 | #Mqtt 78 | conf = serverconf.split('&') 79 | host = conf[0] 80 | port = int(conf[1]) 81 | username = conf[2] 82 | password = conf[3] 83 | 84 | #Mqtt connect 85 | client = mqtt.Client(client_id="GetOutputDataTemp", clean_session=True, protocol=mqtt.MQTTv311) 86 | client.on_connect = on_connect 87 | client.on_message = on_message 88 | client.username_pw_set(username, password=password) 89 | client.connect(host, port=port, keepalive=60) 90 | client.loop_forever() 91 | -------------------------------------------------------------------------------- /data/GetSensorData.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | GetSensorData.py 5 | Subscribe sensor data and save them as CSV file. 6 | """ 7 | 8 | import sys 9 | import numpy as np 10 | import paho.mqtt.client as mqtt 11 | import datetime 12 | 13 | 14 | def init(): 15 | global isFirst 16 | global time 17 | global accel, linear_accel, gravity, gyro, magnet, orientation 18 | 19 | #global variables 20 | isFirst = True 21 | time = 0 22 | accel = np.array([]) 23 | linear_accel = np.array([]) 24 | gravity = np.array([]) 25 | gyro = np.array([]) 26 | magnet = np.array([]) 27 | orientation = np.array([]) 28 | 29 | 30 | #This method is called when mqtt is connected. 31 | def on_connect(client, userdata, flags, rc): 32 | print('[GetSensorData] Connected with result code '+str(rc)) 33 | client.subscribe("SLAM/input/#") 34 | 35 | 36 | #Append new data array to the array. 37 | def appendData(data): 38 | global isFirst 39 | global time 40 | global accel, linear_accel, gravity, gyro, magnet, orientation 41 | 42 | if(isFirst): 43 | time = long(data[0]) 44 | 45 | #accel0 = np.array([long(data[0])-time,float(data[1]),float(data[2]),float(data[3])]) 46 | accel0 = np.array([long(data[0])-time,float(data[1]),float(data[2]),float(data[3]),float(data[4]),float(data[5]),float(data[6])]) 47 | 48 | if(isFirst): 49 | accel = accel0 50 | isFirst = False 51 | else: 52 | accel = np.c_[accel, accel0] 53 | 54 | #print(accel0) 55 | #print(np.array([float(data[4]),float(data[5]),float(data[6])])) 56 | 57 | 58 | #This method is called when message is arrived. 59 | def on_message(client, userdata, msg): 60 | global isFirst 61 | global time 62 | global accel, linear_accel, gravity, gyro, magnet, orientation 63 | 64 | #print(msg.topic + ' ' + str(msg.payload)) 65 | 66 | data = str(msg.payload).split('&') 67 | #Append data to the array 68 | if(str(msg.topic) == "SLAM/input/all"): 69 | appendData(data) 70 | elif(str(msg.topic) == "SLAM/input/stop"): 71 | date = datetime.datetime.now() 72 | datestr = date.strftime("%Y%m%d_%H%M%S_") 73 | np.savetxt('./input/'+datestr+'data.csv', accel, delimiter=',') 74 | print("[GetSensorData] stop") 75 | init() 76 | 77 | 78 | #Main method 79 | if __name__ == '__main__': 80 | 81 | #global variables 82 | isFirst = True 83 | time = 0 84 | accel = np.array([]) 85 | linear_accel = np.array([]) 86 | gravity = np.array([]) 87 | gyro = np.array([]) 88 | magnet = np.array([]) 89 | orientation = np.array([]) 90 | 91 | #Read server conf file 92 | f = open('../../server.conf', 'r') 93 | for line in f: 94 | serverconf = line 95 | f.close() 96 | 97 | #Mqtt 98 | conf = serverconf.split('&') 99 | host = conf[0] 100 | port = int(conf[1]) 101 | username = conf[2] 102 | password = conf[3] 103 | 104 | #Mqtt connect 105 | client = mqtt.Client(client_id="GetSensorData", clean_session=True, protocol=mqtt.MQTTv311) 106 | client.on_connect = on_connect 107 | client.on_message = on_message 108 | client.username_pw_set(username, password=password) 109 | client.connect(host, port=port, keepalive=60) 110 | client.loop_forever() 111 | -------------------------------------------------------------------------------- /data/_run_GetOutputData.cmd: -------------------------------------------------------------------------------- 1 | python GetOutputData.py -------------------------------------------------------------------------------- /data/_run_GetSensorData.cmd: -------------------------------------------------------------------------------- 1 | python GetSensorData.py -------------------------------------------------------------------------------- /data/input/IMUデータ間隔.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/input/IMUデータ間隔.xlsx -------------------------------------------------------------------------------- /data/input/plot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/input/plot.jpg -------------------------------------------------------------------------------- /data/output/lab04/20160114_141755_position - 観測モデルによる違い.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/lab04/20160114_141755_position - 観測モデルによる違い.xlsx -------------------------------------------------------------------------------- /data/output/lab04/inverse+共面条件/20160114_141755_position - 比較結果.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/lab04/inverse+共面条件/20160114_141755_position - 比較結果.xlsx -------------------------------------------------------------------------------- /data/output/lab04/inverse/20160116_151422_position - 比較結果.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/lab04/inverse/20160116_151422_position - 比較結果.xlsx -------------------------------------------------------------------------------- /data/output/lab04/共面条件/20160117_162704_position.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/lab04/共面条件/20160117_162704_position.xlsx -------------------------------------------------------------------------------- /data/output/lab04/部屋内実験_結果.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/lab04/部屋内実験_結果.xlsx -------------------------------------------------------------------------------- /data/output/lab04/部屋内実験_結果_精度評価.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/lab04/部屋内実験_結果_精度評価.xlsx -------------------------------------------------------------------------------- /data/output/passage02/20160114_221145_position - 観測モデルによる違い.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/20160114_221145_position - 観測モデルによる違い.xlsx -------------------------------------------------------------------------------- /data/output/passage02/inverse+共面条件/20160114_221145_position - 比較結果.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/inverse+共面条件/20160114_221145_position - 比較結果.xlsx -------------------------------------------------------------------------------- /data/output/passage02/inverse+共面条件/20160114_221145_position - 比較結果（17番まで）.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/inverse+共面条件/20160114_221145_position - 比較結果（17番まで）.xlsx -------------------------------------------------------------------------------- /data/output/passage02/inverse/20160115_224917_position - 比較結果.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/inverse/20160115_224917_position - 比較結果.xlsx -------------------------------------------------------------------------------- /data/output/passage02/inverse/20160115_224917_position - 比較結果（17番目まで）.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/inverse/20160115_224917_position - 比較結果（17番目まで）.xlsx -------------------------------------------------------------------------------- /data/output/passage02/共面条件/20160115_224917_position.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/共面条件/20160115_224917_position.xlsx -------------------------------------------------------------------------------- /data/output/passage02/建物内実験_結果.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/建物内実験_結果.xlsx -------------------------------------------------------------------------------- /data/output/passage02/建物内実験_結果_精度評価.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/passage02/建物内実験_結果_精度評価.xlsx -------------------------------------------------------------------------------- /data/output/実行時間の計測/IMUデータ観測時/IMUデータ観測時_カルマンフィルタ.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/実行時間の計測/IMUデータ観測時/IMUデータ観測時_カルマンフィルタ.xlsx -------------------------------------------------------------------------------- /data/output/実行時間の計測/IMUデータ観測時/IMUデータ観測時_パーティクルフィルタ.txt: -------------------------------------------------------------------------------- 1 | 0.001507 0.001665245 2 | 0.001315 3 | 0.001369 4 | 0.001368 5 | 0.00134 6 | 0.001545 7 | 0.001413 8 | 0.001352 9 | 0.00134 10 | 0.001278 11 | 0.001384 12 | 0.001316 13 | 0.001287 14 | 0.001297 15 | 0.001283 16 | 0.001579 17 | 0.001361 18 | 0.001389 19 | 0.001331 20 | 0.001335 21 | 0.001343 22 | 0.00128 23 | 0.0013 24 | 0.001285 25 | 0.001349 26 | 0.001299 27 | 0.001398 28 | 0.001457 29 | 0.001328 30 | 0.003203 31 | 0.003121 32 | 0.003161 33 | 0.00136 34 | 0.001322 35 | 0.001695 36 | 0.001352 37 | 0.001355 38 | 0.0013 39 | 0.001383 40 | 0.001356 41 | 0.001339 42 | 0.001391 43 | 0.001305 44 | 0.001373 45 | 0.001482 46 | 0.0016 47 | 0.001384 48 | 0.00135 49 | 0.001344 50 | 0.001378 51 | 0.002192 52 | 0.001314 53 | 0.001331 54 | 0.001634 55 | 0.001354 56 | 0.00135 57 | 0.001319 58 | 0.001327 59 | 0.001312 60 | 0.001352 61 | 0.001328 62 | 0.001503 63 | 0.001359 64 | 0.00134 65 | 0.001347 66 | 0.001356 67 | 0.001489 68 | 0.001327 69 | 0.001304 70 | 0.001318 71 | 0.001286 72 | 0.001316 73 | 0.001302 74 | 0.001315 75 | 0.001351 76 | 0.001303 77 | 0.00128 78 | 0.001299 79 | 0.001462 80 | 0.002507 81 | 0.001369 82 | 0.001653 83 | 0.001357 84 | 0.001355 85 | 0.001296 86 | 0.001392 87 | 0.001315 88 | 0.001343 89 | 0.001609 90 | 0.001347 91 | 0.001353 92 | 0.001326 93 | 0.001312 94 | 0.00134 95 | 0.001328 96 | 0.001382 97 | 0.001307 98 | 0.001296 99 | 0.001292 100 | 0.001297 101 | 0.001331 102 | 0.00131 103 | 0.001459 104 | 0.001355 105 | 0.001346 106 | 0.00131 107 | 0.00133 108 | 0.001315 109 | 0.001308 110 | 0.001288 111 | 0.00138 112 | 0.001373 113 | 0.001397 114 | 0.003267 115 | 0.003223 116 | 0.003221 117 | 0.00321 118 | 0.001322 119 | 0.001273 120 | 0.001337 121 | 0.003199 122 | 0.001345 123 | 0.001286 124 | 0.001353 125 | 0.003248 126 | 0.003213 127 | 0.003199 128 | 0.001362 129 | 0.001307 130 | 0.001302 131 | 0.0014 132 | 0.001319 133 | 0.001504 134 | 0.001691 135 | 0.001443 136 | 0.003227 137 | 0.003213 138 | 0.00321 139 | 0.00322 140 | 0.001305 141 | 0.001296 142 | 0.001396 143 | 0.001322 144 | 0.001303 145 | 0.00131 146 | 0.001356 147 | 0.001939 148 | 0.00138 149 | 0.001382 150 | 0.001323 151 | 0.00188 152 | 0.001772 153 | 0.001813 154 | 0.003201 155 | 0.003239 156 | 0.003279 157 | 0.001326 158 | 0.001283 159 | 0.00137 160 | 0.001476 161 | 0.00131 162 | 0.001296 163 | 0.001294 164 | 0.001298 165 | 0.001273 166 | 0.003229 167 | 0.003214 168 | 0.003212 169 | 0.003727 170 | 0.001669 171 | 0.001428 172 | 0.001376 173 | 0.001368 174 | 0.001338 175 | 0.001363 176 | 0.001626 177 | 0.003035 178 | 0.003213 179 | 0.003244 180 | 0.00285 181 | 0.002294 182 | 0.001517 183 | 0.001394 184 | 0.001628 185 | 0.00163 186 | 0.002369 187 | 0.001641 188 | 0.001384 189 | 0.003207 190 | 0.003399 191 | 0.001624 192 | 0.003201 193 | 0.001638 194 | 0.001562 195 | 0.002153 196 | 0.001628 197 | 0.001792 198 | 0.001807 199 | 0.003191 200 | 0.006739 201 | 0.003207 202 | 0.002625 203 | 0.001615 204 | 0.003201 205 | 0.002287 206 | 0.001629 207 | 0.001415 208 | 0.002411 209 | 0.001554 210 | 0.001622 211 | 0.001365 212 | 0.001561 213 | 0.001535 214 | 0.001408 215 | 0.001448 216 | 0.001472 217 | 0.001444 218 | 0.001464 219 | 0.001437 220 | 0.001414 221 | 0.001398 222 | 0.001447 223 | 0.001433 224 | 0.001418 225 | 0.003221 226 | 0.003209 227 | 0.00325 228 | 0.003239 229 | 0.00504 230 | 0.001328 231 | 0.001386 232 | 0.001379 233 | 0.001592 234 | 0.001334 235 | 0.001318 236 | 0.001485 237 | 0.001548 238 | 0.00321 239 | 0.002019 240 | 0.001591 241 | 0.001329 242 | 0.001297 243 | 0.001572 244 | 0.001345 245 | 0.001344 246 | 0.001471 247 | 0.003242 248 | 0.003366 249 | 0.001385 250 | 0.001348 251 | 0.001425 252 | 0.001358 253 | 0.003238 254 | 0.003244 255 | 0.001366 256 | 0.001503 257 | 0.00135 258 | 0.001334 259 | 0.001327 260 | 0.001326 261 | 0.001346 262 | 0.001385 263 | 0.001598 264 | 0.001353 265 | 0.001352 266 | 0.001679 267 | 0.003063 268 | 0.001375 269 | 0.001652 270 | 0.001443 271 | 0.001317 272 | 0.001308 273 | 0.001291 274 | 0.001458 275 | 0.003223 276 | 0.001375 277 | 0.00151 278 | 0.001364 279 | 0.00135 280 | 0.003237 281 | 0.003206 282 | 0.001758 283 | 0.001764 284 | 0.001773 285 | 0.003205 286 | 0.003212 287 | 0.001328 288 | 0.001423 289 | 0.003323 290 | 0.002157 291 | 0.002167 292 | 0.002162 293 | 0.002211 294 | 0.001339 295 | 0.001277 296 | 0.001313 297 | 0.003248 298 | 0.003562 299 | 0.001362 300 | 0.001315 301 | 0.001442 302 | 0.003201 303 | 0.003201 304 | 0.001334 305 | 0.001544 306 | 0.001448 307 | 0.003274 308 | 0.003218 309 | 0.003128 310 | 0.003137 311 | 0.001701 312 | 0.001329 313 | 0.001314 314 | 0.003239 315 | 0.003258 316 | 0.001333 317 | 0.001364 318 | 0.001314 319 | 0.001405 320 | 0.001362 321 | 0.001366 322 | 0.001321 323 | 0.001485 324 | 0.003232 325 | 0.003163 326 | 0.003195 327 | 0.001408 328 | 0.001373 329 | 0.00137 330 | 0.00131 331 | 0.001406 332 | 0.001346 333 | 0.00152 334 | 0.001369 335 | 0.001323 336 | 0.001315 337 | 0.001306 338 | 0.001307 339 | 0.001388 340 | 0.001422 341 | 0.001312 342 | 0.001455 343 | 0.002324 344 | 0.001402 345 | 0.001374 346 | 0.001329 347 | 0.001456 348 | 0.001396 349 | 0.00135 350 | 0.001288 351 | 0.001462 352 | 0.001346 353 | 0.001351 354 | 0.001332 355 | 0.001461 356 | 0.00157 357 | 0.00134 358 | 0.001312 359 | 0.001489 360 | 0.001462 361 | 0.001357 362 | 0.001349 363 | 0.001394 364 | 0.00135 365 | 0.001339 366 | 0.001333 367 | 0.001299 368 | 0.001293 369 | 0.001307 370 | 0.001401 371 | 0.001367 372 | 0.001399 373 | 0.001431 374 | 0.001444 375 | 0.001492 376 | 0.001403 377 | 0.001568 378 | 0.001342 379 | 0.001327 380 | 0.001308 381 | 0.001316 382 | 0.001389 383 | 0.001297 384 | 0.001598 385 | 0.001376 386 | 0.001395 387 | 0.00133 388 | 0.001338 389 | 0.001365 390 | 0.001496 391 | 0.001433 392 | 0.001416 393 | 0.00322 394 | 0.003217 395 | 0.001397 396 | 0.001357 397 | 0.001305 398 | 0.001278 399 | 0.001269 400 | 0.001354 401 | 0.00129 402 | 0.001343 403 | 0.001474 404 | 0.001338 405 | 0.001333 406 | 0.001341 407 | 0.001342 408 | 0.003203 409 | 0.001462 410 | 0.00134 411 | 0.001363 412 | 0.001431 413 | 0.002036 414 | 0.001358 415 | 0.001366 416 | 0.001338 417 | 0.001368 418 | 0.001368 419 | 0.001492 420 | 0.00161 421 | 0.001378 422 | 0.003373 423 | 0.001569 424 | 0.001558 425 | 0.00169 426 | 0.001785 427 | 0.001543 428 | 0.001485 429 | 0.001319 430 | 0.001277 431 | 0.001645 432 | 0.001368 433 | 0.001416 434 | 0.001346 435 | 0.001509 436 | 0.001442 437 | 0.001393 438 | 0.001363 439 | 0.00132 440 | 0.001346 441 | 0.001298 442 | 0.001461 443 | 0.001361 444 | 0.001318 445 | 0.001301 446 | 0.001321 447 | 0.001367 448 | 0.001387 449 | 0.001311 450 | 0.00133 451 | 0.001317 452 | 0.001313 453 | 0.001287 454 | 0.001372 455 | 0.001738 456 | 0.001389 457 | 0.001425 458 | 0.001303 459 | 0.001319 460 | 0.001308 461 | 0.001308 462 | 0.001605 463 | 0.001351 464 | 0.001308 465 | 0.001282 466 | 0.001276 467 | 0.001284 468 | 0.001294 469 | 0.001331 470 | 0.001697 471 | 0.001355 472 | 0.001369 473 | 0.001356 474 | 0.001368 475 | 0.001307 476 | 0.001417 477 | 0.001442 478 | 0.001341 479 | 0.001375 480 | 0.001322 481 | 0.001372 482 | 0.001338 483 | 0.001334 484 | 0.001458 485 | 0.001362 486 | 0.001346 487 | 0.001308 488 | 0.001368 489 | 0.001315 490 | 0.001355 491 | 0.001368 492 | 0.001343 493 | 0.001473 494 | 0.001345 495 | 0.001369 496 | 0.001486 497 | 0.001357 498 | 0.001345 499 | 0.00127 500 | 0.001311 501 | 0.00134 502 | 0.00172 503 | 0.001353 504 | 0.001371 505 | 0.001572 506 | 0.003212 507 | 0.003244 508 | 0.003211 509 | 0.003236 510 | 0.00132 511 | 0.001354 512 | 0.001327 513 | 0.001312 514 | 0.001311 515 | 0.001375 516 | 0.001389 517 | 0.00136 518 | 0.00136 519 | 0.001534 520 | 0.001371 521 | 0.001349 522 | 0.001308 523 | 0.001382 524 | 0.001285 525 | 0.001346 526 | 0.001455 527 | 0.001359 528 | 0.001344 529 | 0.001282 530 | 0.001458 531 | 0.00134 532 | 0.00132 533 | 0.001595 534 | 0.001351 535 | 0.001348 536 | 0.001313 537 | 0.001301 538 | 0.001317 539 | 0.001278 540 | 0.001286 541 | 0.001294 542 | 0.001378 543 | 0.001428 544 | 0.00139 545 | 0.001349 546 | 0.001353 547 | 0.001928 548 | 0.00131 549 | 0.001396 550 | 0.001336 551 | 0.001444 552 | 0.001397 553 | 0.001363 554 | 0.001369 555 | 0.001326 556 | 0.001447 557 | 0.001806 558 | 0.001357 559 | 0.001381 560 | 0.001408 561 | 0.001351 562 | 0.001431 563 | 0.001355 564 | 0.0014 565 | 0.001341 566 | 0.001373 567 | 0.001333 568 | 0.001365 569 | 0.001374 570 | 0.001468 571 | 0.001342 572 | 0.00134 573 | 0.001326 574 | 0.001376 575 | 0.001302 576 | 0.001296 577 | 0.001343 578 | 0.001341 579 | 0.00251 580 | 0.001589 581 | 0.001432 582 | 0.001377 583 | 0.001359 584 | 0.00131 585 | 0.001299 586 | 0.001457 587 | 0.001543 588 | 0.001521 589 | 0.001363 590 | 0.001352 591 | 0.001341 592 | 0.001344 593 | 0.001276 594 | 0.00137 595 | 0.001298 596 | 0.00129 597 | 0.001376 598 | 0.001329 599 | 0.001552 600 | 0.001323 601 | 0.001309 602 | 0.00131 603 | 0.001627 604 | 0.001371 605 | 0.001375 606 | 0.001556 607 | 0.001434 608 | 0.001356 609 | 0.001353 610 | 0.001305 611 | 0.001279 612 | 0.001435 613 | 0.001326 614 | 0.001356 615 | 0.00323 616 | 0.001506 617 | 0.001357 618 | 0.001338 619 | 0.001308 620 | 0.001307 621 | 0.00129 622 | 0.001308 623 | 0.001355 624 | 0.001302 625 | 0.001505 626 | 0.001366 627 | 0.001347 628 | 0.001374 629 | 0.003692 630 | 0.003229 631 | 0.003432 632 | 0.003226 633 | 0.001362 634 | 0.001291 635 | 0.001304 636 | 0.001721 637 | 0.001338 638 | 0.001337 639 | 0.001341 640 | 0.001681 641 | 0.001352 642 | 0.001341 643 | 0.001357 644 | 0.001446 645 | 0.001329 646 | 0.001347 647 | 0.001331 648 | 0.00131 649 | 0.001814 650 | 0.001357 651 | 0.001577 652 | 0.001354 653 | 0.001361 654 | 0.001476 655 | 0.001399 656 | 0.00134 657 | 0.001301 658 | 0.001346 659 | 0.001343 660 | 0.001365 661 | 0.001271 662 | 0.00143 663 | 0.001317 664 | 0.001293 665 | 0.00133 666 | 0.001333 667 | 0.001696 668 | 0.001361 669 | 0.001368 670 | 0.001328 671 | 0.001388 672 | 0.001352 673 | 0.001327 674 | 0.001489 675 | 0.001302 676 | 0.001441 677 | 0.003219 678 | 0.003223 679 | 0.003213 680 | 0.003226 681 | 0.00321 682 | 0.001298 683 | 0.001274 684 | 0.001312 685 | 0.001312 686 | 0.001285 687 | 0.001309 688 | 0.001321 689 | 0.00131 690 | 0.001305 691 | 0.001276 692 | 0.001346 693 | 0.001398 694 | 0.001361 695 | 0.001569 696 | 0.001365 697 | 0.001352 698 | 0.001356 699 | 0.001285 700 | 0.001394 701 | 0.001354 702 | 0.001321 703 | 0.001308 704 | 0.001345 705 | 0.001372 706 | 0.001308 707 | 0.001304 708 | 0.00128 709 | 0.001273 710 | 0.001326 711 | 0.001619 712 | 0.001414 713 | 0.001523 714 | 0.001334 715 | 0.001387 716 | 0.001317 717 | 0.001333 718 | 0.001351 719 | 0.001328 720 | 0.001527 721 | 0.001374 722 | 0.001334 723 | 0.001308 724 | 0.001353 725 | 0.001371 726 | 0.001336 727 | 0.001331 728 | 0.00132 729 | 0.001289 730 | 0.001416 731 | 0.001457 732 | 0.00135 733 | 0.001286 734 | 0.001303 735 | 0.001305 736 | 0.00141 737 | 0.001678 738 | 0.001649 739 | 0.003216 740 | 0.003826 741 | 0.001413 742 | 0.00132 743 | 0.001342 744 | 0.001342 745 | 0.001364 746 | 0.003224 747 | 0.00321 748 | 0.001607 749 | 0.001624 750 | 0.001625 751 | 0.001565 752 | 0.001562 753 | 0.001631 754 | 0.001555 755 | 0.001585 756 | 0.003207 757 | 0.003226 758 | 0.001355 759 | 0.001328 760 | 0.001414 761 | 0.001688 762 | 0.001605 763 | 0.001648 764 | 0.001407 765 | 0.001432 766 | 0.003226 767 | 0.001732 768 | 0.001319 769 | 0.001581 770 | 0.001375 771 | 0.00321 772 | 0.001362 773 | 0.001539 774 | 0.00135 775 | 0.001365 776 | 0.001383 777 | 0.001787 778 | 0.002903 779 | 0.001561 780 | 0.00175 781 | 0.001391 782 | 0.001329 783 | 0.001271 784 | 0.001577 785 | 0.001344 786 | 0.001356 787 | 0.001336 788 | 0.003093 789 | 0.00324 790 | 0.003234 791 | 0.003206 792 | 0.001388 793 | 0.001366 794 | 0.001337 795 | 0.001336 796 | 0.003205 797 | 0.0032 798 | 0.0032 799 | 0.003211 800 | 0.003185 801 | 0.003203 802 | 0.003201 803 | 0.002024 804 | 0.001691 805 | 0.001407 806 | 0.001413 807 | 0.001324 808 | 0.001347 809 | 0.001277 810 | 0.001295 811 | 0.001276 812 | 0.003046 813 | 0.003018 814 | 0.001422 815 | 0.001308 816 | 0.001414 817 | 0.001479 818 | 0.001359 819 | 0.001297 820 | 0.001341 821 | 0.001369 822 | 0.001441 823 | 0.003252 824 | 0.00325 825 | 0.003249 826 | 0.001325 827 | 0.001327 828 | 0.001273 829 | 0.001319 830 | 0.001301 831 | 0.001357 832 | 0.001305 833 | 0.001275 834 | 0.001307 835 | 0.001291 836 | 0.001372 837 | 0.001658 838 | 0.001348 839 | 0.001349 840 | 0.001275 841 | 0.001289 842 | 0.001329 843 | 0.001402 844 | 0.001669 845 | 0.00136 846 | 0.00135 847 | 0.001288 848 | 0.00131 849 | 0.001295 850 | 0.001397 851 | 0.001317 852 | 0.001394 853 | 0.001361 854 | 0.001305 855 | 0.001305 856 | 0.001272 857 | 0.001366 858 | 0.001281 859 | 0.001265 860 | 0.00134 861 | 0.001275 862 | 0.001574 863 | 0.001342 864 | 0.001407 865 | 0.00132 866 | 0.001387 867 | 0.001693 868 | 0.001409 869 | 0.001397 870 | 0.001354 871 | 0.001304 872 | 0.001329 873 | 0.001291 874 | 0.001304 875 | 0.001349 876 | 0.001295 877 | 0.0014 878 | 0.001424 879 | 0.001341 880 | 0.001337 881 | 0.001302 882 | 0.003229 883 | 0.003218 884 | 0.003113 885 | 0.003162 886 | 0.001301 887 | 0.00146 888 | 0.00133 889 | 0.001325 890 | 0.001557 891 | 0.001337 892 | 0.001315 893 | 0.00132 894 | 0.001305 895 | 0.00141 896 | 0.001395 897 | 0.00134 898 | 0.001331 899 | 0.001296 900 | 0.001291 901 | 0.003224 902 | 0.003242 903 | 0.00346 904 | 0.001829 905 | 0.001829 906 | 0.003239 907 | 0.001889 908 | 0.001891 909 | 0.001425 910 | 0.001759 911 | 0.001661 912 | 0.001567 913 | 0.001353 914 | 0.001424 915 | 0.001352 916 | 0.001323 917 | 0.001297 918 | 0.001316 919 | 0.001538 920 | 0.001574 921 | 0.001638 922 | 0.001607 923 | 0.001308 924 | 0.001382 925 | 0.001538 926 | 0.001365 927 | 0.001381 928 | 0.001303 929 | 0.001282 930 | 0.00136 931 | 0.001332 932 | 0.001378 933 | 0.001621 934 | 0.001385 935 | 0.001353 936 | 0.001344 937 | 0.001352 938 | 0.001526 939 | 0.001333 940 | 0.001305 941 | 0.001331 942 | 0.001429 943 | 0.001419 944 | 0.00135 945 | 0.001473 946 | 0.001386 947 | 0.003281 948 | 0.001304 949 | 0.001324 950 | 0.001684 951 | 0.001588 952 | 0.001357 953 | 0.00135 954 | 0.001322 955 | 0.001359 956 | 0.001389 957 | 0.001325 958 | 0.001604 959 | 0.00131 960 | 0.001507 961 | 0.001345 962 | 0.001388 963 | 0.001339 964 | 0.00138 965 | 0.001544 966 | 0.001425 967 | 0.001353 968 | 0.0013 969 | 0.002199 970 | 0.001279 971 | 0.001312 972 | 0.001472 973 | 0.001926 974 | 0.001372 975 | 0.00135 976 | 0.001325 977 | 0.001352 978 | 0.001287 979 | 0.00129 980 | 0.001316 981 | 0.001312 982 | 0.001346 983 | 0.001319 984 | 0.001317 985 | 0.001297 986 | 0.003283 987 | 0.003268 988 | 0.00334 989 | 0.003298 990 | 0.001414 991 | 0.003267 992 | 0.003233 993 | 0.003225 994 | 0.003276 995 | 0.001298 996 | 0.001303 997 | 0.001319 998 | 0.001555 999 | 0.001383 1000 | 0.001353 1001 | 0.001348 1002 | 0.003207 1003 | 0.003213 1004 | 0.001795 1005 | 0.001424 1006 | 0.001354 1007 | 0.001353 1008 | 0.001311 1009 | 0.001649 1010 | 0.001423 1011 | 0.001375 1012 | 0.001334 1013 | 0.001318 1014 | 0.001421 1015 | 0.001429 1016 | 0.001423 1017 | 0.001301 1018 | 0.001638 1019 | 0.002226 1020 | 0.001347 1021 | 0.001338 1022 | 0.001399 1023 | 0.001344 1024 | 0.001338 1025 | 0.001314 1026 | 0.001326 1027 | 0.001295 1028 | 0.001555 1029 | 0.001324 1030 | 0.001335 1031 | 0.00133 1032 | 0.001295 1033 | 0.003214 1034 | 0.003186 1035 | 0.001699 1036 | 0.001351 1037 | 0.001317 1038 | 0.001808 1039 | 0.003298 1040 | 0.001358 1041 | 0.001585 1042 | 0.001717 1043 | 0.00144 1044 | 0.001384 1045 | 0.00146 1046 | 0.001397 1047 | 0.003233 1048 | 0.003187 1049 | 0.001373 1050 | 0.001304 1051 | 0.00154 1052 | 0.001346 1053 | 0.001342 1054 | 0.001316 1055 | 0.001486 1056 | 0.001368 1057 | 0.001417 1058 | 0.003298 1059 | 0.001441 1060 | 0.00137 1061 | 0.001321 1062 | 0.001286 1063 | 0.001274 1064 | 0.001376 1065 | 0.001359 1066 | 0.001396 1067 | 0.001365 1068 | 0.001628 1069 | 0.001334 1070 | 0.001464 1071 | 0.001717 1072 | 0.001356 1073 | 0.001374 1074 | 0.001439 1075 | 0.001623 1076 | 0.001622 1077 | 0.003258 1078 | 0.003515 1079 | 0.001345 1080 | 0.001372 1081 | 0.00138 1082 | 0.00137 1083 | 0.001339 1084 | 0.001301 1085 | 0.001791 1086 | 0.001372 1087 | 0.001358 1088 | 0.001343 1089 | 0.001314 1090 | 0.001512 1091 | 0.001353 1092 | 0.001384 1093 | 0.00148 1094 | 0.001457 1095 | 0.003206 1096 | 0.003195 1097 | 0.003316 1098 | 0.003188 1099 | 0.001343 1100 | 0.001409 1101 | 0.00143 1102 | 0.001863 1103 | 0.002041 1104 | 0.001441 1105 | 0.001481 1106 | 0.001638 1107 | 0.001353 1108 | 0.001361 1109 | 0.001318 1110 | 0.001415 1111 | 0.001315 1112 | 0.001421 1113 | 0.001365 1114 | 0.001418 1115 | 0.001439 1116 | 0.003209 1117 | 0.003235 1118 | 0.003206 1119 | 0.001362 1120 | 0.003217 1121 | 0.001565 1122 | 0.001361 1123 | 0.001408 1124 | 0.001427 1125 | 0.001405 1126 | 0.001426 1127 | 0.001554 1128 | 0.001462 1129 | 0.001413 1130 | 0.001392 1131 | 0.001409 1132 | 0.001376 1133 | 0.001383 1134 | 0.001382 1135 | 0.001388 1136 | 0.001384 1137 | 0.001346 1138 | 0.001356 1139 | 0.001422 1140 | 0.001436 1141 | 0.001546 1142 | 0.001434 1143 | 0.001461 1144 | 0.00388 1145 | 0.003025 1146 | 0.003288 1147 | 0.003022 1148 | 0.001568 1149 | 0.003268 1150 | 0.001377 1151 | 0.001349 1152 | 0.001286 1153 | 0.001294 1154 | 0.001282 1155 | 0.001437 1156 | 0.003204 1157 | 0.003258 1158 | 0.001349 1159 | 0.001316 1160 | 0.001309 1161 | 0.001304 1162 | 0.0013 1163 | 0.001449 1164 | 0.001446 1165 | 0.001322 1166 | 0.0013 1167 | 0.001301 1168 | 0.001283 1169 | 0.001279 1170 | 0.001418 1171 | 0.003277 1172 | 0.001764 1173 | 0.001587 1174 | 0.001626 1175 | 0.001344 1176 | 0.001324 1177 | 0.001394 1178 | 0.001326 1179 | 0.001418 1180 | 0.001307 1181 | 0.001302 1182 | 0.003242 1183 | 0.001399 1184 | 0.001354 1185 | 0.001331 1186 | 0.001326 1187 | 0.001322 1188 | 0.001383 1189 | 0.001356 1190 | 0.001366 1191 | 0.001455 1192 | 0.001451 1193 | 0.001493 1194 | 0.001399 1195 | 0.001345 1196 | 0.001337 1197 | 0.001664 1198 | 0.001793 1199 | 0.001469 1200 | 0.001454 1201 | 0.001603 1202 | 0.001365 1203 | 0.001351 1204 | 0.001336 1205 | 0.001293 1206 | 0.001337 1207 | 0.001301 1208 | 0.001567 1209 | 0.003002 1210 | 0.002993 1211 | 0.002993 1212 | 0.003178 1213 | 0.003205 1214 | 0.001728 1215 | 0.001377 1216 | 0.003216 1217 | 0.003194 1218 | 0.003471 1219 | 0.003188 1220 | 0.001556 1221 | 0.003421 1222 | 0.003219 1223 | 0.003197 1224 | 0.003188 1225 | 0.001573 1226 | 0.001513 1227 | 0.001362 1228 | 0.001558 1229 | 0.001483 1230 | 0.002406 1231 | 0.001349 1232 | 0.001342 1233 | 0.001328 1234 | 0.001352 1235 | 0.001384 1236 | 0.00132 1237 | 0.001317 1238 | 0.00138 1239 | 0.001707 1240 | 0.001353 1241 | 0.001344 1242 | 0.001312 1243 | 0.001281 1244 | 0.003198 1245 | 0.001335 1246 | 0.001337 1247 | 0.001397 1248 | 0.001323 1249 | 0.001348 1250 | 0.00137 1251 | 0.001547 1252 | 0.001369 1253 | 0.001305 1254 | 0.001335 1255 | 0.001495 1256 | 0.001355 1257 | 0.001345 1258 | 0.001321 1259 | 0.001397 1260 | 0.001331 1261 | 0.001309 1262 | 0.001334 1263 | 0.001327 1264 | 0.00132 1265 | 0.001343 1266 | 0.001327 1267 | 0.001324 1268 | 0.001339 1269 | 0.001534 1270 | 0.003236 1271 | 0.003282 1272 | 0.002815 1273 | 0.002757 1274 | 0.002659 1275 | 0.002231 1276 | 0.001299 1277 | 0.001375 1278 | 0.001298 1279 | 0.001385 1280 | 0.001381 1281 | 0.002124 1282 | 0.001419 1283 | 0.001334 1284 | 0.001966 1285 | 0.001359 1286 | 0.001348 1287 | 0.001279 1288 | 0.00136 1289 | 0.001332 1290 | 0.00136 1291 | 0.001824 1292 | 0.001343 1293 | 0.001338 1294 | 0.001323 1295 | 0.00173 1296 | 0.003545 1297 | 0.001407 1298 | 0.003235 1299 | 0.00324 1300 | 0.003258 1301 | 0.003221 1302 | 0.003256 1303 | 0.003226 1304 | 0.00146 1305 | 0.001607 1306 | 0.001563 1307 | 0.001749 1308 | 0.001594 1309 | 0.001561 1310 | 0.00156 1311 | 0.001568 1312 | 0.001643 1313 | 0.001346 1314 | 0.001308 1315 | 0.001327 1316 | 0.001389 1317 | 0.00134 1318 | 0.001364 1319 | 0.003246 1320 | 0.001309 1321 | 0.001392 1322 | 0.001362 1323 | 0.001314 1324 | 0.001319 1325 | 0.001271 1326 | 0.001441 1327 | 0.001681 1328 | 0.001564 1329 | 0.001568 1330 | 0.001588 1331 | 0.001575 1332 | 0.001576 1333 | 0.001563 1334 | 0.001584 1335 | 0.001594 1336 | 0.001574 1337 | 0.001564 1338 | 0.001561 1339 | 0.001991 1340 | 0.0016 1341 | 0.001573 1342 | 0.001628 1343 | 0.001391 1344 | 0.001346 1345 | 0.001268 1346 | 0.001303 1347 | 0.001278 1348 | 0.001404 1349 | 0.00158 1350 | 0.001638 1351 | 0.001415 1352 | 0.002962 1353 | 0.001378 1354 | 0.001384 1355 | 0.001663 1356 | 0.001426 1357 | 0.001363 1358 | 0.001324 1359 | 0.001324 1360 | 0.001318 1361 | 0.001301 1362 | 0.001316 1363 | 0.001315 1364 | 0.001317 1365 | 0.001334 1366 | 0.003329 1367 | 0.003119 1368 | 0.003144 1369 | 0.003262 1370 | 0.001588 1371 | 0.001435 1372 | 0.001351 1373 | 0.001329 1374 | 0.001335 1375 | 0.001336 1376 | 0.00136 1377 | 0.003251 1378 | 0.003277 1379 | 0.001306 1380 | 0.001313 1381 | 0.001308 1382 | 0.001417 1383 | 0.001311 1384 | 0.001308 1385 | 0.001321 1386 | 0.001369 1387 | 0.001348 1388 | 0.001358 1389 | 0.001447 1390 | 0.001343 1391 | 0.001807 1392 | 0.003058 1393 | 0.00132 1394 | 0.001668 1395 | 0.001353 1396 | 0.001453 1397 | 0.001369 1398 | 0.001353 1399 | 0.001403 1400 | 0.001432 1401 | 0.001392 1402 | 0.00139 1403 | 0.001413 1404 | 0.001344 1405 | 0.001344 1406 | 0.001288 1407 | 0.001543 1408 | 0.001323 1409 | 0.001341 1410 | 0.001384 1411 | 0.001417 1412 | 0.003128 1413 | 0.001861 1414 | 0.001306 1415 | 0.001295 1416 | 0.001396 1417 | 0.00137 1418 | 0.001324 1419 | 0.00144 1420 | 0.001344 1421 | 0.001354 1422 | 0.001342 1423 | 0.001346 1424 | 0.00134 1425 | 0.001384 1426 | 0.001341 1427 | 0.001324 1428 | 0.00131 1429 | 0.00142 1430 | 0.001416 1431 | 0.001369 1432 | 0.001313 1433 | 0.001292 1434 | 0.001296 1435 | 0.0016 1436 | 0.001676 1437 | 0.001837 1438 | 0.001355 1439 | 0.001293 1440 | 0.001344 1441 | 0.001337 1442 | 0.004041 1443 | 0.003409 1444 | 0.003232 1445 | 0.003189 1446 | 0.001331 1447 | 0.00134 1448 | 0.001298 1449 | 0.00135 1450 | 0.001577 1451 | 0.001353 1452 | 0.001339 1453 | 0.001339 1454 | 0.001441 1455 | 0.001344 1456 | 0.00135 1457 | 0.001267 1458 | 0.003253 1459 | 0.001427 1460 | 0.001347 1461 | 0.001307 1462 | 0.001309 1463 | 0.001349 1464 | 0.001399 1465 | 0.001332 1466 | 0.001352 1467 | 0.001297 1468 | 0.001379 1469 | 0.001302 1470 | 0.001276 1471 | 0.001277 1472 | 0.001497 1473 | 0.001347 1474 | 0.001354 1475 | 0.001554 1476 | 0.001346 1477 | 0.001351 1478 | 0.001406 1479 | 0.001369 1480 | 0.001351 1481 | 0.001314 1482 | 0.00151 1483 | 0.001391 1484 | 0.001408 1485 | 0.001371 1486 | 0.001359 1487 | 0.001347 1488 | 0.001333 1489 | 0.001343 1490 | 0.001306 1491 | 0.001574 1492 | 0.001368 1493 | 0.00137 1494 | 0.001337 1495 | 0.001281 1496 | 0.001288 1497 | 0.001293 1498 | 0.001289 1499 | 0.003297 1500 | 0.003225 1501 | 0.003267 1502 | 0.003194 1503 | 0.003209 1504 | 0.003296 1505 | 0.003129 1506 | 0.003244 1507 | 0.00323 1508 | 0.001307 1509 | 0.001473 1510 | 0.001405 1511 | 0.001595 1512 | 0.001439 1513 | 0.001395 1514 | 0.001389 1515 | 0.003431 1516 | 0.003194 1517 | 0.001312 1518 | 0.003428 1519 | 0.00132 1520 | 0.001314 1521 | 0.001315 1522 | 0.001281 1523 | 0.00129 1524 | 0.001311 1525 | 0.001503 1526 | 0.001449 1527 | 0.001329 1528 | 0.001311 1529 | 0.001307 1530 | 0.001526 1531 | 0.001306 1532 | 0.00143 1533 | 0.001319 1534 | 0.001318 1535 | 0.001296 1536 | 0.001271 1537 | 0.001327 1538 | 0.001316 1539 | 0.001376 1540 | 0.001315 1541 | 0.001302 1542 | 0.001299 1543 | 0.001321 1544 | 0.001473 1545 | 0.001356 1546 | 0.003245 1547 | 0.001342 1548 | 0.001322 1549 | 0.001267 1550 | 0.001682 1551 | 0.001352 1552 | 0.001381 1553 | 0.003196 1554 | 0.003214 1555 | 0.001405 1556 | 0.003264 1557 | 0.001682 1558 | 0.003323 1559 | 0.003282 1560 | 0.001321 1561 | 0.001363 1562 | 0.001305 1563 | 0.001301 1564 | 0.001604 1565 | 0.003502 1566 | 0.001551 1567 | 0.001548 1568 | 0.003255 1569 | 0.003229 1570 | 0.001364 1571 | 0.001344 1572 | 0.001321 1573 | 0.001291 1574 | 0.001356 1575 | 0.001354 1576 | 0.001316 1577 | 0.001372 1578 | 0.00135 1579 | 0.001304 1580 | 0.001278 1581 | 0.001427 1582 | 0.001288 1583 | 0.001433 1584 | 0.003217 1585 | 0.003307 1586 | 0.003225 1587 | 0.001328 1588 | 0.001427 1589 | 0.003022 1590 | 0.003225 1591 | 0.001447 1592 | 0.001992 1593 | 0.003213 1594 | 0.001389 1595 | 0.001589 1596 | 0.001449 1597 | 0.003214 1598 | 0.001388 1599 | 0.001341 1600 | 0.001339 1601 | 0.001351 1602 | 0.001331 1603 | 0.001316 1604 | 0.001297 1605 | 0.001318 1606 | 0.001438 1607 | 0.001344 1608 | 0.001331 1609 | 0.001324 1610 | 0.001298 1611 | 0.001391 1612 | 0.001379 1613 | 0.00129 1614 | 0.001432 1615 | 0.001287 1616 | 0.001306 1617 | 0.003199 1618 | 0.001362 1619 | 0.00134 1620 | 0.001326 1621 | 0.001316 1622 | 0.00134 1623 | 0.001278 1624 | 0.001331 1625 | 0.001405 1626 | 0.001315 1627 | 0.001316 1628 | 0.0032 1629 | 0.001315 1630 | 0.001306 1631 | 0.001484 1632 | 0.001351 1633 | 0.001334 1634 | 0.003441 1635 | 0.003337 1636 | 0.001388 1637 | 0.001309 1638 | 0.00129 1639 | 0.003175 1640 | 0.003057 1641 | 0.003078 1642 | 0.003244 1643 | 0.001389 1644 | 0.001484 1645 | 0.00136 1646 | 0.001343 1647 | 0.001304 1648 | 0.001326 1649 | 0.001487 1650 | 0.00133 1651 | 0.001421 1652 | 0.00158 1653 | 0.001352 1654 | 0.001347 1655 | 0.001271 1656 | 0.001483 1657 | 0.001387 1658 | 0.001334 1659 | 0.001339 1660 | 0.001968 1661 | 0.001546 1662 | 0.001348 1663 | 0.001426 1664 | 0.001357 1665 | 0.001318 1666 | 0.003197 1667 | 0.001419 1668 | 0.001434 1669 | 0.00135 1670 | 0.00143 1671 | 0.003338 1672 | 0.003223 1673 | 0.003208 1674 | 0.003207 1675 | 0.001412 1676 | 0.001362 1677 | 0.001362 1678 | 0.001453 1679 | 0.001429 1680 | 0.00133 1681 | 0.001305 1682 | 0.001409 1683 | 0.003209 1684 | 0.003226 1685 | 0.003207 1686 | 0.001346 1687 | 0.001321 1688 | 0.001277 1689 | 0.001825 1690 | 0.001347 1691 | 0.001342 1692 | 0.001321 1693 | 0.003219 1694 | 0.003239 1695 | 0.003279 1696 | 0.001381 1697 | 0.001303 1698 | 0.001284 1699 | 0.001313 1700 | 0.001299 1701 | 0.001358 1702 | 0.001796 1703 | 0.001574 1704 | 0.003253 1705 | 0.001291 1706 | 0.001447 1707 | 0.001342 1708 | 0.001342 1709 | 0.001269 1710 | 0.003255 1711 | 0.001321 1712 | 0.001447 1713 | 0.001567 1714 | 0.001372 1715 | 0.001348 1716 | 0.001327 1717 | 0.001297 1718 | 0.003223 1719 | 0.003215 1720 | 0.001367 1721 | 0.001306 1722 | 0.001316 1723 | 0.001316 1724 | 0.001309 1725 | 0.001337 1726 | 0.001372 1727 | 0.001435 1728 | 0.001303 1729 | 0.001371 1730 | 0.001311 1731 | 0.001307 1732 | 0.001302 1733 | 0.001773 1734 | 0.001568 1735 | 0.001354 1736 | 0.001354 1737 | 0.001434 1738 | 0.001331 1739 | 0.001283 1740 | 0.001288 1741 | 0.001418 1742 | 0.001375 1743 | 0.001355 1744 | 0.001354 1745 | 0.001397 1746 | 0.001309 1747 | -------------------------------------------------------------------------------- /data/output/実行時間の計測/IMUデータ観測時/IMUデータ観測時_パーティクルフィルタ.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/実行時間の計測/IMUデータ観測時/IMUデータ観測時_パーティクルフィルタ.xlsx -------------------------------------------------------------------------------- /data/output/実行時間の計測/画像データ更新時/InverseDepth+共面条件.txt: -------------------------------------------------------------------------------- 1 | 0.190997 0.455959247 2 | 0.824116 3 | 0.667796 4 | 0.797261 5 | 0.790956 6 | 0.967615 7 | 0.723112 8 | 0.754083 9 | 0.806397 10 | 0.839254 11 | 0.836598 12 | 0.81777 13 | 0.807955 14 | 0.831908 15 | 0.625513 16 | 0.592859 17 | 0.261915 18 | 0.299498 19 | 0.335389 20 | 0.522875 21 | 0.594958 22 | 0.405795 23 | 0.436934 24 | 0.539161 25 | 0.474753 26 | 0.436094 27 | 0.27639 28 | 0.068564 29 | 0.041654 30 | 0.039703 31 | 0.099864 32 | 0.183529 33 | 0.568763 34 | 0.282584 35 | 0.31791 36 | 0.677051 37 | 0.428951 38 | 0.583428 39 | 0.354663 40 | 0.438565 41 | 0.27175 42 | 0.394946 43 | 0.222748 44 | 0.301206 45 | 0.267665 46 | 0.25171 47 | 0.235227 48 | 0.369674 49 | 0.492601 50 | 0.547931 51 | 0.736422 52 | 0.714134 53 | 0.643214 54 | 0.683769 55 | 0.311344 56 | 0.335465 57 | 0.915389 58 | 0.751229 59 | 0.670239 60 | 0.550357 61 | 0.65124 62 | 0.586329 63 | 0.700825 64 | 0.680087 65 | 0.690594 66 | 0.682913 67 | 0.763499 68 | 0.859816 69 | 0.901647 70 | 0.780992 71 | 0.570259 72 | 0.689718 73 | 0.69018 74 | 0.543325 75 | 0.79682 76 | 0.967434 77 | 0.80838 78 | 0.632365 79 | 0.675616 80 | 0.894417 81 | 0.798422 82 | 0.851013 83 | 0.647322 84 | 0.610368 85 | 0.835777 86 | 0.443822 87 | 0.375597 88 | 0.836227 89 | 0.750171 90 | 0.566915 91 | 0.793027 92 | 0.800036 93 | 0.603348 94 | 0.389394 95 | 0.388526 96 | 0.840641 97 | 0.581172 98 | 0.62702 99 | 0.527405 100 | 0.573748 101 | 0.557441 102 | 0.441116 103 | 0.651097 104 | 0.504296 105 | 0.710749 106 | 0.783723 107 | 0.742184 108 | 0.670885 109 | 0.746841 110 | 0.362061 111 | 0.483629 112 | 0.303724 113 | 0.226243 114 | 0.549058 115 | 0.479369 116 | 0.397913 117 | 0.531386 118 | 0.795882 119 | 0.42961 120 | 0.631297 121 | 0.330149 122 | 0.500205 123 | 0.304772 124 | 0.661036 125 | 0.799007 126 | 0.646041 127 | 0.691275 128 | 0.806738 129 | 0.670334 130 | 0.699195 131 | 0.680794 132 | 0.583102 133 | 0.759314 134 | 0.807607 135 | 0.596459 136 | 0.724331 137 | 0.979068 138 | 0.880276 139 | 0.754278 140 | 0.335072 141 | 0.659019 142 | 0.61889 143 | 0.656717 144 | 0.724731 145 | 0.788726 146 | 0.567024 147 | 0.333277 148 | 0.261561 149 | 0.659798 150 | 0.588373 151 | 0.593065 152 | 0.545021 153 | 0.446454 154 | 0.145745 155 | 0.130883 156 | 0.712418 157 | 0.668589 158 | 0.521196 159 | 0.436868 160 | 0.716748 161 | 0.501739 162 | 0.195372 163 | 0.253688 164 | 0.549262 165 | 0.31327 166 | 0.084962 167 | 0.395781 168 | 0.540938 169 | 0.497962 170 | 0.540612 171 | 0.304088 172 | 0.403936 173 | 0.465417 174 | 0.478757 175 | 0.520488 176 | 0.601209 177 | 0.253003 178 | 0.13121 179 | 0.526862 180 | 0.632869 181 | 0.759032 182 | 0.69221 183 | 0.654767 184 | 0.526755 185 | 0.614688 186 | 0.637373 187 | 0.684678 188 | 0.632011 189 | 0.754197 190 | 0.645176 191 | 0.550796 192 | 0.704309 193 | 0.728385 194 | 0.717468 195 | 0.792103 196 | 0.761755 197 | 0.805607 198 | 0.722344 199 | 0.763928 200 | 0.619651 201 | 0.583641 202 | 0.574428 203 | 0.637788 204 | 0.523002 205 | 0.761688 206 | 0.482306 207 | 0.844827 208 | 0.380224 209 | 0.455923 210 | 0.502825 211 | 0.499173 212 | 0.565601 213 | 0.68931 214 | 0.662047 215 | 0.703014 216 | 0.608033 217 | 0.599257 218 | 0.58174 219 | 0.169444 220 | 0.175049 221 | 1.001415 222 | 0.753655 223 | 0.582062 224 | 0.602809 225 | 0.547866 226 | 0.385877 227 | 0.704072 228 | 0.464975 229 | 0.216542 230 | 0.230983 231 | 0.274587 232 | 0.435307 233 | 0.582425 234 | 0.563274 235 | 0.570467 236 | 0.557123 237 | 0.502408 238 | 0.464763 239 | 0.301719 240 | 0.685123 241 | 0.556841 242 | 0.354013 243 | 0.328136 244 | 0.21507 245 | 0.29449 246 | 0.429664 247 | 0.262293 248 | 0.694875 249 | 0.82052 250 | 0.616052 251 | 0.548252 252 | 0.413456 253 | 0.391211 254 | 0.741052 255 | 0.74607 256 | 0.636132 257 | 0.742515 258 | 0.441305 259 | 0.424627 260 | 0.591905 261 | 0.59371 262 | 0.584307 263 | 0.560492 264 | 0.438236 265 | 0.411553 266 | 0.483363 267 | 0.762463 268 | 0.579029 269 | 0.229691 270 | 0.206694 271 | 0.163938 272 | 0.453858 273 | 0.276622 274 | 0.671092 275 | 0.640992 276 | 0.714606 277 | 0.676088 278 | 0.494657 279 | 0.752027 280 | 0.496776 281 | 0.31182 282 | 0.479035 283 | 0.929398 284 | 0.903183 285 | 0.560799 286 | 0.558042 287 | 0.824793 288 | 0.792427 289 | 0.799912 290 | 0.822447 291 | 0.707481 292 | 0.348959 293 | 0.804626 294 | 0.320905 295 | 0.459953 296 | 0.900841 297 | 0.733813 298 | 0.63969 299 | 0.675778 300 | 0.654231 301 | 0.67005 302 | 0.599522 303 | 0.812462 304 | 0.506333 305 | 0.356174 306 | 0.78035 307 | 0.762256 308 | 0.703773 309 | 0.730235 310 | 0.607507 311 | 0.640314 312 | 0.710121 313 | 0.545671 314 | 0.344575 315 | 0.337869 316 | 0.196527 317 | 0.382138 318 | 0.889517 319 | 0.634836 320 | 0.89739 321 | 0.62089 322 | 0.58605 323 | 0.428022 324 | 0.699983 325 | 0.761635 326 | 0.686499 327 | 0.758493 328 | 0.794856 329 | 0.654248 330 | 0.587683 331 | 0.325275 332 | 0.116682 333 | 0.412059 334 | 0.486718 335 | 0.266463 336 | 0.682569 337 | 0.570025 338 | 0.462918 339 | 0.70145 340 | 0.67348 341 | 0.538429 342 | 0.691311 343 | 0.504928 344 | 0.584709 345 | 0.548618 346 | 0.583005 347 | 0.616871 348 | 0.709714 349 | 0.495278 350 | 0.583468 351 | 0.55652 352 | 0.558216 353 | 0.676544 354 | 0.662891 355 | 0.491791 356 | 0.513882 357 | 0.252726 358 | 0.459732 359 | 0.199183 360 | 0.448941 361 | 0.208376 362 | 0.043973 363 | 0.047199 364 | 0.021731 365 | 0.098906 366 | 0.114616 367 | 0.108106 368 | 0.066102 369 | 0.042709 370 | 0.046746 371 | 0.061142 372 | 0.100976 373 | 0.179018 374 | 0.107809 375 | 0.040209 376 | 0.024554 377 | 0.043195 378 | 0.051463 379 | 0.084135 380 | 0.14096 381 | 0.045274 382 | 0.338104 383 | 0.23255 384 | 0.095515 385 | 0.130465 386 | 0.131128 387 | 0.342331 388 | 0.252339 389 | 0.283967 390 | 0.175182 391 | 0.122732 392 | 0.429313 393 | 0.412725 394 | 0.286668 395 | 0.214674 396 | 0.685989 397 | 0.613661 398 | 0.222967 399 | 0.172414 400 | 0.13936 401 | 0.123365 402 | 0.10558 403 | 0.189208 404 | 0.307477 405 | 0.670198 406 | 0.947232 407 | 0.467453 408 | 0.386745 409 | 0.296824 410 | 0.431266 411 | 0.57776 412 | 0.185428 413 | 0.158856 414 | 0.157118 415 | 0.093545 416 | 0.143567 417 | 0.210188 418 | 0.181033 419 | 0.084392 420 | 0.150478 421 | 0.272466 422 | 0.179879 423 | 0.323099 424 | 0.308268 425 | 0.313309 426 | 0.517868 427 | 0.608733 428 | 0.650874 429 | 0.670307 430 | 0.523841 431 | 0.599256 432 | 0.381624 433 | 0.543667 434 | 0.245425 435 | 0.611802 436 | 0.42268 437 | 0.500924 438 | 0.399554 439 | 0.393382 440 | 0.382413 441 | 0.352494 442 | 0.27271 443 | 0.23579 444 | 0.074148 445 | 0.248853 446 | 0.032178 447 | 0.034129 448 | 0.066928 449 | 0.025798 450 | 0.049717 451 | 0.079078 452 | 0.14487 453 | 0.085216 454 | 0.032414 455 | 0.066868 456 | 0.10051 457 | 0.593708 458 | 0.491767 459 | 0.407082 460 | 0.255216 461 | 0.079551 462 | 0.038101 463 | 0.063679 464 | 0.248062 465 | 0.070714 466 | 0.084344 467 | 0.027971 468 | 0.304489 469 | 0.15242 470 | 0.474175 471 | 0.126382 472 | 0.119642 473 | 0.17661 474 | 0.080611 475 | 0.115727 476 | 0.658242 477 | 0.139073 478 | 0.108027 479 | 0.058754 480 | 0.074039 481 | 0.059318 482 | 0.066701 483 | 0.052445 484 | 0.081441 485 | 0.039773 486 | 0.052004 487 | 0.126682 488 | 0.13066 489 | 0.061334 490 | 0.198637 491 | 0.054972 492 | 0.057376 493 | 0.220762 494 | 0.10306 495 | 0.124004 496 | 0.231456 497 | 0.075602 498 | 0.0808 499 | 0.03794 500 | 0.050285 501 | 0.083028 502 | 0.06646 503 | 0.076032 504 | 0.084254 505 | 0.0919 506 | 0.394086 507 | 0.345968 508 | 0.199212 509 | 0.118501 510 | 0.059284 511 | 0.087509 512 | 0.067483 513 | 0.047096 514 | 0.072107 515 | 0.041141 516 | 0.075551 517 | 0.032715 518 | 0.415864 519 | 0.141073 520 | -------------------------------------------------------------------------------- /data/output/実行時間の計測/画像データ更新時/InverseDepth+共面条件.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/実行時間の計測/画像データ更新時/InverseDepth+共面条件.xlsx -------------------------------------------------------------------------------- /data/output/実行時間の計測/画像データ更新時/InverseDepth.txt: -------------------------------------------------------------------------------- 1 | 0.187279 0.318466655 2 | 0.552802 3 | 0.509544 4 | 0.574899 5 | 0.562185 6 | 0.674001 7 | 0.611535 8 | 0.497478 9 | 0.453999 10 | 0.58754 11 | 0.691959 12 | 0.583348 13 | 0.589754 14 | 0.604151 15 | 0.453641 16 | 0.429524 17 | 0.179115 18 | 0.194528 19 | 0.232509 20 | 0.349591 21 | 0.411994 22 | 0.282574 23 | 0.317857 24 | 0.419058 25 | 0.339652 26 | 0.304563 27 | 0.187952 28 | 0.054268 29 | 0.029064 30 | 0.03655 31 | 0.063677 32 | 0.154135 33 | 0.377768 34 | 0.19543 35 | 0.211515 36 | 0.458997 37 | 0.290926 38 | 0.4185 39 | 0.245744 40 | 0.298687 41 | 0.191069 42 | 0.28653 43 | 0.187609 44 | 0.219594 45 | 0.202435 46 | 0.195046 47 | 0.187866 48 | 0.260849 49 | 0.338905 50 | 0.376844 51 | 0.508254 52 | 0.493493 53 | 0.473877 54 | 0.480682 55 | 0.223157 56 | 0.221262 57 | 0.690777 58 | 0.532137 59 | 0.481122 60 | 0.403481 61 | 0.437556 62 | 0.414067 63 | 0.484635 64 | 0.494773 65 | 0.489029 66 | 0.542127 67 | 0.576434 68 | 0.56551 69 | 0.615621 70 | 0.514405 71 | 0.375141 72 | 0.531186 73 | 0.507069 74 | 0.417698 75 | 0.581012 76 | 0.68278 77 | 0.541462 78 | 0.424779 79 | 0.451052 80 | 0.541498 81 | 0.478923 82 | 0.522412 83 | 0.463124 84 | 0.428945 85 | 0.596418 86 | 0.330391 87 | 0.266974 88 | 0.592007 89 | 0.516949 90 | 0.44027 91 | 0.526072 92 | 0.477736 93 | 0.39657 94 | 0.258736 95 | 0.307046 96 | 0.587492 97 | 0.390325 98 | 0.426194 99 | 0.414095 100 | 0.421681 101 | 0.380686 102 | 0.329688 103 | 0.432633 104 | 0.322508 105 | 0.535658 106 | 0.56202 107 | 0.50465 108 | 0.447853 109 | 0.532082 110 | 0.267831 111 | 0.343922 112 | 0.234781 113 | 0.19387 114 | 0.394665 115 | 0.377826 116 | 0.274704 117 | 0.321188 118 | 0.505071 119 | 0.244212 120 | 0.382279 121 | 0.272394 122 | 0.377337 123 | 0.255794 124 | 0.485203 125 | 0.556885 126 | 0.467182 127 | 0.493227 128 | 0.565485 129 | 0.467389 130 | 0.477027 131 | 0.471152 132 | 0.434155 133 | 0.584741 134 | 0.529027 135 | 0.389602 136 | 0.526212 137 | 0.658957 138 | 0.500883 139 | 0.45324 140 | 0.219704 141 | 0.446972 142 | 0.45633 143 | 0.43499 144 | 0.465179 145 | 0.587881 146 | 0.348196 147 | 0.224396 148 | 0.206395 149 | 0.522405 150 | 0.422074 151 | 0.377003 152 | 0.383019 153 | 0.295192 154 | 0.103975 155 | 0.101825 156 | 0.520019 157 | 0.453008 158 | 0.387498 159 | 0.28693 160 | 0.524897 161 | 0.364573 162 | 0.147437 163 | 0.193715 164 | 0.429012 165 | 0.214669 166 | 0.070624 167 | 0.248056 168 | 0.346744 169 | 0.357326 170 | 0.371435 171 | 0.208535 172 | 0.286607 173 | 0.341895 174 | 0.333906 175 | 0.362446 176 | 0.398442 177 | 0.211074 178 | 0.099028 179 | 0.378364 180 | 0.449343 181 | 0.580845 182 | 0.53984 183 | 0.419411 184 | 0.33667 185 | 0.406645 186 | 0.43916 187 | 0.481159 188 | 0.445247 189 | 0.543694 190 | 0.502504 191 | 0.337587 192 | 0.397241 193 | 0.458194 194 | 0.383196 195 | 0.42885 196 | 0.452786 197 | 0.483918 198 | 0.423971 199 | 0.452991 200 | 0.365711 201 | 0.368882 202 | 0.360996 203 | 0.371469 204 | 0.347108 205 | 0.469743 206 | 0.347847 207 | 0.601932 208 | 0.250208 209 | 0.323601 210 | 0.366983 211 | 0.354494 212 | 0.392564 213 | 0.478987 214 | 0.453843 215 | 0.481471 216 | 0.393493 217 | 0.412255 218 | 0.375861 219 | 0.09312 220 | 0.119115 221 | 0.668533 222 | 0.561618 223 | 0.41694 224 | 0.415671 225 | 0.360352 226 | 0.30823 227 | 0.539402 228 | 0.28266 229 | 0.141123 230 | 0.133205 231 | 0.195496 232 | 0.301579 233 | 0.388816 234 | 0.391074 235 | 0.389911 236 | 0.397236 237 | 0.372106 238 | 0.310383 239 | 0.221737 240 | 0.564033 241 | 0.412369 242 | 0.258646 243 | 0.226408 244 | 0.1462 245 | 0.219957 246 | 0.301397 247 | 0.189062 248 | 0.495846 249 | 0.545885 250 | 0.387392 251 | 0.3573 252 | 0.273663 253 | 0.308136 254 | 0.529506 255 | 0.499997 256 | 0.41754 257 | 0.524792 258 | 0.310354 259 | 0.293039 260 | 0.423383 261 | 0.395581 262 | 0.381098 263 | 0.363653 264 | 0.28358 265 | 0.329151 266 | 0.343018 267 | 0.551328 268 | 0.384077 269 | 0.148063 270 | 0.142783 271 | 0.125494 272 | 0.324487 273 | 0.191412 274 | 0.439057 275 | 0.445481 276 | 0.491904 277 | 0.441086 278 | 0.357331 279 | 0.525879 280 | 0.329887 281 | 0.203384 282 | 0.351829 283 | 0.618426 284 | 0.572568 285 | 0.310753 286 | 0.416905 287 | 0.557508 288 | 0.454263 289 | 0.53194 290 | 0.543036 291 | 0.479664 292 | 0.26494 293 | 0.542885 294 | 0.213136 295 | 0.328272 296 | 0.625006 297 | 0.497328 298 | 0.421244 299 | 0.461114 300 | 0.432304 301 | 0.502404 302 | 0.395473 303 | 0.573902 304 | 0.304277 305 | 0.269452 306 | 0.606679 307 | 0.490464 308 | 0.480231 309 | 0.496817 310 | 0.403703 311 | 0.440087 312 | 0.486488 313 | 0.358529 314 | 0.265455 315 | 0.237033 316 | 0.141891 317 | 0.28059 318 | 0.611999 319 | 0.436801 320 | 0.736357 321 | 0.429637 322 | 0.467323 323 | 0.335807 324 | 0.569885 325 | 0.605176 326 | 0.507225 327 | 0.569027 328 | 0.577263 329 | 0.483363 330 | 0.43335 331 | 0.215812 332 | 0.080902 333 | 0.325464 334 | 0.301259 335 | 0.223261 336 | 0.437547 337 | 0.317829 338 | 0.257731 339 | 0.507906 340 | 0.483886 341 | 0.381635 342 | 0.472178 343 | 0.351919 344 | 0.382499 345 | 0.421025 346 | 0.397003 347 | 0.43766 348 | 0.494664 349 | 0.372728 350 | 0.425408 351 | 0.391475 352 | 0.403639 353 | 0.467218 354 | 0.477378 355 | 0.340612 356 | 0.366179 357 | 0.177934 358 | 0.330525 359 | 0.14591 360 | 0.314122 361 | 0.173545 362 | 0.05575 363 | 0.047276 364 | 0.016669 365 | 0.079117 366 | 0.088944 367 | 0.083174 368 | 0.046448 369 | 0.038009 370 | 0.04861 371 | 0.04916 372 | 0.08802 373 | 0.138193 374 | 0.065309 375 | 0.042955 376 | 0.03665 377 | 0.028552 378 | 0.022929 379 | 0.072314 380 | 0.13313 381 | 0.014288 382 | 0.285174 383 | 0.149797 384 | 0.056043 385 | 0.09863 386 | 0.068592 387 | 0.266616 388 | 0.195519 389 | 0.202338 390 | 0.132725 391 | 0.095332 392 | 0.311357 393 | 0.296934 394 | 0.205855 395 | 0.150339 396 | 0.494113 397 | 0.433428 398 | 0.156398 399 | 0.116102 400 | 0.095322 401 | 0.084111 402 | 0.082128 403 | 0.1265 404 | 0.233049 405 | 0.47045 406 | 0.532401 407 | 0.337951 408 | 0.303463 409 | 0.198253 410 | 0.354594 411 | 0.387457 412 | 0.145276 413 | 0.092797 414 | 0.099759 415 | 0.071005 416 | 0.096435 417 | 0.158683 418 | 0.129847 419 | 0.060254 420 | 0.108689 421 | 0.197661 422 | 0.142374 423 | 0.229878 424 | 0.222883 425 | 0.213093 426 | 0.357041 427 | 0.432728 428 | 0.46308 429 | 0.474439 430 | 0.385179 431 | 0.420416 432 | 0.261849 433 | 0.415962 434 | 0.166916 435 | 0.424082 436 | 0.339517 437 | 0.341165 438 | 0.246046 439 | 0.292879 440 | 0.276293 441 | 0.252822 442 | 0.188956 443 | 0.16017 444 | 0.072191 445 | 0.100159 446 | 0.042799 447 | 0.041135 448 | 0.054061 449 | 0.031851 450 | 0.04346 451 | 0.041078 452 | 0.096759 453 | 0.045861 454 | 0.023089 455 | 0.051537 456 | 0.06415 457 | 0.437558 458 | 0.337114 459 | 0.287361 460 | 0.171868 461 | 0.030003 462 | 0.042553 463 | 0.060114 464 | 0.187044 465 | 0.072453 466 | 0.06218 467 | 0.042713 468 | 0.234678 469 | 0.11472 470 | 0.35051 471 | 0.101135 472 | 0.095461 473 | 0.113719 474 | 0.062662 475 | 0.096236 476 | 0.372363 477 | 0.101311 478 | 0.076956 479 | 0.045867 480 | 0.054706 481 | 0.062349 482 | 0.04346 483 | 0.059905 484 | 0.074317 485 | 0.032957 486 | 0.050731 487 | 0.105198 488 | 0.092953 489 | 0.048922 490 | 0.148798 491 | 0.044196 492 | 0.044227 493 | 0.163441 494 | 0.074251 495 | 0.094246 496 | 0.167025 497 | 0.058243 498 | 0.063601 499 | 0.03278 500 | 0.044359 501 | 0.062176 502 | 0.053986 503 | 0.061906 504 | 0.067785 505 | 0.06866 506 | 0.289057 507 | 0.234792 508 | 0.141931 509 | 0.085418 510 | 0.043251 511 | 0.0641 512 | 0.057732 513 | 0.046648 514 | 0.062602 515 | 0.032823 516 | 0.063946 517 | 0.042019 518 | 0.293068 519 | 0.104699 520 | -------------------------------------------------------------------------------- /data/output/実行時間の計測/画像データ更新時/InverseDepth.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/実行時間の計測/画像データ更新時/InverseDepth.xlsx -------------------------------------------------------------------------------- /data/output/実行時間の計測/画像データ更新時/共面条件.txt: -------------------------------------------------------------------------------- 1 | 0.183723 2 | 0.267615 3 | 0.217928 4 | 0.257948 5 | 0.246134 6 | 0.314413 7 | 0.266853 8 | 0.264154 9 | 0.267653 10 | 0.278397 11 | 0.279926 12 | 0.281923 13 | 0.263137 14 | 0.264894 15 | 0.212236 16 | 0.182618 17 | 0.090419 18 | 0.093655 19 | 0.115043 20 | 0.164668 21 | 0.181010 22 | 0.132197 23 | 0.142012 24 | 0.159896 25 | 0.159412 26 | 0.150852 27 | 0.088925 28 | 0.026433 29 | 0.024095 30 | 0.027938 31 | 0.038158 32 | 0.075969 33 | 0.167875 34 | 0.088890 35 | 0.149854 36 | 0.210379 37 | 0.172629 38 | 0.210073 39 | 0.137418 40 | 0.165479 41 | 0.115239 42 | 0.116016 43 | 0.072699 44 | 0.102429 45 | 0.092248 46 | 0.091959 47 | 0.083563 48 | 0.110912 49 | 0.171207 50 | 0.183713 51 | 0.247078 52 | 0.244541 53 | 0.213515 54 | 0.236675 55 | 0.122313 56 | 0.110697 57 | 0.253922 58 | 0.242250 59 | 0.216834 60 | 0.242774 61 | 0.211837 62 | 0.220802 63 | 0.234980 64 | 0.220437 65 | 0.227374 66 | 0.233783 67 | 0.264266 68 | 0.279931 69 | 0.304003 70 | 0.298380 71 | 0.204448 72 | 0.248530 73 | 0.236839 74 | 0.188636 75 | 0.238649 76 | 0.295678 77 | 0.285962 78 | 0.247710 79 | 0.261940 80 | 0.299900 81 | 0.270204 82 | 0.276976 83 | 0.224054 84 | 0.198574 85 | 0.280198 86 | 0.164531 87 | 0.139908 88 | 0.259515 89 | 0.244698 90 | 0.195893 91 | 0.243834 92 | 0.250755 93 | 0.227702 94 | 0.151624 95 | 0.137856 96 | 0.249576 97 | 0.180576 98 | 0.200121 99 | 0.190473 100 | 0.202515 101 | 0.182113 102 | 0.151265 103 | 0.210940 104 | 0.187256 105 | 0.251774 106 | 0.240846 107 | 0.240859 108 | 0.213629 109 | 0.239117 110 | 0.129692 111 | 0.148336 112 | 0.117036 113 | 0.091873 114 | 0.174959 115 | 0.165349 116 | 0.146796 117 | 0.156367 118 | 0.218810 119 | 0.125531 120 | 0.186392 121 | 0.115113 122 | 0.177158 123 | 0.121919 124 | 0.202889 125 | 0.235748 126 | 0.221666 127 | 0.223175 128 | 0.252299 129 | 0.227321 130 | 0.221524 131 | 0.224146 132 | 0.190616 133 | 0.258316 134 | 0.287450 135 | 0.212651 136 | 0.256054 137 | 0.308699 138 | 0.291777 139 | 0.259363 140 | 0.123293 141 | 0.211581 142 | 0.223136 143 | 0.302555 144 | 0.256967 145 | 0.281175 146 | 0.195200 147 | 0.113575 148 | 0.101366 149 | 0.214736 150 | 0.235421 151 | 0.189406 152 | 0.179515 153 | 0.144864 154 | 0.055114 155 | 0.071699 156 | 0.225539 157 | 0.257794 158 | 0.261241 159 | 0.164441 160 | 0.236986 161 | 0.199357 162 | 0.084418 163 | 0.095379 164 | 0.154929 165 | 0.103571 166 | 0.045186 167 | 0.112767 168 | 0.160841 169 | 0.159778 170 | 0.239770 171 | 0.111170 172 | 0.130826 173 | 0.157736 174 | 0.156707 175 | 0.169022 176 | 0.203887 177 | 0.112057 178 | 0.048079 179 | 0.162976 180 | 0.205567 181 | 0.228178 182 | 0.252800 183 | 0.235020 184 | 0.210352 185 | 0.230803 186 | 0.212428 187 | 0.231950 188 | 0.201199 189 | 0.240224 190 | 0.224713 191 | 0.203053 192 | 0.214748 193 | 0.233545 194 | 0.220770 195 | 0.197754 196 | 0.226537 197 | 0.275741 198 | 0.216126 199 | 0.274242 200 | 0.230477 201 | 0.232754 202 | 0.200599 203 | 0.194861 204 | 0.156241 205 | 0.231931 206 | 0.155313 207 | 0.233077 208 | 0.129591 209 | 0.156079 210 | 0.151367 211 | 0.162391 212 | 0.188715 213 | 0.204864 214 | 0.214213 215 | 0.225732 216 | 0.213383 217 | 0.196411 218 | 0.191112 219 | 0.066242 220 | 0.052712 221 | 0.255307 222 | 0.226190 223 | 0.178889 224 | 0.244800 225 | 0.181080 226 | 0.148736 227 | 0.225627 228 | 0.171622 229 | 0.083907 230 | 0.064993 231 | 0.083326 232 | 0.146869 233 | 0.182121 234 | 0.178561 235 | 0.172914 236 | 0.192447 237 | 0.182500 238 | 0.170625 239 | 0.097166 240 | 0.199812 241 | 0.181187 242 | 0.113677 243 | 0.094902 244 | 0.058963 245 | 0.088930 246 | 0.141068 247 | 0.075543 248 | 0.186191 249 | 0.226736 250 | 0.205667 251 | 0.202863 252 | 0.148697 253 | 0.142162 254 | 0.264468 255 | 0.229102 256 | 0.217327 257 | 0.239782 258 | 0.162386 259 | 0.133435 260 | 0.191539 261 | 0.219119 262 | 0.202511 263 | 0.202640 264 | 0.151561 265 | 0.176323 266 | 0.164331 267 | 0.249462 268 | 0.219818 269 | 0.096323 270 | 0.082604 271 | 0.068592 272 | 0.142815 273 | 0.083789 274 | 0.192158 275 | 0.214201 276 | 0.217402 277 | 0.214239 278 | 0.178538 279 | 0.240327 280 | 0.166424 281 | 0.113527 282 | 0.144174 283 | 0.259699 284 | 0.273813 285 | 0.217913 286 | 0.165483 287 | 0.271812 288 | 0.275271 289 | 0.262783 290 | 0.265606 291 | 0.249223 292 | 0.144427 293 | 0.233613 294 | 0.130640 295 | 0.164457 296 | 0.252799 297 | 0.256200 298 | 0.213388 299 | 0.207386 300 | 0.194377 301 | 0.229477 302 | 0.187407 303 | 0.252452 304 | 0.172174 305 | 0.130288 306 | 0.260992 307 | 0.240323 308 | 0.227866 309 | 0.236933 310 | 0.215740 311 | 0.206149 312 | 0.226036 313 | 0.198568 314 | 0.136543 315 | 0.110288 316 | 0.073945 317 | 0.114866 318 | 0.240271 319 | 0.204984 320 | 0.268903 321 | 0.212843 322 | 0.175904 323 | 0.154953 324 | 0.217224 325 | 0.239940 326 | 0.229827 327 | 0.255222 328 | 0.273635 329 | 0.218556 330 | 0.207027 331 | 0.112135 332 | 0.056369 333 | 0.127629 334 | 0.145521 335 | 0.092081 336 | 0.193858 337 | 0.164396 338 | 0.143782 339 | 0.228656 340 | 0.223330 341 | 0.181076 342 | 0.225142 343 | 0.147817 344 | 0.181823 345 | 0.190429 346 | 0.199322 347 | 0.193929 348 | 0.224377 349 | 0.157903 350 | 0.192918 351 | 0.181858 352 | 0.176276 353 | 0.223206 354 | 0.222306 355 | 0.162565 356 | 0.161960 357 | 0.099650 358 | 0.130793 359 | 0.064201 360 | 0.138055 361 | 0.064589 362 | 0.017523 363 | 0.017580 364 | 0.012234 365 | 0.045295 366 | 0.033350 367 | 0.033076 368 | 0.039628 369 | 0.024716 370 | 0.027858 371 | 0.017274 372 | 0.041771 373 | 0.057338 374 | 0.050069 375 | 0.012506 376 | 0.028586 377 | 0.028167 378 | 0.022402 379 | 0.037347 380 | 0.039981 381 | 0.025757 382 | 0.106444 383 | 0.088367 384 | 0.042319 385 | 0.045350 386 | 0.047376 387 | 0.112568 388 | 0.097170 389 | 0.102861 390 | 0.073253 391 | 0.049580 392 | 0.129851 393 | 0.145767 394 | 0.092711 395 | 0.083677 396 | 0.202992 397 | 0.205664 398 | 0.088686 399 | 0.065301 400 | 0.064877 401 | 0.054966 402 | 0.050956 403 | 0.070668 404 | 0.108319 405 | 0.182279 406 | 0.224439 407 | 0.167687 408 | 0.138519 409 | 0.102818 410 | 0.141977 411 | 0.180302 412 | 0.071407 413 | 0.056596 414 | 0.059711 415 | 0.044028 416 | 0.069125 417 | 0.088513 418 | 0.070490 419 | 0.044878 420 | 0.058831 421 | 0.098233 422 | 0.074557 423 | 0.105632 424 | 0.127447 425 | 0.106124 426 | 0.150807 427 | 0.177551 428 | 0.243583 429 | 0.214229 430 | 0.173404 431 | 0.172517 432 | 0.128286 433 | 0.181523 434 | 0.084394 435 | 0.201696 436 | 0.154799 437 | 0.157150 438 | 0.131238 439 | 0.121588 440 | 0.126402 441 | 0.119270 442 | 0.101068 443 | 0.086814 444 | 0.043907 445 | 0.051802 446 | 0.014372 447 | 0.028234 448 | 0.023153 449 | 0.029689 450 | 0.030571 451 | 0.033793 452 | 0.053496 453 | 0.035338 454 | 0.027777 455 | 0.028705 456 | 0.048452 457 | 0.171657 458 | 0.151012 459 | 0.137626 460 | 0.085222 461 | 0.030001 462 | 0.027753 463 | 0.036621 464 | 0.087146 465 | 0.039743 466 | 0.045017 467 | 0.027439 468 | 0.101549 469 | 0.053800 470 | 0.144664 471 | 0.057199 472 | 0.052660 473 | 0.062988 474 | 0.052588 475 | 0.048780 476 | 0.148116 477 | 0.059456 478 | 0.055300 479 | 0.037530 480 | 0.042656 481 | 0.035433 482 | 0.045437 483 | 0.032786 484 | 0.045808 485 | 0.027978 486 | 0.030645 487 | 0.049207 488 | 0.065021 489 | 0.040166 490 | 0.068183 491 | 0.040143 492 | 0.041581 493 | 0.066015 494 | 0.043042 495 | 0.049912 496 | 0.079472 497 | 0.036457 498 | 0.037506 499 | 0.027908 500 | 0.027386 501 | 0.046703 502 | 0.021044 503 | 0.039508 504 | 0.043281 505 | 0.053270 506 | 0.123756 507 | 0.137148 508 | 0.070792 509 | 0.058249 510 | 0.039545 511 | 0.048895 512 | 0.038949 513 | 0.029475 514 | 0.026539 515 | 0.016563 516 | 0.041806 517 | 0.022330 518 | 0.131982 519 | 0.059473 -------------------------------------------------------------------------------- /data/output/実行時間の計測/画像データ更新時/共面条件.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/knagara/SLAMwithCameraIMUforPython/15ddf2dff199997965856581098def23b08b6717/data/output/実行時間の計測/画像データ更新時/共面条件.xlsx -------------------------------------------------------------------------------- /data/plot3d.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from matplotlib import pyplot 4 | from mpl_toolkits.mplot3d import Axes3D 5 | from scipy import genfromtxt 6 | 7 | 8 | # ファイル読み込み 9 | d = genfromtxt("./output/particle_20160110_161058_22_2.csv", delimiter=",") 10 | 11 | # 表示範囲 12 | r = 0.2 13 | 14 | # グラフ作成 15 | fig = pyplot.figure() 16 | ax = Axes3D(fig) 17 | 18 | # 軸ラベルの設定 19 | ax.set_xlabel("X-axis") 20 | ax.set_ylabel("Y-axis") 21 | ax.set_zlabel("Z-axis") 22 | 23 | 24 | # 表示範囲の設定 25 | ax.set_xlim(-r, r) 26 | ax.set_ylim(-r, r) 27 | ax.set_zlim(-r, r) 28 | 29 | # 抽出条件設定 30 | #d1 = d[d[:,0] >= 7] 31 | #d2 = d[(d[:,0] < 7) & ((d[:,1] > 3) & (d[:,1] <= 3.5))] 32 | #d3 = d[(d[:,0] < 7) & ((d[:,1] <= 3) | (d[:,1] > 3.5))] 33 | 34 | 35 | # グラフ描画 36 | ax.plot(d[:,0], d[:,1], d[:,2], "o", color="#ff0000", ms=4, mew=0.5) 37 | #ax.plot(d1[:,0], d1[:,1], d1[:,2], "o", color="#cccccc", ms=4, mew=0.5) 38 | #ax.plot(d2[:,0], d2[:,1], d2[:,2], "o", color="#00cccc", ms=4, mew=0.5) 39 | #ax.plot(d3[:,0], d3[:,1], d3[:,2], "o", color="#ff0000", ms=4, mew=0.5) 40 | pyplot.show() -------------------------------------------------------------------------------- /data/run_GetImageData.cmd: -------------------------------------------------------------------------------- 1 | python GetImageData.py -------------------------------------------------------------------------------- /data/run_GetImageDescriptorData.cmd: -------------------------------------------------------------------------------- 1 | python GetImageDescriptorData.py -------------------------------------------------------------------------------- /data/run_GetImagePlot.cmd: -------------------------------------------------------------------------------- 1 | python GetImagePlot.py -------------------------------------------------------------------------------- /data/run_GetOutputDataTemp.cmd: -------------------------------------------------------------------------------- 1 | python GetOutputDataTemp.py -------------------------------------------------------------------------------- /descriptor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | descriptor.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | Class for descriptor in image 9 | """ 10 | 11 | class Descriptor: 12 | 13 | def __init__(self,data_): 14 | 15 | data__ = data_.split(',') 16 | self.data = [] 17 | 18 | for d in data__: 19 | if(d != ''): 20 | self.data.append(int(d)) 21 | 22 | 23 | def printData(self): 24 | 25 | for d in self.data: 26 | print(d), 27 | 28 | print(" ") -------------------------------------------------------------------------------- /image.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Image 4 | This class is factory class. 5 | """ 6 | 7 | from image_coplanarity import ImageCoplanarity 8 | from image_RBPF import ImageRBPF 9 | 10 | class Image: 11 | 12 | def __init__(self): 13 | pass 14 | 15 | def getImageClass(self,stateType): 16 | if(stateType=="Coplanarity"): 17 | return ImageCoplanarity() 18 | elif(stateType=="RBPF"): 19 | return ImageRBPF() 20 | else: 21 | pass 22 | -------------------------------------------------------------------------------- /image_RBPF.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | image.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | This class is called from "Main.py", and process image data. 9 | 10 | methods: 11 | processData(data) <- called from "Main.py" 12 | """ 13 | 14 | import sys 15 | from math import * 16 | import cv2 as cv 17 | import numpy as np 18 | import Util 19 | from keypoint import KeyPoint 20 | 21 | class ImageRBPF: 22 | 23 | def __init__(self): 24 | pass 25 | 26 | def init(self): 27 | #state.py 28 | #self.state.init() #this is also called in sensor.py 29 | #variables 30 | pass 31 | 32 | def setState(self,_state): 33 | #state.py 34 | self.state = _state 35 | 36 | 37 | 38 | #Set new data and Execute all functions 39 | def processData(self,time_,data): 40 | 41 | #if nomatch then nothing to do 42 | if(data[0] == "nomatch"): 43 | #print("nomatch") 44 | self.state.setImageData(0.0,"nomatch") 45 | return 46 | 47 | time = (float(long(time_) / 1000.0)) 48 | 49 | keypoints = [] 50 | for d in data: 51 | if(d != ''): 52 | keypoints.append(KeyPoint(d)) 53 | 54 | self.state.setImageData(time,keypoints) 55 | 56 | -------------------------------------------------------------------------------- /image_coplanarity.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | image.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | This class is called from "Main.py", and process image data. 9 | 10 | methods: 11 | processData(data) <- called from "Main.py" 12 | """ 13 | 14 | import sys 15 | from math import * 16 | import cv2 as cv 17 | import numpy as np 18 | import Util 19 | from keypoint_pair import KeyPointPair 20 | 21 | class ImageCoplanarity: 22 | 23 | def __init__(self): 24 | pass 25 | 26 | def init(self): 27 | #state.py 28 | #self.state.init() #this is also called in sensor.py 29 | #variables 30 | pass 31 | 32 | def setState(self,_state): 33 | #state.py 34 | self.state = _state 35 | 36 | 37 | 38 | #Set new data and Execute all functions 39 | def processData(self,time_,data): 40 | 41 | #if nomatch then nothing to do 42 | if(data[0] == "nomatch"): 43 | print("nomatch") 44 | return 45 | 46 | time = (float(long(time_) / 1000.0)) 47 | 48 | keypointPairs = [] 49 | for d in data: 50 | if(d != ''): 51 | keypointPairs.append(KeyPointPair(d)) 52 | 53 | self.state.setImageData(time,keypointPairs) 54 | 55 | -------------------------------------------------------------------------------- /keypoint.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | keypoint.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | Class for key point in image 9 | """ 10 | 11 | from descriptor import Descriptor 12 | 13 | class KeyPoint: 14 | 15 | def __init__(self,data_): 16 | 17 | #cx = 360.0 - 6.361694 # (image size X)/2 + principal point X 18 | #cy = 640.0 - 22.962158 # (image size Y)/2 + principal point Y 19 | 20 | cx = 540.0 # (image size X)/2 + principal point X 21 | cy = 960.0 # (image size Y)/2 + principal point Y 22 | 23 | data = data_.split(':') 24 | 25 | self.prevIndex = int(data[0]) 26 | self.index = int(data[1]) 27 | self.x1 = float(data[2]) - cx 28 | self.y1 = float(data[3]) - cy 29 | self.x = float(data[4]) - cx 30 | self.y = float(data[5]) - cy 31 | 32 | #self.descriptor = Descriptor(data[4]) -------------------------------------------------------------------------------- /keypoint_pair.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | keypoint.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | Class for key points pair between images 9 | """ 10 | 11 | class KeyPointPair: 12 | 13 | def __init__(self,data_): 14 | 15 | cx = 360.0 - 6.361694 # (image size X)/2 + principal point X 16 | cy = 640.0 - 22.962158 # (image size Y)/2 + principal point Y 17 | 18 | data = data_.split(':') 19 | 20 | self.prevIndex = int(data[0]) 21 | self.index = int(data[1]) 22 | self.x1 = float(data[2]) - cx 23 | self.y1 = float(data[3]) - cy 24 | self.x2 = float(data[4]) - cx 25 | self.y2 = float(data[5]) - cy -------------------------------------------------------------------------------- /landmark.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | landmark.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | Class for landmark in 3D space 9 | """ 10 | 11 | from math import * 12 | import numpy as np 13 | import Util 14 | 15 | class Landmark: 16 | 17 | def __init__(self): 18 | self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0]) 19 | self.sigma = np.zeros([]) 20 | #self.cx = - 6.361694 # principal point X 21 | #self.cy = - 22.962158 # principal point Y 22 | self.cx = 0.0 # principal point X 23 | self.cy = 0.0 # principal point Y 24 | 25 | 26 | def init(self, X, keypoint, P, focus): 27 | self.mu[0] = X.x[0] # xi (Device position X at first observation) 28 | self.mu[1] = X.x[1] # yi (Device position Y at first observation) 29 | self.mu[2] = X.x[2] # zi (Device position Z at first observation) 30 | self.mu[3], self.mu[4] = self.initThetaPhi(X, keypoint, focus) # theta, phi (Azimuth & elevation of the ray at first observation) 31 | self.mu[5] = 0.2 # d_inv (Inverse depth at first observation. 0.2 means that depth is 5 meter.) 32 | 33 | self.sigma = np.vstack((np.hstack((P,np.zeros([3,3]))),np.zeros([3,6]))) 34 | self.sigma[3][3] = 0.01 35 | self.sigma[4][4] = 0.01 36 | self.sigma[5][5] = 0.25 37 | 38 | 39 | def initThetaPhi(self, X, keypoint, focus): 40 | uvf = np.array([keypoint.x/focus, -keypoint.y/focus, -1]) # Camera coordinates -> Device coordinates, and normalized 41 | 42 | # Rotation matrix (Local coordinates -> Global coordinates) 43 | rotX = Util.rotationMatrixX(X.o[0]) 44 | rotY = Util.rotationMatrixY(X.o[1]) 45 | rotZ = Util.rotationMatrixZ(X.o[2]) 46 | # h = R(z)R(y)R(x)uvf 47 | h = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf))) 48 | theta = atan2(h[0], h[2]) 49 | phi = atan2(-h[1], hypot(h[0],h[2])) 50 | return theta, phi 51 | 52 | 53 | def initPrev(self, X1, keypoint, P1, focus): 54 | self.mu[0] = X1.x[0] # xi (Device position X at first observation) 55 | self.mu[1] = X1.x[1] # yi (Device position Y at first observation) 56 | self.mu[2] = X1.x[2] # zi (Device position Z at first observation) 57 | self.mu[3], self.mu[4] = self.initThetaPhiPrev(X1, keypoint, focus) # theta, phi (Azimuth & elevation of the ray at first observation) 58 | self.mu[5] = 0.2 # d_inv (Inverse depth at first observation. 0.2 means that depth is 5 meter.) 59 | 60 | self.sigma = np.vstack((np.hstack((P1,np.zeros([3,3]))),np.zeros([3,6]))) 61 | self.sigma[3][3] = 0.01 62 | self.sigma[4][4] = 0.01 63 | self.sigma[5][5] = 0.25 64 | 65 | 66 | def initThetaPhiPrev(self, X1, keypoint, focus): 67 | uvf = np.array([keypoint.x1/focus, -keypoint.y1/focus, -1]) # Camera coordinates -> Device coordinates, and normalized 68 | 69 | # Rotation matrix (Local coordinates -> Global coordinates) 70 | rotX = Util.rotationMatrixX(X1.o[0]) 71 | rotY = Util.rotationMatrixY(X1.o[1]) 72 | rotZ = Util.rotationMatrixZ(X1.o[2]) 73 | # h = R(z)R(y)R(x)uvf 74 | h = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf))) 75 | theta = atan2(h[0], h[2]) 76 | phi = atan2(-h[1], hypot(h[0],h[2])) 77 | return theta, phi 78 | 79 | 80 | def getXYZ(self): 81 | # xi, yi, zi, xt, yt, zt, p (Inverse depth) 82 | xi = self.mu[0] 83 | yi = self.mu[1] 84 | zi = self.mu[2] 85 | p = self.mu[5] 86 | # sin, cos 87 | sinTheta = sin(self.mu[3]) 88 | cosTheta = cos(self.mu[3]) 89 | sinPhi = sin(self.mu[4]) 90 | cosPhi = cos(self.mu[4]) 91 | # XYZ = landmark position in XYZ 92 | XYZ = np.array([xi + (cosPhi * sinTheta)/p, 93 | yi - sinPhi/p, 94 | zi + (cosPhi * cosTheta)/p]) 95 | return XYZ 96 | 97 | 98 | 99 | def h(self, position, o, focus): 100 | 101 | # often used variables 102 | # xi, yi, zi, xt, yt, zt, p (Inverse depth) 103 | xi = self.mu[0] 104 | yi = self.mu[1] 105 | zi = self.mu[2] 106 | xt = position[0] 107 | yt = position[1] 108 | zt = position[2] 109 | p = self.mu[5] 110 | # sin, cos 111 | sinTheta = sin(self.mu[3]) 112 | cosTheta = cos(self.mu[3]) 113 | sinPhi = sin(self.mu[4]) 114 | cosPhi = cos(self.mu[4]) 115 | 116 | # Rotation matrix (Global coordinates -> Local coordinates) 117 | rotXinv = Util.rotationMatrixX(-o[0]) 118 | rotYinv = Util.rotationMatrixY(-o[1]) 119 | rotZinv = Util.rotationMatrixZ(-o[2]) 120 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 121 | 122 | # hG = [hx, hy, hz].T in the global coordinates 123 | hG = np.array([p * (xi - xt) + cosPhi * sinTheta, 124 | p * (yi - yt) - sinPhi, 125 | p * (zi - zt) + cosPhi * cosTheta]) 126 | 127 | # XYZ = landmark position in XYZ 128 | XYZ = np.array([xi + (cosPhi * sinTheta)/p, 129 | yi - sinPhi/p, 130 | zi + (cosPhi * cosTheta)/p]) 131 | 132 | # hL = h Local = [hx, hy, hz].T in the local coordinates 133 | hL = np.dot(R, hG) 134 | hx = hL[0] 135 | hy = hL[1] 136 | hz = hL[2] 137 | 138 | # h1 = - f*hx/hz, h2 = - f*hy/hz , and Device coordinates -> Camera coordinates 139 | h1 = - (focus * hx / hz) 140 | h2 = focus * hy / hz 141 | 142 | return np.array([h1,h2]), XYZ 143 | 144 | 145 | def calcObservation(self, X, focus): 146 | """ 147 | Calc h and H (Jacobian matrix of h) 148 | 149 | Observation function 150 | z = h(x) + v 151 | 152 | h(x) = [h1(x), h2(x)].T 153 | h1(x) = f*hx/hz - cx 154 | h2(x) = f*hy/hz - cy 155 | """ 156 | 157 | # often used variables 158 | # xi, yi, zi, xt, yt, zt, p (Inverse depth) 159 | xi = self.mu[0] 160 | yi = self.mu[1] 161 | zi = self.mu[2] 162 | xt = X.x[0] 163 | yt = X.x[1] 164 | zt = X.x[2] 165 | p = self.mu[5] 166 | # sin, cos 167 | sinTheta = sin(self.mu[3]) 168 | cosTheta = cos(self.mu[3]) 169 | sinPhi = sin(self.mu[4]) 170 | cosPhi = cos(self.mu[4]) 171 | 172 | # Rotation matrix (Global coordinates -> Local coordinates) 173 | rotXinv = Util.rotationMatrixX(-X.o[0]) 174 | rotYinv = Util.rotationMatrixY(-X.o[1]) 175 | rotZinv = Util.rotationMatrixZ(-X.o[2]) 176 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 177 | 178 | # hG = [hx, hy, hz].T in the global coordinates 179 | hG = np.array([p * (xi - xt) + cosPhi * sinTheta, 180 | p * (yi - yt) - sinPhi, 181 | p * (zi - zt) + cosPhi * cosTheta]) 182 | 183 | # hL = h Local = [hx, hy, hz].T in the local coordinates 184 | hL = np.dot(R, hG) 185 | hx = hL[0] 186 | hy = hL[1] 187 | hz = hL[2] 188 | 189 | # h1 = - f*hx/hz, h2 = - f*hy/hz , and Device coordinates -> Camera coordinates 190 | h1 = - (focus * hx / hz) 191 | h2 = focus * hy / hz 192 | 193 | # derivative 194 | R11 = R[0][0] 195 | R12 = R[0][1] 196 | R13 = R[0][2] 197 | R21 = R[1][0] 198 | R22 = R[1][1] 199 | R23 = R[1][2] 200 | R31 = R[2][0] 201 | R32 = R[2][1] 202 | R33 = R[2][2] 203 | 204 | dhxxi = p * R11 205 | dhyxi = p * R21 206 | dhzxi = p * R31 207 | 208 | dhxyi = p * R12 209 | dhyyi = p * R22 210 | dhzyi = p * R32 211 | 212 | dhxzi = p * R13 213 | dhyzi = p * R23 214 | dhzzi = p * R33 215 | 216 | dhxTheta = R11 * cosPhi * cosTheta - R13 * cosPhi * sinTheta 217 | dhyTheta = R21 * cosPhi * cosTheta - R23 * cosPhi * sinTheta 218 | dhzTheta = R31 * cosPhi * cosTheta - R33 * cosPhi * sinTheta 219 | 220 | dhxPhi = - R11 * sinTheta * sinPhi - R12 * cosPhi - R13 * cosTheta * sinPhi 221 | dhyPhi = - R21 * sinTheta * sinPhi - R22 * cosPhi - R23 * cosTheta * sinPhi 222 | dhzPhi = - R31 * sinTheta * sinPhi - R32 * cosPhi - R33 * cosTheta * sinPhi 223 | 224 | dhxp = R11 * (xi - xt) + R12 * (yi - yt) + R13 * (zi - zt) 225 | dhyp = R21 * (xi - xt) + R22 * (yi - yt) + R23 * (zi - zt) 226 | dhzp = R31 * (xi - xt) + R32 * (yi - yt) + R33 * (zi - zt) 227 | 228 | # Jacobian 229 | f_hz2 = focus / (hz * hz) # focus / (hz)^2 230 | 231 | dh1xi = - f_hz2 * (dhxxi * hz - hx * dhzxi) 232 | dh1yi = - f_hz2 * (dhxyi * hz - hx * dhzyi) 233 | dh1zi = - f_hz2 * (dhxzi * hz - hx * dhzzi) 234 | dh1Theta = - f_hz2 * (dhxTheta * hz - hx * dhzTheta) 235 | dh1Phi = - f_hz2 * (dhxPhi * hz - hx * dhzPhi) 236 | dh1p = - f_hz2 * (dhxp * hz - hx * dhzp) 237 | 238 | dh2xi = f_hz2 * (dhyxi * hz - hy * dhzxi) 239 | dh2yi = f_hz2 * (dhyyi * hz - hy * dhzyi) 240 | dh2zi = f_hz2 * (dhyzi * hz - hy * dhzzi) 241 | dh2Theta = f_hz2 * (dhyTheta * hz - hy * dhzTheta) 242 | dh2Phi = f_hz2 * (dhyPhi * hz - hy * dhzPhi) 243 | dh2p = f_hz2 * (dhyp * hz - hy * dhzp) 244 | 245 | Hm = np.array([[dh1xi, dh1yi, dh1zi, dh1Theta, dh1Phi, dh1p], 246 | [dh2xi, dh2yi, dh2zi, dh2Theta, dh2Phi, dh2p]]) 247 | 248 | Hx = np.array([[-dh1xi, -dh1yi, -dh1zi], 249 | [-dh2xi, -dh2yi, -dh2zi]]) 250 | 251 | return np.array([h1,h2]), Hx, Hm 252 | -------------------------------------------------------------------------------- /landmarkObservation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | landmarkObservation.py 4 | 5 | author: Keita Nagara　永良慶太 (University of Tokyo) 6 | 7 | """ 8 | 9 | import theano 10 | import theano.tensor as T 11 | import numpy as np 12 | 13 | class LandmarkObservation: 14 | 15 | def __init__(self): 16 | 17 | # ------------------- # 18 | # --- variables --- # 19 | # ------------------- # 20 | 21 | # focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p 22 | 23 | focus = T.dscalar('focus') # focus length od camera 24 | 25 | # variables of device state 26 | xt, yt, zt = T.dscalars('xt', 'yt', 'zt') # position 27 | ox, oy, oz = T.dscalars('ox', 'oy', 'oz') # orientation 28 | 29 | # variables of landmark state 30 | xi, yi, zi = T.dscalars('xi', 'yi', 'zi') # Device position at first observation 31 | theta, phi = T.dscalars('theta', 'phi') # theta, phi (Azimuth & elevation of the ray at first observation) 32 | p = T.dscalar('p') # d_inv (Inverse depth at first observation. 0.1 means depth is 10 meter.) 33 | 34 | # ------------------- # 35 | # --- observation --- # 36 | # ------------------- # 37 | # T.sin(ox) 38 | # T.cos(ox) 39 | # T.sin(oy) 40 | # T.cos(oy) 41 | # T.sin(oz) 42 | # T.cos(oz) 43 | # T.sin(theta) 44 | # T.cos(theta) 45 | # T.sin(phi) 46 | # T.cos(phi) 47 | 48 | # --- # h_ = [hx, hy, hz].T in the global coordinates --- # 49 | h_x = p*(xi - xt) + T.cos(phi)*T.sin(theta) 50 | h_y = p*(yi - yt) - T.sin(phi) 51 | h_z = p*(zi - zt) + T.cos(phi)*T.cos(theta) 52 | 53 | # ---- hx, hy, hz ---- # 54 | hx = (T.cos(oy)*T.cos(oz)) * h_x + (T.cos(oy)*T.sin(oz)) * h_y + (-T.sin(oy)) * h_z 55 | hy = (-T.cos(ox)*T.sin(oz)+T.sin(ox)*T.sin(oy)*T.cos(oz)) * h_x + (T.cos(ox)*T.cos(oz)+T.sin(ox)*T.sin(oy)*T.sin(oz)) * h_y + (T.sin(ox)*T.cos(oy)) * h_z 56 | hz = (T.sin(ox)*T.sin(oz)+T.cos(ox)*T.sin(oy)*T.cos(oz)) * h_x + (-T.sin(ox)*T.cos(oz) + T.cos(ox)*T.sin(oy)*T.sin(oz)) * h_y + (T.cos(ox)*T.cos(oy)) * h_z 57 | 58 | # --- h1, h2 --- # 59 | h1 = - (focus * hx / hz) 60 | h2 = focus * hy / hz 61 | 62 | self.fh1 = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=h1) 63 | self.fh2 = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=h2) 64 | 65 | # partial derivative 66 | dh1xt = T.grad(cost=h1, wrt=xt) 67 | dh1yt = T.grad(cost=h1, wrt=yt) 68 | dh1zt = T.grad(cost=h1, wrt=zt) 69 | dh1ox = T.grad(cost=h1, wrt=ox) 70 | dh1oy = T.grad(cost=h1, wrt=oy) 71 | dh1oz = T.grad(cost=h1, wrt=oz) 72 | dh1xi = T.grad(cost=h1, wrt=xi) 73 | dh1yi = T.grad(cost=h1, wrt=yi) 74 | dh1zi = T.grad(cost=h1, wrt=zi) 75 | dh1theta = T.grad(cost=h1, wrt=theta) 76 | dh1phi = T.grad(cost=h1, wrt=phi) 77 | dh1p = T.grad(cost=h1, wrt=p) 78 | 79 | dh2xt = T.grad(cost=h2, wrt=xt) 80 | dh2yt = T.grad(cost=h2, wrt=yt) 81 | dh2zt = T.grad(cost=h2, wrt=zt) 82 | dh2ox = T.grad(cost=h2, wrt=ox) 83 | dh2oy = T.grad(cost=h2, wrt=oy) 84 | dh2oz = T.grad(cost=h2, wrt=oz) 85 | dh2xi = T.grad(cost=h2, wrt=xi) 86 | dh2yi = T.grad(cost=h2, wrt=yi) 87 | dh2zi = T.grad(cost=h2, wrt=zi) 88 | dh2theta = T.grad(cost=h2, wrt=theta) 89 | dh2phi = T.grad(cost=h2, wrt=phi) 90 | dh2p = T.grad(cost=h2, wrt=p) 91 | 92 | # function of partial derivative 93 | self.fdh1xt = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1xt) 94 | self.fdh1yt = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1yt) 95 | self.fdh1zt = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1zt) 96 | self.fdh1ox = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1ox) 97 | self.fdh1oy = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1oy) 98 | self.fdh1oz = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1oz) 99 | self.fdh1xi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1xi) 100 | self.fdh1yi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1yi) 101 | self.fdh1zi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1zi) 102 | self.fdh1theta = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1theta) 103 | self.fdh1phi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1phi) 104 | self.fdh1p = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh1p) 105 | 106 | self.fdh2xt = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2xt) 107 | self.fdh2yt = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2yt) 108 | self.fdh2zt = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2zt) 109 | self.fdh2ox = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2ox) 110 | self.fdh2oy = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2oy) 111 | self.fdh2oz = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2oz) 112 | self.fdh2xi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2xi) 113 | self.fdh2yi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2yi) 114 | self.fdh2zi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2zi) 115 | self.fdh2theta = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2theta) 116 | self.fdh2phi = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2phi) 117 | self.fdh2p = theano.function(inputs=[focus, xt, yt, zt, ox, oy, oz, xi, yi, zi, theta, phi, p], outputs=dh2p) 118 | -------------------------------------------------------------------------------- /particle.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | particle.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | Class for particle 9 | """ 10 | 11 | import numpy as np 12 | import time 13 | 14 | 15 | class Particle: 16 | 17 | def __init__(self): 18 | 19 | self.x = np.array([0.0,0.0,0.0]) 20 | self.v = np.array([0.0,0.0,0.0]) 21 | self.a = np.array([0.0,0.0,0.0]) 22 | self.o = np.array([0.0,0.0,0.0]) 23 | 24 | self.landmarks = {} 25 | 26 | 27 | def initWithPositionAndOrientation(self, x_, o_): 28 | self.x = x_ 29 | self.o = o_ 30 | 31 | 32 | def initWithMu(self,mu): 33 | self.x = np.array([mu[0],mu[1],mu[2]]) 34 | self.o = np.array([mu[9],mu[10],mu[11]]) 35 | 36 | 37 | def initWithIMU(self,accel,ori): 38 | self.a = accel 39 | self.o = ori 40 | 41 | 42 | def initWithStateVector(self, mu, sigma): 43 | #generate random state from mu. sigma 44 | try: 45 | state = np.random.multivariate_normal(mu, sigma) 46 | except: 47 | print("Error on initWithStateVector np.random.multivariate_normal(mu, sigma)") 48 | state = mu 49 | 50 | self.x = np.array([state[0],state[1],state[2]]) 51 | self.v = np.array([state[3],state[4],state[5]]) 52 | self.a = np.array([state[6],state[7],state[8]]) 53 | self.o = np.array([state[9],state[10],state[11]]) 54 | 55 | 56 | def appendLandmark(self, key, landmark): 57 | self.landmarks[key] = landmark 58 | 59 | 60 | def printXYZ(self): 61 | print(str(self.x[0])+","+str(self.x[1])+","+str(self.x[2])) 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /particle_filter.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Particle Filter 4 | This class is factory class. 5 | """ 6 | 7 | from particle_filter_coplanarity import ParticleFilterCoplanarity 8 | from particle_filter_RBPF import ParticleFilterRBPF 9 | from particle_filter_IMU import ParticleFilterIMU 10 | from particle_filter_IMU2 import ParticleFilterIMU2 11 | 12 | class ParticleFilter: 13 | 14 | def __init__(self): 15 | pass 16 | 17 | def getParticleFilterClass(self,stateType): 18 | if(stateType=="Coplanarity"): 19 | return ParticleFilterCoplanarity() 20 | elif(stateType=="RBPF"): 21 | return ParticleFilterRBPF() 22 | elif(stateType=="IMUPF"): 23 | return ParticleFilterIMU() 24 | elif(stateType=="IMUPF2"): 25 | return ParticleFilterIMU2() 26 | else: 27 | pass 28 | -------------------------------------------------------------------------------- /particle_filter_IMU.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Particle Filter 4 | (IMUPF model version) 5 | 6 | Difference between IMUPF and IMUPF2: 7 | IMUPF -> IMU data is regarded as observation 8 | IMUPF2 -> IMU data is regarded as control 9 | """ 10 | 11 | import numpy 12 | import copy 13 | import math 14 | 15 | class ParticleFilterIMU: 16 | 17 | def __init__(self): 18 | self.var_sys = 1.0 19 | self.var_obs = 1.0 20 | 21 | def setParameter(self, param1, param2): 22 | self.var_sys = param1 * param2 23 | self.var_obs = param1 24 | 25 | 26 | def f(self, dt, X): 27 | """ Transition model 28 | - 状態方程式 29 | x_t = f(x_t-1) + w 30 | w ~ N(0, sigma) 31 | """ 32 | 33 | w_mean = numpy.zeros(3) # mean of noise 34 | w_cov_a = numpy.eye(3) * self.var_sys # covariance matrix of noise (accel) 35 | w_a = numpy.random.multivariate_normal(w_mean, w_cov_a) # generate random 36 | w_cov_o = numpy.eye(3) * self.var_sys # covariance matrix of noise (ori) 37 | w_o = numpy.random.multivariate_normal(w_mean, w_cov_o) # generate random 38 | 39 | dt2 = dt*dt 40 | dt3 = dt*dt*dt 41 | 42 | X_new = copy.deepcopy(X) 43 | X_new.x = X.x + dt*X.v + 0.5*dt2*X.a + 0.166666*dt3*w_a 44 | X_new.v = X.v + dt*X.a + 0.5*dt2*w_a 45 | X_new.a = X.a + dt*w_a 46 | X_new.o = X.o + dt*w_o 47 | 48 | return X_new 49 | 50 | 51 | def likelihood(self, accel, ori, X): 52 | """ Likelihood function 53 | - 尤度関数 54 | p(y|x) ~ exp(-1/2 * (|y-h(x)|.t * sigma * |y-h(x)|) 55 | - 観測モデル 56 | z = h(x) + v 57 | v ~ N(0, sigma) 58 | Parameters 59 | ---------- 60 | accel : 観測 (加速度) Observation set (accel) 61 | ori : 観測 (姿勢) Observation set (orientation) 62 | X : 予測　Predicted particle 63 | Returns 64 | ------- 65 | likelihood : 尤度 Likelihood 66 | """ 67 | sigma_a_inv = 1.0/self.var_obs # 1.0 / sigma_a 68 | sigma_o_inv = 1.0/self.var_obs # 1.0 / sigma_o 69 | v_cov_a = numpy.eye(3) * sigma_a_inv # inv of covariance matrix 70 | v_cov_o = numpy.eye(3) * sigma_o_inv # inv of covariance matrix 71 | 72 | y_X_a = accel - X.a # y - X (accel) 73 | y_X_o = ori - X.o # y - X (orientation) 74 | 75 | return numpy.exp(-0.5 * (numpy.dot(y_X_a, numpy.dot(v_cov_a, y_X_a)) + numpy.dot(y_X_o, numpy.dot(v_cov_o, y_X_o)))) / (((2*math.pi)**3) * math.sqrt(self.var_obs*6)) 76 | #return numpy.exp(-0.5 * (numpy.dot(y_X_a, numpy.dot(v_cov_a, y_X_a)) + numpy.dot(y_X_o, numpy.dot(v_cov_o, y_X_o)))) 77 | 78 | 79 | def resampling(self, X, W, M): 80 | """ Resampling 81 | - 等間隔サンプリング 82 | M : パーティクルの数 num of particles 83 | """ 84 | X_resampled = [] 85 | Minv = 1/float(M) 86 | r = numpy.random.rand() * Minv 87 | c = W[0] 88 | i = 0 89 | for m in range(M): 90 | U = r + m * Minv 91 | while U > c: 92 | i += 1 93 | c += W[i] 94 | X_resampled.append(X[i]) 95 | return X_resampled 96 | 97 | 98 | def pf_step(self, X, dt, accel, ori, M): 99 | """ One Step of Sampling Importance Resampling for Particle Filter 100 | for IMU sensor 101 | Parameters 102 | ---------- 103 | X : 状態 List of state set 104 | dt: 時刻の差分 delta of time 105 | accel : 観測 (加速度) Observation set (accel) 106 | ori : 観測 (姿勢) Observation set (orientation) 107 | M : パーティクルの数 num of particles 108 | Returns 109 | ------- 110 | X_resampled : 次の状態 List updated state 111 | """ 112 | 113 | # 初期化 114 | X_predicted = range(M) 115 | weight = range(M) 116 | 117 | for i in range(M): 118 | # 推定 prediction 119 | X_predicted[i] = self.f(dt, X[i]) 120 | # 更新 update 121 | weight[i] = self.likelihood(accel, ori, X_predicted[i]) 122 | # 正規化 normalization 123 | weight_sum = sum(weight) # 総和 the sum of weights 124 | for i in range(M): 125 | weight[i] /= weight_sum 126 | # リサンプリング re-sampling (if necessary) 127 | X_resampled = self.resampling(X_predicted, weight, M) 128 | 129 | return X_resampled 130 | -------------------------------------------------------------------------------- /particle_filter_IMU2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Particle Filter 4 | (IMUPF2 model version) 5 | 6 | Difference between IMUPF and IMUPF2: 7 | IMUPF -> IMU data is regarded as observation 8 | IMUPF2 -> IMU data is regarded as control 9 | """ 10 | 11 | import numpy 12 | import copy 13 | import math 14 | import Util 15 | 16 | class ParticleFilterIMU2: 17 | 18 | def __init__(self): 19 | self.var_sys = 1.0 20 | self.var_obs = 1.0 21 | 22 | def setParameter(self, param1, param2): 23 | self.var_sys = param1 24 | self.var_obs = param2 25 | 26 | 27 | def f(self, dt, X, accel, ori): 28 | """ Transition model 29 | - 状態方程式 30 | x_t = f(x_t-1, u) + w 31 | w ~ N(0, sigma) 32 | """ 33 | 34 | w_mean = numpy.zeros(3) # mean of noise 35 | w_cov_a = numpy.eye(3) * self.var_sys # covariance matrix of noise (accel) 36 | w_a = numpy.random.multivariate_normal(w_mean, w_cov_a) # generate random 37 | w_cov_o = numpy.eye(3) * self.var_sys # covariance matrix of noise (ori) 38 | w_o = numpy.random.multivariate_normal(w_mean, w_cov_o) # generate random 39 | 40 | dt2 = dt*dt 41 | dt3 = dt*dt*dt 42 | 43 | X_new = copy.deepcopy(X) 44 | 45 | X_new.x = X.x + dt*X.v + 0.5*dt2*X.a + 0.5*dt2*w_a 46 | X_new.v = X.v + dt*X.a + dt*w_a 47 | X_new.a = accel 48 | X_new.o = ori 49 | 50 | 51 | return X_new 52 | 53 | 54 | def likelihood(self, y, x): 55 | """ Likelihood function 56 | - 尤度関数 57 | p(y|x) ~ exp(-1/2 * (|y-h(x)|.t * sigma * |y-h(x)|) 58 | - 観測モデル 59 | z = h(x) + v 60 | v ~ N(0, sigma) 61 | Parameters 62 | ---------- 63 | y : 観測 Observation 64 | x : 予測　Predicted particle 65 | Returns 66 | ------- 67 | likelihood : 尤度 Likelihood 68 | """ 69 | 70 | return 1.0 71 | 72 | 73 | def resampling(self, X, W, M): 74 | """ Resampling 75 | - 等間隔サンプリング 76 | M : パーティクルの数 num of particles 77 | """ 78 | X_resampled = [] 79 | Minv = 1/float(M) 80 | r = numpy.random.rand() * Minv 81 | c = W[0] 82 | i = 0 83 | for m in range(M): 84 | U = r + m * Minv 85 | while U > c: 86 | i += 1 87 | c += W[i] 88 | X_resampled.append(X[i]) 89 | return X_resampled 90 | 91 | 92 | def pf_step(self, X, dt, accel, ori, M): 93 | """ One Step of Sampling Importance Resampling for Particle Filter 94 | for IMU sensor 95 | Parameters 96 | ---------- 97 | X : 状態 List of state set 98 | dt: 時刻の差分 delta of time 99 | accel : 制御 (加速度) Control (accel) 100 | ori : 制御 (姿勢) Control (orientation) 101 | M : パーティクルの数 num of particles 102 | Returns 103 | ------- 104 | X_resampled : 次の状態 List updated state 105 | """ 106 | 107 | # 初期化 108 | X_predicted = range(M) 109 | weight = range(M) 110 | 111 | for i in range(M): 112 | # 推定 prediction 113 | X_predicted[i] = self.f(dt, X[i], accel, ori) 114 | # 更新 update 115 | #weight[i] = self.likelihood(y, X_predicted[i]) 116 | # 正規化 normalization 117 | #weight_sum = sum(weight) # 総和 the sum of weights 118 | #for i in range(M): 119 | # weight[i] /= weight_sum 120 | # リサンプリング re-sampling (if necessary) 121 | #X_resampled = self.resampling(X_predicted, weight, M) 122 | 123 | #観測がないので，パーティクルは予測だけで，更新されない 124 | X_resampled = X_predicted 125 | 126 | return X_resampled 127 | -------------------------------------------------------------------------------- /particle_filter_RBPF.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Particle Filter 4 | (RBPF model version) 5 | """ 6 | 7 | import time 8 | import datetime 9 | import numpy as np 10 | import copy 11 | import math 12 | from particle import Particle 13 | from landmark import Landmark 14 | import KF 15 | import Util 16 | 17 | 18 | class ParticleFilterRBPF: 19 | 20 | def __init__(self): 21 | pass 22 | 23 | def setFocus(self, f_): 24 | self.focus = f_ 25 | 26 | def setParameter(self, param1, param2, param3, param4, param5, param6): 27 | self.noise_x_sys = param1 # system noise of position (SD)　位置のシステムノイズ（標準偏差） 28 | self.noise_a_sys = param2 # system noise of acceleration (SD)　加速度のシステムノイズ（標準偏差） 29 | self.noise_g_sys = param3 # system noise of orientation (SD)　角度のシステムノイズ（標準偏差） 30 | self.noise_camera = param4 # observation noise of camera (SD) カメラの観測ノイズ（標準偏差） 31 | self.noise_coplanarity = param5 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差） 32 | self.noise_x_sys_coefficient = param6 #パーティクルフィルタのパラメータ（ノイズ） parameters (noise) 33 | 34 | self.Q = np.diag([self.noise_x_sys**2, self.noise_x_sys**2, self.noise_x_sys**2]) 35 | 36 | self.R = np.diag([self.noise_camera**2, self.noise_camera**2]) 37 | self.Rinv = np.diag([1.0/(self.noise_camera**2), 1.0/(self.noise_camera**2)]) 38 | 39 | def setObservationModel(self, observation_): 40 | self.observation = observation_ 41 | 42 | 43 | def f_IMU(self, X, dt, dt2, accel, ori, noise): 44 | """ Transition model 45 | - 状態方程式 46 | x_t = f(x_t-1, u) + w 47 | w ~ N(0, sigma) 48 | """ 49 | 50 | X_new = Particle() 51 | X_new.landmarks = X.landmarks 52 | 53 | # Transition with noise (only x,v) 54 | X_new.x = X.x + dt*X.v + dt2*X.a + noise 55 | X_new.v = X.v + dt*X.a 56 | #X_new.v = X.v + dt*X.a + noise*25 57 | #X_new.v = X.v + dt*X.a + noise*50 58 | X_new.a = accel 59 | X_new.o = ori 60 | 61 | return X_new 62 | 63 | 64 | def predictionAndUpdateOneParticle_firsttime(self, X, dt, dt2, keypoints, step, P): 65 | 66 | """ 67 | FastSLAM1.0 68 | """ 69 | 70 | weight = 0.0 # weight (return value) 71 | weights = [] 72 | 73 | # 姿勢予測 prediction of position 74 | X_ = Particle() 75 | X_.landmarks = X.landmarks 76 | X_.x = X.x + dt*X.v + dt2*X.a 77 | X_.v = X.v + dt*X.a 78 | X_.a = X.a 79 | X_.o = X.o 80 | 81 | for keypoint in keypoints: 82 | # previous landmark id 83 | prevLandmarkId = (step-1)*10000 + keypoint.prevIndex 84 | # new landmark id 85 | landmarkId = step*10000 + keypoint.index 86 | # The landmark is already observed or not? 87 | if(X_.landmarks.has_key(prevLandmarkId) == False): 88 | # Fisrt observation 89 | # Initialize landmark and append to particle 90 | landmark = Landmark() 91 | landmark.init(X_, keypoint, P, self.focus) 92 | X_.landmarks[landmarkId] = landmark 93 | else: 94 | print("Error on pf_step_camera_firsttime") 95 | 96 | self.count+=1 97 | 98 | return X_, weight 99 | 100 | 101 | def predictionAndUpdateOneParticle(self, X, dt, dt2, noise, keypoints, step, P, X1, P1, dt_camera, noise_o): 102 | 103 | """ 104 | FastSLAM1.0 105 | """ 106 | 107 | weight = 0.0 # weight (return value) 108 | weights = [] 109 | count_of_first_observation = 0 110 | 111 | # 姿勢予測 prediction of position 112 | X_ = Particle() 113 | X_.landmarks = X.landmarks 114 | X_.x = X.x + dt*X.v + dt2*X.a + noise 115 | #X_.v = X.v + dt*X.a 116 | #X_.v = X.v + dt*X.a + noise*25 117 | #X_.v = X.v + dt*X.a + noise*50 118 | X_.a = X.a 119 | X_.o = X.o 120 | #X_.o = X.o + noise_o 121 | 122 | # 速度調整 velocity adjustment 123 | #X_.v = ((X_.x - X1.x)/dt_camera) 124 | X_.v = ((X_.x - X1.x)/dt_camera)*0.25 125 | #X_.v = ((X_.x - X1.x)/dt_camera)*0.5 126 | 127 | # 共面条件モデルのための計算 Calc for Coplanarity 128 | xyz = np.array([X_.x[0] - X1.x[0], X_.x[1] - X1.x[1], X_.x[2] - X1.x[2]]) 129 | 130 | # 前回ランドマークの全てのキーを取得しておく　あとで消す 131 | prevLandmarkKeys = [] 132 | for key in X_.landmarks: 133 | prevLandmarkKeys.append(key) 134 | 135 | for keypoint in keypoints: 136 | 137 | # ---------------------------- # 138 | # Calc weight of Inverse Depth # 139 | # ---------------------------- # 140 | # previous landmark id 141 | prevLandmarkId = (step-1)*10000 + keypoint.prevIndex 142 | # new landmark id 143 | landmarkId = step*10000 + keypoint.index 144 | # The landmark is already observed or not? 145 | if(X_.landmarks.has_key(prevLandmarkId) == False): 146 | # Fisrt observation 147 | # Initialize landmark and append to particle 148 | landmark = Landmark() 149 | landmark.initPrev(X1, keypoint, P1, self.focus) 150 | X_.landmarks[landmarkId] = landmark 151 | if(self.count == 0): 152 | count_of_first_observation += 1 153 | else: 154 | # Already observed 155 | X_.landmarks[landmarkId] = X_.landmarks[prevLandmarkId] 156 | #del X_.landmarks[prevLandmarkId] 157 | # Update landmark and calc weight 158 | # Observation z 159 | z = np.array([keypoint.x, keypoint.y]) 160 | # Calc h and H (Jacobian matrix of h) 161 | h, Hx, H = X_.landmarks[landmarkId].calcObservation(X_, self.focus) 162 | # Kalman filter (Landmark update) 163 | S = H.dot(X_.landmarks[landmarkId].sigma.dot(H.T)) + self.R 164 | Sinv = np.linalg.inv(S) 165 | K = X_.landmarks[landmarkId].sigma.dot(H.T.dot(Sinv)) 166 | X_.landmarks[landmarkId].mu += K.dot(z - h) 167 | X_.landmarks[landmarkId].sigma = X_.landmarks[landmarkId].sigma - K.dot(H.dot(X_.landmarks[landmarkId].sigma)) 168 | # Calc weight 169 | w = 0.0 170 | try: 171 | #w = (1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * S) ))) * np.exp( -0.5 * ( (z-h).T.dot(Sinv.dot(z-h)) ) ) 172 | #w = (1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * self.R) ))) * np.exp( -0.5 * ( (z-h).T.dot(self.Rinv.dot(z-h)) ) ) 173 | # log likelihood 174 | #w = np.log(1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * self.R) ))) + ( -0.5 * ( (z-h).T.dot(self.Rinv.dot(z-h)) ) ) 175 | w = ( -0.5 * ( (z-h).T.dot(self.Rinv.dot(z-h)) ) ) 176 | except: 177 | print("Error on calc inverse weight ******") 178 | w = 0.0 179 | 180 | # -------------------------- # 181 | # Calc weight of Coplanarity # 182 | # -------------------------- # 183 | # Generate uvw1 (time:t-1) 184 | uvf1 = np.array([keypoint.x1, -keypoint.y1, -self.focus]) # Camera coordinates -> Device coordinates 185 | rotX = Util.rotationMatrixX(X1.o[0]) 186 | rotY = Util.rotationMatrixY(X1.o[1]) 187 | rotZ = Util.rotationMatrixZ(X1.o[2]) 188 | # uvw1 = R(z)R(y)R(x)uvf1 189 | uvw1 = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf1))) 190 | uvw1 /= 100.0 # Adjust scale to decrease calculation error. This doesn't have an influence to estimation. 191 | # Generate uvw2 (time:t) 192 | uvf2 = np.array([keypoint.x, -keypoint.y, -self.focus]) # Camera coordinates -> Device coordinates 193 | rotX = Util.rotationMatrixX(X_.o[0]) 194 | rotY = Util.rotationMatrixY(X_.o[1]) 195 | rotZ = Util.rotationMatrixZ(X_.o[2]) 196 | # uvw2 = R(z)R(y)R(x)uvf2 197 | uvw2 = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf2))) 198 | uvw2 /= 100.0 # Adjust scale to decrease calculation error. This doesn't have an influence to estimation. 199 | # Generate coplanarity matrix 200 | coplanarity_matrix = np.array([xyz,uvw1,uvw2]) 201 | # Calc determinant 202 | determinant = np.linalg.det(coplanarity_matrix) 203 | # Weight 204 | w_coplanarity = 0.0 205 | try: 206 | #w_coplanarity = (1.0 / (math.sqrt( 2.0 * math.pi * self.noise_coplanarity**2 ))) * np.exp((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) ) 207 | # log likelihood 208 | #w_coplanarity = np.log(1.0 / (math.sqrt( 2.0 * math.pi * self.noise_coplanarity**2 ))) + ((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) ) 209 | w_coplanarity = ((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) ) 210 | except: 211 | print("Error on calc coplanarity weight ******") 212 | w_coplanarity = 0.0 213 | 214 | # --------------------------- # 215 | # inverse depth * coplanarity # 216 | # --------------------------- # 217 | if(self.count == 0): 218 | #print(w), 219 | #print(w_coplanarity) 220 | pass 221 | 222 | w = w + w_coplanarity # use inverse depth * coplanarity log likelihood 223 | #w = w_coplanarity # use only coplanarity log likelihood 224 | #w = w # use only inverse depth log likelihood 225 | 226 | weights.append(w) 227 | 228 | # ----------------------------------- # 229 | # sum log likelihood of all keypoints # 230 | # ----------------------------------- # 231 | for i,w in enumerate(weights): 232 | if(i==0): 233 | weight = w 234 | else: 235 | weight += w 236 | 237 | # ----------------------------- # 238 | # Average of log likelihood sum # 239 | # ----------------------------- # 240 | weight /= float(len(weights)) 241 | 242 | ############################### 243 | #print("weight "+str(weight)) 244 | if(self.count == 0): 245 | #print("weight "+str(weight)) 246 | #print("first_ratio "+str(float(count_of_first_observation)/float(len(keypoints)))), 247 | #print("("+str(count_of_first_observation)+"/"+str(len(keypoints))+")") 248 | pass 249 | ########################### 250 | 251 | # 前回ランドマークをすべて消す 252 | for key in prevLandmarkKeys: 253 | del X_.landmarks[key] 254 | 255 | self.count+=1 256 | 257 | return X_, weight 258 | 259 | 260 | def resampling(self, X, W, M): 261 | """ Resampling 262 | - 等間隔サンプリング 263 | M : パーティクルの数 num of particles 264 | """ 265 | X_resampled = [] 266 | Minv = 1.0/float(M) 267 | r = np.random.rand() * Minv 268 | c = W[0] 269 | i = 0 270 | for m in xrange(M): 271 | U = r + m * Minv 272 | while U > c: 273 | i += 1 274 | c += W[i] 275 | X_resampled.append(copy.deepcopy(X[i])) 276 | 277 | return X_resampled 278 | 279 | 280 | def reduce_particle_variance(self, X, X1, dt_camera): 281 | """ 282 | This method is called when No-resampling = True. 283 | Reduce particle variance to avoid divergence of particles. 284 | """ 285 | 286 | x = [] 287 | o = [] 288 | # Calc average 289 | for X_ in X: 290 | x.append(X_.x) 291 | o.append(X_.o) 292 | average = np.mean(x, axis=0) 293 | average_o = np.mean(o, axis=0) 294 | 295 | # Reduce variance 296 | for X_ in X: 297 | difference = X_.x - average 298 | X_.x = average + difference * 0.25 299 | X_.v = (X_.x - X1.x)/dt_camera 300 | 301 | #difference_o = X_.o - average_o 302 | X_.o = average_o 303 | 304 | return X 305 | 306 | 307 | def pf_step_camera_firsttime(self, X, dt, keypoints, step, P, M): 308 | """ One Step of Sampling Importance Resampling for Particle Filter 309 | for IMU sensor 310 | Parameters 311 | ---------- 312 | X : 状態 List of state set 313 | dt : 時刻の差分 delta of time 314 | keypoints : 特徴点 keypoints 315 | step : 現在のステップ数 current step 316 | P : デバイス位置の分散共分散行列 Variance-covariance matrix of position 317 | M : パーティクルの数 num of particles 318 | Returns 319 | ------- 320 | X_resampled : 次の状態 List updated state 321 | """ 322 | 323 | 324 | # 推定と更新 prediction and update 325 | self.count=0 326 | 327 | X_predicted = range(M) 328 | weight = range(M) 329 | 330 | dt2 = 0.5*dt*dt 331 | 332 | for i in xrange(M): 333 | X_predicted[i], weight[i] = self.predictionAndUpdateOneParticle_firsttime(X[i], dt, dt2, keypoints, step, P) 334 | 335 | return X_predicted 336 | 337 | 338 | def pf_step_camera(self, X, dt, keypoints, step, P, M, X1, P1, dt_camera, gyro): 339 | """ One Step of Sampling Importance Resampling for Particle Filter 340 | for IMU sensor 341 | Parameters 342 | ---------- 343 | X : 状態 List of state set 344 | dt : 時刻の差分 delta of time 345 | keypoints : 特徴点 keypoints 346 | step : 現在のステップ数 current step 347 | P : デバイス位置の分散共分散行列 Variance-covariance matrix of position 348 | M : パーティクルの数 num of particles 349 | Returns 350 | ------- 351 | X_resampled : 次の状態 List updated state 352 | """ 353 | 354 | # 初期化 init 355 | self.count = 0 356 | no_resampling = False 357 | 358 | weight = range(M) 359 | log_likelihood = range(M) 360 | log_likelihood_max = 0.0 361 | X_resampled = range(M) 362 | X_predicted = range(M) 363 | 364 | dt2 = 0.5*dt*dt 365 | 366 | # 角度のノイズ　noise of orientation 367 | noise_o = gyro[2] * 0.1 368 | 369 | if(noise_o < 0.000001): 370 | noise_o = 0.000001 371 | 372 | # 推定と対数尤度計算 prediction and calc log likelihood 373 | for i in xrange(M): 374 | X_predicted[i], log_likelihood[i] = self.predictionAndUpdateOneParticle(X[i], dt, dt2, np.random.normal(0, self.noise_x_sys, 3), keypoints, step, P, X1, P1, dt_camera, np.array([0.0, 0.0, np.random.normal(0, noise_o)])) 375 | # 一番大きい対数尤度を記憶しておく 376 | if(i == 0): 377 | log_likelihood_max = log_likelihood[i] 378 | else: 379 | if(log_likelihood[i] > log_likelihood_max): 380 | log_likelihood_max = log_likelihood[i] 381 | 382 | # 対数尤度から重みを算出 log likelihood -> weight 383 | for i in xrange(M): 384 | weight[i] = np.exp(log_likelihood[i] - log_likelihood_max) 385 | 386 | # 正規化 normalization of weight 387 | try: 388 | weight_sum = sum(weight) # 総和 the sum of weights 389 | if(weight_sum > 1.01): 390 | # 重みの総和が大きい（尤度が高い）場合 likelihood is high enough 391 | #print("weight_sum "+str(weight_sum)) ######################### 392 | # 正規化 normalization of weight 393 | weight = [(w/weight_sum) for w in weight] 394 | #for i in xrange(M): 395 | # weight[i] /= weight_sum 396 | # リサンプリング re-sampling 397 | X_resampled = self.resampling(X_predicted, weight, M) 398 | else: 399 | # 重みの総和が小さい（尤度が低い）場合 likelihood is low 400 | #print("weight_sum "+str(weight_sum)), ######################### 401 | #print("************************************") 402 | # リサンプリングを行わない No re-sampling 403 | no_resampling = True 404 | except: 405 | print("Error on normalization of weight") 406 | # リサンプリングを行わない No re-sampling 407 | no_resampling = True 408 | 409 | if(no_resampling): 410 | #X_resampled = copy.deepcopy(X_predicted) 411 | X_resampled = self.reduce_particle_variance(X_predicted, X1, dt_camera) 412 | 413 | return X_resampled 414 | 415 | 416 | def pf_step_IMU(self, X, dt, accel, ori, M, isFirstTimeCamera): 417 | """ One Step of Sampling Importance Resampling for Particle Filter 418 | for IMU sensor 419 | Parameters 420 | ---------- 421 | X : 状態 List of state set 422 | dt: 時刻の差分 delta of time 423 | accel : 制御 (加速度) Control (accel) 424 | ori : 制御 (姿勢) Control (orientation) 425 | M : パーティクルの数 num of particles 426 | Returns 427 | ------- 428 | X_resampled : 次の状態 List updated state 429 | """ 430 | 431 | #X_predicted = range(M) 432 | dt2 = 0.5*dt*dt 433 | 434 | # noise 435 | length = np.linalg.norm(accel) * self.noise_x_sys_coefficient 436 | 437 | if(isFirstTimeCamera): 438 | length *= 0.1 439 | 440 | if(length < 0.0000001): 441 | length = 0.0000001 442 | 443 | """ 444 | direction = accel 445 | theta = math.atan2(direction[1],direction[0]) 446 | phi = math.atan2(direction[2],math.hypot(direction[0],direction[1])) 447 | rotZ = Util.rotationMatrixZ(theta) 448 | rotY = Util.rotationMatrixY(-phi) 449 | rot = rotZ.dot(rotY) 450 | """ 451 | 452 | return [self.f_IMU(Xi, dt, dt2, accel, ori, np.array([np.random.normal(0, length), np.random.normal(0, (length)), np.random.normal(0, (length))])) for Xi in X] 453 | 454 | #return [self.f_IMU(Xi, dt, dt2, accel, ori, isMoving, rot.dot(np.array([np.random.normal(0, length), np.random.normal(0, (length*0.4)), np.random.normal(0, (length*0.4))]))) for Xi in X] 455 | 456 | #return [self.f_IMU(Xi, dt, dt2, accel, ori, isMoving, np.random.normal(0, self.noise_x_sys, 3)) for Xi in X] -------------------------------------------------------------------------------- /particle_filter_coplanarity.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Particle Filter 4 | (Coplanarity model version) 5 | """ 6 | 7 | import numpy as np 8 | import copy 9 | import math 10 | import datetime 11 | import Util 12 | 13 | class ParticleFilterCoplanarity: 14 | 15 | def __init__(self): 16 | pass 17 | 18 | def setFocus(self, f_): 19 | self.focus = f_ 20 | 21 | def setParameter(self, param1, param2): 22 | self.noise_x_sys = param1 # system noise of position　位置のシステムノイズ 23 | self.noise_coplanarity = param2 # observation noise of coplanarity 共面条件の観測ノイズ 24 | 25 | def f(self, dt, dt2, dt_camera, X, X1): 26 | """ Transition model 27 | - 状態方程式 28 | x_t = f(x_t-1) + w 29 | w ~ N(0, sigma) 30 | """ 31 | 32 | X_new = copy.deepcopy(X) 33 | 34 | # Transition with noise 35 | #w_mean = np.zeros(3) # mean of noise 36 | #w_cov_x = np.eye(3) * self.noise_x_sys # covariance matrix of noise (accel) 37 | #w_x = np.random.multivariate_normal(w_mean, w_cov_x) # generate random 38 | w_x = np.random.normal(0, self.noise_x_sys, 3) 39 | 40 | X_new.x = X.x + dt*X.v + dt2*X.a + w_x 41 | X_new.v = (X_new.x - X1.x)/dt_camera 42 | X_new.a = X.a 43 | X_new.o = X.o 44 | 45 | return X_new 46 | 47 | 48 | def likelihood(self, keypointPairs, X, X1): 49 | """ Likelihood function 50 | - 尤度関数 51 | p(y|x) ~ exp(-|coplanarity|^2 / 2*sigma^2) 52 | Parameters 53 | ---------- 54 | keypointPairs : 特徴点ペア pairs of keyponts between t-1 frame and t frame 55 | X : 予測　Predicted particle 56 | X1 : t-1の状態　particle at time t-1 57 | Returns 58 | ------- 59 | likelihood : 尤度 Likelihood 60 | """ 61 | 62 | weight = 0.0 # weight (return value) 63 | weights = [] 64 | 65 | #total_keypoints = len(keypointPairs) 66 | 67 | # Generate coplanarity matrix and calc determinant. 68 | # | x-x1 y-y1 z-z1 | -> xyz 69 | # | u1 v1 w1 | -> uvw1 70 | # | u2 v2 w2 | -> uvw2 71 | 72 | #coplanarity_determinant_square_total = 0.0 73 | xyz = np.array([X.x[0] - X1.x[0], X.x[1] - X1.x[1], X.x[2] - X1.x[2]]) 74 | for KP in keypointPairs: 75 | # Generate uvw1 (time:t-1) 76 | uvf1 = np.array([KP.x1, -KP.y1, -self.focus]) # Camera coordinates -> Device coordinates 77 | rotX = Util.rotationMatrixX(X1.o[0]) 78 | rotY = Util.rotationMatrixY(X1.o[1]) 79 | rotZ = Util.rotationMatrixZ(X1.o[2]) 80 | # uvw1 = R(z)R(y)R(x)uvf1 81 | uvw1 = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf1))) 82 | uvw1 /= 100.0 # Adjust scale to decrease calculation error. This doesn't have an influence to estimation. 83 | # Generate uvw2 (time:t) 84 | uvf2 = np.array([KP.x2, -KP.y2, -self.focus]) # Camera coordinates -> Device coordinates 85 | rotX = Util.rotationMatrixX(X.o[0]) 86 | rotY = Util.rotationMatrixY(X.o[1]) 87 | rotZ = Util.rotationMatrixZ(X.o[2]) 88 | # uvw2 = R(z)R(y)R(x)uvf2 89 | uvw2 = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf2))) 90 | uvw2 /= 100.0 # Adjust scale to decrease calculation error. This doesn't have an influence to estimation. 91 | # Generate coplanarity matrix 92 | coplanarity_matrix = np.array([xyz,uvw1,uvw2]) 93 | # Calc determinant 94 | determinant = np.linalg.det(coplanarity_matrix) 95 | # Weight 96 | w = (1.0 / (math.sqrt( 2.0 * math.pi * self.noise_coplanarity**2 ))) * np.exp((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) ) 97 | weights.append(w) 98 | # Add 99 | #coplanarity_determinant_square_total += (determinant**2) 100 | 101 | 102 | for i,w in enumerate(weights): 103 | if(i==0): 104 | weight = w 105 | else: 106 | weight *= w 107 | 108 | 109 | #weight = (1.0 / (math.sqrt( 2.0 * math.pi * (self.noise_coplanarity*total_keypoints)**2 ))) * np.exp(coplanarity_determinant_square_total / (-2.0 * ((self.noise_coplanarity*total_keypoints)**2)) ) 110 | 111 | ########################################### 112 | #print("weight"), 113 | #print(weight) 114 | ########################################### 115 | 116 | # return likelihood 117 | return weight 118 | 119 | 120 | def resampling(self, X, W, M): 121 | """ Resampling 122 | - 等間隔サンプリング 123 | W : 重み weights 124 | M : パーティクルの数 num of particles 125 | """ 126 | X_resampled = [] 127 | Minv = 1/float(M) 128 | r = np.random.rand() * Minv 129 | c = W[0] 130 | i = 0 131 | for m in range(M): 132 | U = r + m * Minv 133 | while U > c: 134 | i += 1 135 | c += W[i] 136 | X_resampled.append(X[i]) 137 | return X_resampled 138 | 139 | 140 | def pf_step(self, X, X1, dt, dt_camera, keypointPairs, M): 141 | """ One Step of Sampling Importance Resampling for Particle Filter 142 | for IMU sensor 143 | Parameters 144 | ---------- 145 | X : 状態 List of state set 146 | X1 : 前回の状態 state at time t-1 147 | dt : 時刻の差分 delta of time 148 | keypointPairs : 特徴点ペア pairs of keypoints between t-1 frame and t frame 149 | M : パーティクルの数 num of particles 150 | Returns 151 | ------- 152 | X_resampled : 次の状態 List updated state 153 | """ 154 | 155 | # 初期化 init 156 | X_predicted = range(M) 157 | weight = range(M) 158 | 159 | dt2 = 0.5 * dt * dt 160 | 161 | for i in range(M): 162 | # 推定 prediction 163 | X_predicted[i] = self.f(dt, dt2, dt_camera, X[i], X1) 164 | # 更新 update 165 | weight[i] = self.likelihood(keypointPairs, X_predicted[i], X1) 166 | 167 | # 正規化 normalization of weight 168 | weight_sum = sum(weight) # 総和 the sum of weights 169 | if(weight_sum > 0.0): 170 | # 重みの総和が大きい（尤度が高い）場合 likelihood is high enough 171 | print("weight_sum "+str(weight_sum)) 172 | # 正規化 normalization of weight 173 | for i in range(M): 174 | weight[i] /= weight_sum 175 | # リサンプリング re-sampling 176 | X_resampled = self.resampling(X_predicted, weight, M) 177 | else: 178 | # 重みの総和が小さい（尤度が低い）場合 likelihood is low 179 | print("weight_sum "+str(weight_sum)), 180 | print("***") 181 | # リサンプリングを行わない No re-sampling 182 | X_resampled = copy.deepcopy(X_predicted) 183 | 184 | return X_resampled 185 | -------------------------------------------------------------------------------- /particle_filter_normal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Particle Filter 4 | (IMU with PF model version) 5 | """ 6 | 7 | import numpy 8 | import math 9 | 10 | class ParticleFilterNormal: 11 | 12 | def __init__(self): 13 | pass 14 | 15 | 16 | def f(self, dt, x, u): 17 | """ Transition model 18 | - 状態方程式 19 | x_t = f(x_t-1, u) + w 20 | w ~ N(0, sigma) 21 | """ 22 | 23 | return x 24 | 25 | 26 | def likelihood(self, y, x): 27 | """ Likelihood function 28 | - 尤度関数 29 | p(y|x) ~ exp(-1/2 * (|y-h(x)|.t * sigma * |y-h(x)|) 30 | - 観測モデル 31 | z = h(x) + v 32 | v ~ N(0, sigma) 33 | Parameters 34 | ---------- 35 | y : 観測 Observation 36 | x : 予測　Predicted particle 37 | Returns 38 | ------- 39 | likelihood : 尤度 Likelihood 40 | """ 41 | 42 | return 1.0 43 | 44 | 45 | def resampling(self, X, W, M): 46 | """ Resampling 47 | - 等間隔サンプリング 48 | M : パーティクルの数 num of particles 49 | """ 50 | X_resampled = [] 51 | Minv = 1/float(M) 52 | r = numpy.random.rand() * Minv 53 | c = W[0] 54 | i = 0 55 | for m in range(M): 56 | U = r + m * Minv 57 | while U > c: 58 | i += 1 59 | c += W[i] 60 | X_resampled.append(X[i]) 61 | return X_resampled 62 | 63 | 64 | def pf_step(self, X, u, y, N): 65 | """One Step of Sampling Importance Resampling for Particle Filter 66 | Parameters 67 | ---------- 68 | X : array of [float|array] 69 | 状態 List of state set 70 | u : float or array 71 | 制御入力 Control input 72 | y : float or array 73 | 観測 Observation set 74 | N : int 75 | パーティクルの数 num of particles 76 | Returns 77 | ------- 78 | X_resampled : array of [float|array] 79 | 次の状態 List updated state 80 | """ 81 | 82 | # 初期化 83 | X_predicted = range(N) 84 | weight = range(N) 85 | 86 | for i in range(N): 87 | # 推定 prediction 88 | X_predicted[i] = self.f(X[i], u) 89 | # 更新 update 90 | weight[i] = self.likelihood(y, X_predicted[i]) 91 | # 正規化 normalization 92 | weight_sum = sum(weight) # 総和 the sum of weights 93 | for i in range(N): 94 | weight[i] /= weight_sum 95 | # リサンプリング re-sampling (if necessary) 96 | X_resampled = self.resampling(X_predicted, weight, N) 97 | 98 | return X_resampled 99 | -------------------------------------------------------------------------------- /sensor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | sensor.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | This class is called from "Main.py", and process sensor data. 9 | 10 | """ 11 | 12 | from math import * 13 | import numpy as np 14 | import Util 15 | import KF 16 | 17 | class Sensor: 18 | 19 | def __init__(self,_state): 20 | #state.py 21 | self.state = _state 22 | self.init() 23 | 24 | 25 | def init(self): 26 | #variables 27 | self.isFirstTime = True 28 | self.time = 0.0 29 | self.time1 = 0.0 30 | self.accel = np.array([]) 31 | self.accel_g = np.array([]) 32 | self.gravity = np.array([]) # 33 | self.magnet = np.array([]) 34 | self.magnet_fixed = np.array([]) 35 | self.gyro = np.array([]) 36 | self.gyro_diff = np.array([]) 37 | self.orientation = np.array([0.0,0.0,0.0]) 38 | self.orientation_g = np.array([0.0,0.0,0.0]) 39 | self.orientation_gyro = np.array([0.0,0.0,0.0]) 40 | self.rotX_ = np.identity(3) 41 | self.rotY_ = np.identity(3) 42 | self.rotX = np.identity(3) 43 | self.rotY = np.identity(3) 44 | self.rotZ = np.identity(3) 45 | self.rot = np.identity(3) 46 | self.I = np.identity(3) 47 | self.P = np.array([0.0,0.0,0.0]) # covariance matrix of KF for orientation 48 | self.Q = np.diag([0.1,0.1,0.1]) # noise of KF for orientation 49 | self.R = np.diag([0.01,0.01,0.01]) # noise of KF for orientation 50 | self.centrifugal = np.array([0.0,0.0,0.0]) # 51 | self.tangential = np.array([0.0,0.0,0.0]) # 52 | self.r = np.array([0.0,0.0,0.0]) 53 | self.v = np.array([0.0,0.0,0.0]) 54 | self.v1 = np.array([0.0,0.0,0.0]) 55 | 56 | 57 | 58 | #Set new data and Execute all functions 59 | def processData(self,data): 60 | 61 | self.time1 = self.time 62 | self.time = (float(long(data[0]) / 1000.0)) 63 | 64 | self.accel = np.array([float(data[1]),float(data[2]),float(data[3])]) 65 | self.orientation_g = np.array([float(data[4]),float(data[5]),float(data[6])]) 66 | self.magnet = np.array([float(data[7]),float(data[8]),float(data[9])]) 67 | self.gyro = np.array([float(data[10]),float(data[11]),float(data[12])]) 68 | #self.gyro_diff = np.array([float(data[13]),float(data[14]),float(data[15])]) 69 | 70 | #self.calcOrientation() 71 | #self.calcRotationMatrix() 72 | #self.calcGlobalAcceleration() 73 | self.pushDataToState() 74 | 75 | if(self.isFirstTime): 76 | self.isFirstTime = False 77 | 78 | 79 | #Calc orientation 80 | def calcOrientation(self): 81 | self.calcOrientationByGravity() 82 | if(self.isFirstTime): 83 | self.orientation = self.orientation_g 84 | else: 85 | t = self.time - self.time1 86 | matrixGyro2Euler = Util.matrixGyro2Euler(self.orientation[0],self.orientation[1]) * t 87 | 88 | #Kalman Filter 89 | resultKF = KF.execKF1(self.orientation_g, self.gyro, self.orientation, self.P, self.I, matrixGyro2Euler, self.I, self.Q, self.R) 90 | self.orientation = resultKF[0] 91 | self.P = resultKF[1] 92 | 93 | if(self.orientation[0]>=pi): 94 | self.orientation[0] -= pi*2.0 95 | if(self.orientation[1]>=pi): 96 | self.orientation[1] -= pi*2.0 97 | if(self.orientation[2]>=pi): 98 | self.orientation[2] -= pi*2.0 99 | if(self.orientation[0]<-pi): 100 | self.orientation[0] += pi*2.0 101 | if(self.orientation[1]<-pi): 102 | self.orientation[1] += pi*2.0 103 | if(self.orientation[2]<-pi): 104 | self.orientation[2] += pi*2.0 105 | 106 | 107 | 108 | #Calc orientation by using gyro 109 | def calcOrientationByGyro(self): 110 | if(self.isFirstTime): 111 | self.orientation_gyro = self.orientation_g 112 | else: 113 | t = self.time - self.time1 114 | gyroEuler = np.dot(Util.matrixGyro2Euler(self.orientation_gyro[0],self.orientation_gyro[1]),self.gyro) 115 | self.orientation_gyro = self.orientation_gyro + gyroEuler * t 116 | 117 | if(self.orientation_gyro[0]>=pi): 118 | self.orientation_gyro[0] -= pi*2.0 119 | if(self.orientation_gyro[1]>=pi): 120 | self.orientation_gyro[1] -= pi*2.0 121 | if(self.orientation_gyro[2]>=pi): 122 | self.orientation_gyro[2] -= pi*2.0 123 | if(self.orientation_gyro[0]<-pi): 124 | self.orientation_gyro[0] += pi*2.0 125 | if(self.orientation_gyro[1]<-pi): 126 | self.orientation_gyro[1] += pi*2.0 127 | if(self.orientation_gyro[2]<-pi): 128 | self.orientation_gyro[2] += pi*2.0 129 | 130 | 131 | #Calc orientation by using gravity and magnet 132 | #return orientation 133 | #see also "Studies on Orientation Measurement in Sports Using Inertial and Magnetic Field Sensors" 134 | # https://www.jstage.jst.go.jp/article/sposun/22/2/22_255/_pdf 135 | def calcOrientationByGravity(self): 136 | #x roll 137 | self.orientation_g[0] = atan2(self.gravity[1],self.gravity[2]) 138 | #y pitch (-90 ～ +90) 139 | self.orientation_g[1] = atan2(-self.gravity[0],hypot(self.gravity[1],self.gravity[2])) 140 | #y pitch (-180 ～ +180) 141 | """ 142 | if(self.gravity[2]<0): #decided by z axis 143 | self.orientation_g[1] = atan2(-self.gravity[0],-hypot(self.gravity[1],self.gravity[2])) 144 | else: 145 | self.orientation_g[1] = atan2(-self.gravity[0],hypot(self.gravity[1],self.gravity[2])) 146 | """ 147 | #z yaw 148 | self.rotX_ = Util.rotationMatrixX(self.orientation_g[0]) 149 | self.rotY_ = Util.rotationMatrixY(self.orientation_g[1]) 150 | self.magnet_fixed = np.dot(np.dot(self.rotY_,self.rotX_),self.magnet) 151 | self.orientation_g[2] = atan2(-self.magnet_fixed[1],self.magnet_fixed[0]) 152 | 153 | 154 | #Calc rotation matrix from orientation 155 | def calcRotationMatrix(self): 156 | #Rotation matrix R(Z)R(Y)R(X) 157 | self.rotX = Util.rotationMatrixX(self.orientation[0]) 158 | self.rotY = Util.rotationMatrixY(self.orientation[1]) 159 | self.rotZ = Util.rotationMatrixZ(self.orientation[2]) 160 | self.rot = np.dot(self.rotZ,np.dot(self.rotY,self.rotX)) 161 | 162 | 163 | #Remove Centrifugal Accel 164 | def removeCentrifugalAndTangentialAccel(self): 165 | #Angular velocity 166 | w = self.gyro 167 | #Angular acceleration 168 | #wa = self.gyro_diff 169 | #wn2 (norm of gyro vector)^2 170 | wn2 = pow(np.linalg.norm(w),2) 171 | #norm of global v 172 | vn = np.linalg.norm(self.state.v) 173 | #centrifugal 174 | centrifugal_x = np.array([0.0,0.0,0.0]) 175 | centrifugal_y = np.array([0.0,0.0,0.0]) 176 | if(w[0] > 0.3 or w[0] < -0.3): 177 | #v of x 178 | vx = np.array([0.0,vn*sin(self.orientation[0]),vn*cos(self.orientation[0])]) 179 | #w of x 180 | wx = np.array([w[0],0.0,0.0]) 181 | #w*(w*r) of x 182 | centrifugal_x = np.cross(wx,np.cross(wx,np.cross(vx,wx)/wn2)) 183 | if(w[1] > 0.3 or w[1] < -0.3): 184 | #v of y 185 | vy = np.array([-vn*sin(self.orientation[1]),0.0,vn*cos(self.orientation[1])]) 186 | #w of y 187 | wy = np.array([0.0,w[1],0.0]) 188 | #w*(w*r) of y 189 | centrifugal_y = np.cross(wy,np.cross(wy,np.cross(vy,wy)/wn2)) 190 | #a = a - w*(w*r) 191 | self.centrifugal = centrifugal_x + centrifugal_y 192 | self.accel = self.accel - self.centrifugal 193 | 194 | 195 | 196 | #Calc accel in global coordinates by using orientation 197 | def calcGlobalAcceleration(self): 198 | #accel in global = R(Z)R(Y)R(X) * accel 199 | self.accel_g = np.dot(self.rot,self.accel) 200 | 201 | 202 | #Push all data to State class 203 | def pushDataToState(self): 204 | self.state.setSensorData(self.time, self.accel, self.orientation_g, self.gyro) 205 | 206 | -------------------------------------------------------------------------------- /state.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | state.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | This class is called from "sensor.py" and "image.py", and estimate state variables. 9 | This class is factory class. Model (state type & estimation model) is selected by argment. 10 | 11 | """ 12 | 13 | from state_coplanarity import StateCoplanarity 14 | from state_RBPF import StateRBPF 15 | from state_IMU_KF import StateIMUKF 16 | from state_IMU_PF import StateIMUPF 17 | 18 | class State: 19 | 20 | def __init__(self): 21 | pass 22 | 23 | def getStateClass(self,stateType): 24 | if(stateType=="Coplanarity"): 25 | return StateCoplanarity() 26 | elif(stateType=="RBPF"): 27 | return StateRBPF() 28 | elif(stateType=="IMUKF"): 29 | return StateIMUKF() 30 | elif(stateType=="IMUPF"): 31 | state = StateIMUPF() 32 | state.initWithType("IMUPF") 33 | return state 34 | elif(stateType=="IMUPF2"): 35 | state = StateIMUPF() 36 | state.initWithType("IMUPF2") 37 | return state 38 | else: 39 | pass 40 | -------------------------------------------------------------------------------- /state_IMU_KF.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | state_IMU_KF.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | State and estimation model of IMU with Kalman Filter. 9 | 10 | This class is generated from "state.py". 11 | 12 | """ 13 | 14 | import sys 15 | import math 16 | import time 17 | import copy 18 | import cv2 as cv 19 | import numpy as np 20 | import KF 21 | import Util 22 | 23 | class StateIMUKF: 24 | 25 | def __init__(self): 26 | self.init() 27 | 28 | 29 | def init(self): 30 | self.isFirstTime = True 31 | self.t = 0 32 | self.t1 = 0 33 | self.accel1 = np.array([0.0, 0.0, 0.0]) 34 | self.accel2 = np.array([0.0, 0.0, 0.0]) 35 | self.accel3 = np.array([0.0, 0.0, 0.0]) 36 | 37 | self.initKalmanFilter() 38 | 39 | 40 | def initKalmanFilter(self): 41 | self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]) 42 | self.sigma = np.zeros([12,12]) 43 | self.A = np.identity(12) 44 | self.C = np.array([ [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0], 45 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0], 46 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0], 47 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0], 48 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0], 49 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]]) 50 | self.Q = np.diag([0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1]) # sys noise 51 | self.R = np.diag([0.01,0.01,0.01,0.01,0.01,0.01]) # obs noise 52 | 53 | 54 | 55 | """ 56 | This method is called from "sensor.py" when new IMU sensor data are arrived. 57 | time : time (sec) 58 | accel : acceleration in global coordinates 59 | ori : orientaion 60 | """ 61 | def setSensorData(self, time_, accel, ori, gyro): 62 | 63 | self.t1 = self.t 64 | self.t = time_ 65 | 66 | if(self.isFirstTime): 67 | #init mu 68 | self.mu = np.array([0.0,0.0,0.0, 69 | 0.0,0.0,0.0, 70 | accel[0],accel[1],accel[2], 71 | ori[0],ori[1],ori[2]]) 72 | else: 73 | 74 | #start_time = time.clock() 75 | #observation 76 | Y = np.array([accel[0],accel[1],accel[2], 77 | ori[0],ori[1],ori[2]]) 78 | dt = self.t - self.t1 79 | dt2 = 0.5 * dt * dt 80 | self.A = np.array([[1.0,0.0,0.0,dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0,0.0], 81 | [0.0,1.0,0.0,0.0,dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0], 82 | [0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,dt2,0.0,0.0,0.0], 83 | [0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0], 84 | [0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0], 85 | [0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0], 86 | [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0], 87 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0], 88 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0], 89 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0], 90 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0], 91 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]]) 92 | 93 | """ 94 | self.accel3 = copy.deepcopy(self.accel2) 95 | self.accel2 = copy.deepcopy(self.accel1) 96 | self.accel1 = copy.deepcopy(accel) 97 | if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False): 98 | self.mu[3] = 0.0 99 | if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False): 100 | self.mu[4] = 0.0 101 | if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False): 102 | self.mu[5] = 0.0 103 | """ 104 | self.mu, self.sigma = KF.execKF1Simple(Y,self.mu,self.sigma,self.A,self.C,self.Q,self.R) 105 | 106 | #end_time = time.clock() 107 | #print "%f" %(end_time-start_time) 108 | 109 | if(self.isFirstTime): 110 | self.isFirstTime = False 111 | 112 | 113 | 114 | """ 115 | This method is called from "Main.py" 116 | return estimated state vector 117 | """ 118 | def getState(self): 119 | x = np.array([self.mu[0],self.mu[1],self.mu[2]]) 120 | v = np.array([self.mu[3],self.mu[4],self.mu[5]]) 121 | a = np.array([self.mu[6],self.mu[7],self.mu[8]]) 122 | o = np.array([self.mu[9],self.mu[10],self.mu[11]]) 123 | return x,v,a,o 124 | 125 | -------------------------------------------------------------------------------- /state_IMU_PF.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | state_IMU_PF.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | State and estimation model of IMU with Particle Filter. 9 | 10 | This class is generated from "state.py". 11 | 12 | """ 13 | 14 | import sys 15 | import math 16 | import cv2 as cv 17 | import numpy as np 18 | from particle_filter import ParticleFilter 19 | from particle import Particle 20 | 21 | class StateIMUPF: 22 | 23 | def __init__(self): 24 | pass 25 | 26 | def initWithType(self,PFtype_): 27 | self.PFtype = PFtype_ 28 | self.init() 29 | 30 | def init(self): 31 | self.isFirstTime = True 32 | self.t = 0.0 33 | self.t1 = 0.0 34 | 35 | self.initParticleFilter(self.PFtype) 36 | 37 | 38 | def initParticleFilter(self,PFtype): 39 | self.pf = ParticleFilter().getParticleFilterClass(PFtype) #PFtype = "IMUPF" or "IMUPF2" 40 | self.pf.setParameter(0.01 ,0.001) #パーティクルフィルタのパラメータ（ノイズの分散） variance of noise 41 | self.M = 100 # パーティクルの数 num of particles 42 | self.X = [] # パーティクルセット set of particles 43 | self.loglikelihood = 0.0 44 | self.count = 0 45 | 46 | 47 | def initParticle(self, accel, ori): 48 | X = [] 49 | particle = Particle() 50 | particle.initWithIMU(accel, ori) 51 | for i in range(self.M): 52 | X.append(particle) 53 | return X 54 | 55 | 56 | 57 | """ 58 | This method is called from "sensor.py" when new IMU sensor data are arrived. 59 | time : time (sec) 60 | accel : acceleration in global coordinates 61 | ori : orientaion 62 | """ 63 | def setSensorData(self, time, accel, ori): 64 | 65 | self.t1 = self.t 66 | self.t = time 67 | 68 | if(self.isFirstTime): 69 | # init particle 70 | self.X = self.initParticle(accel, ori) 71 | else: 72 | # exec particle filter 73 | self.X = self.pf.pf_step(self.X, self.t - self.t1, accel, ori, self.M) 74 | """ The code below is used to get loglikelihood to decide parameters. 75 | self.X, likelihood = self.pf.pf_step(self.X, self.t - self.t1, accel, ori, self.M) 76 | self.loglikelihood += math.log(likelihood) 77 | self.count += 1 78 | if(self.count==300): 79 | print(str(self.loglikelihood)) 80 | """ 81 | 82 | if(self.isFirstTime): 83 | self.isFirstTime = False 84 | 85 | 86 | """ 87 | This method is called from "Main.py" 88 | return estimated state vector 89 | """ 90 | def getState(self): 91 | x = [] 92 | v = [] 93 | a = [] 94 | o = [] 95 | for X_ in self.X: 96 | x.append(X_.x) 97 | v.append(X_.v) 98 | a.append(X_.a) 99 | o.append(X_.o) 100 | return np.mean(x, axis=0),np.mean(v, axis=0),np.mean(a, axis=0),np.mean(o, axis=0) 101 | 102 | -------------------------------------------------------------------------------- /state_RBPF.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | state_RBPF.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | State and estimation model of Rao-Blackwellized Particle Filter. 9 | 10 | This class is generated from "state.py". 11 | 12 | """ 13 | 14 | import sys 15 | import math 16 | import time 17 | import copy 18 | import datetime 19 | import cv2 as cv 20 | import numpy as np 21 | from particle_filter import ParticleFilter 22 | from particle import Particle 23 | import Util 24 | 25 | class StateRBPF: 26 | 27 | def __init__(self): 28 | # ----- Set parameters here! ----- # 29 | self.M = 200 # total number of particles パーティクルの数 30 | self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px] 31 | #self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px] 32 | # Particle Filter 33 | self.noise_x_sys = 0.001 # system noise of position (SD)　位置のシステムノイズ（標準偏差） 34 | self.noise_x_sys_coefficient = 0.05 # system noise of position (coefficient)　位置のシステムノイズ（係数） 35 | self.noise_a_sys = 0.1 # system noise of acceleration (SD)　加速度のシステムノイズ（標準偏差） 36 | self.noise_g_sys = 0.01 # system noise of orientation (SD)　角度のシステムノイズ（標準偏差） 37 | self.noise_a_obs = 0.001 # observation noise of acceleration (SD)　加速度の観測ノイズ（標準偏差） 38 | self.noise_g_obs = 0.0001 # observation noise of orientation (SD)　角度の観測ノイズ（標準偏差） 39 | self.noise_camera = 5.0 # observation noise of camera (SD) カメラの観測ノイズ（標準偏差） 40 | self.noise_coplanarity = 0.1 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差） 41 | 42 | self.init() 43 | 44 | def init(self): 45 | self.isFirstTimeIMU = True 46 | self.isFirstTimeCamera = True 47 | self.lock = False 48 | 49 | self.t = 0.0 50 | self.t1 = 0.0 51 | self.t_camera = 0.0 52 | self.t1_camera = 0.0 53 | 54 | self.accel1 = np.array([0.0, 0.0, 0.0]) 55 | self.accel2 = np.array([0.0, 0.0, 0.0]) 56 | self.accel3 = np.array([0.0, 0.0, 0.0]) 57 | 58 | self.gyro = np.array([0.0, 0.0, 0.0]) 59 | 60 | self.P1 = np.identity(3) 61 | 62 | self.initParticleFilter() 63 | 64 | 65 | def initParticleFilter(self): 66 | self.pf = ParticleFilter().getParticleFilterClass("RBPF") 67 | self.pf.setFocus(self.f) 68 | self.pf.setParameter(self.noise_x_sys, self.noise_a_sys, self.noise_g_sys, self.noise_camera, self.noise_coplanarity, self.noise_x_sys_coefficient) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise) 69 | self.X = [] # パーティクルセット set of particles 70 | self.loglikelihood = 0.0 71 | self.count = 1 72 | self.step = 1 73 | 74 | 75 | def initParticle(self, accel, ori): 76 | X = [] 77 | for i in xrange(self.M): 78 | particle = Particle() 79 | particle.initWithIMU(accel, ori) 80 | X.append(particle) 81 | return X 82 | 83 | 84 | def setObservationModel(self, observation_): 85 | self.pf.setObservationModel(observation_) 86 | 87 | 88 | 89 | """ 90 | This method is called from "sensor.py" when new IMU sensor data are arrived. 91 | time : time (sec) 92 | accel : acceleration in global coordinates 93 | ori : orientaion 94 | """ 95 | def setSensorData(self, time_, accel, ori, gyro_): 96 | 97 | # If process is locked by Image Particle Filter, do nothing 98 | if(self.lock): 99 | print("locked") 100 | return 101 | 102 | # Get current time 103 | self.t1 = self.t 104 | self.t = time_ 105 | self.dt = self.t - self.t1 106 | 107 | self.gyro = gyro_ 108 | 109 | if(self.isFirstTimeIMU): 110 | # init particle 111 | self.X = self.initParticle(accel, ori) 112 | else: 113 | # exec particle filter 114 | self.X = self.pf.pf_step_IMU(self.X, self.dt, accel, ori, self.M, self.isFirstTimeCamera) 115 | 116 | if(self.isFirstTimeIMU): 117 | self.isFirstTimeIMU = False 118 | 119 | # Count 120 | self.count+=1 121 | 122 | 123 | """ 124 | This method is called from Image class when new camera image data are arrived. 125 | time_ : time (sec) 126 | keypointPairs : list of KeyPointPair class objects 127 | """ 128 | def setImageData(self, time_, keypoints): 129 | 130 | # If IMU data has not been arrived yet, do nothing 131 | if(self.isFirstTimeIMU): 132 | return 133 | 134 | ######################## 135 | #print("=================") 136 | #print("step "+str(self.step)+" count "+str(self.count)) 137 | ########################### 138 | 139 | if(keypoints == "nomatch"): 140 | print("nomatch ***********************") 141 | self.reduce_particle_variance(self.X) 142 | self.count += 1 143 | self.step += 1 144 | return 145 | 146 | # Lock IMU process 147 | self.lock = True 148 | 149 | #start_time = time.clock() 150 | 151 | # Get current time 152 | self.t1 = self.t 153 | self.t = time_ 154 | self.dt = self.t - self.t1 155 | 156 | self.t1_camera = self.t_camera 157 | self.t_camera = time_ 158 | dt_camera = self.t_camera - self.t1_camera 159 | 160 | # covariance matrix of position 161 | P = self.createPositionCovarianceMatrixFromParticle(self.X) 162 | #P *= 0.01 163 | 164 | if(self.step > 0 and self.step < 10): 165 | #self.saveXYZasCSV(self.X,"1") 166 | pass 167 | 168 | if(self.isFirstTimeCamera): 169 | # exec particle filter 170 | self.X = self.pf.pf_step_camera_firsttime(self.X, self.dt, keypoints, self.step, P, self.M) 171 | else: 172 | # exec particle filter 173 | self.X = self.pf.pf_step_camera(self.X, self.dt, keypoints, self.step, P, self.M, self.X1, self.P1, dt_camera, self.gyro) 174 | 175 | if(self.step > 0 and self.step < 10): 176 | #self.saveXYZasCSV(self.X,"2") 177 | pass 178 | 179 | # Get prev position and orientation 180 | prevXx, prevXo = self.getPositionAndOrientation() 181 | self.X1 = Particle() 182 | self.X1.initWithPositionAndOrientation(prevXx, prevXo) 183 | 184 | self.P1 = P 185 | 186 | # Count 187 | self.count += 1 188 | 189 | # Step (camera only observation step) 190 | self.step += 1 191 | 192 | #end_time = time.clock() 193 | #print "%f" %(end_time-start_time) 194 | 195 | # Unlock IMU process 196 | self.lock = False 197 | 198 | if(self.isFirstTimeCamera): 199 | self.isFirstTimeCamera = False 200 | 201 | 202 | def reduce_particle_variance(self, X): 203 | """ 204 | This method is called when No-resampling = True. 205 | Reduce particle variance to avoid divergence of particles. 206 | """ 207 | 208 | x = [] 209 | # Calc average of position 210 | for X_ in X: 211 | x.append(X_.x) 212 | average = np.mean(x, axis=0) 213 | 214 | # Reduce variance of position 215 | for X_ in X: 216 | difference = X_.x - average 217 | X_.x = average + difference * 0.1 218 | 219 | return X 220 | 221 | 222 | """ 223 | print Landmark (X,Y,Z) 224 | """ 225 | def printLandmark(self,X): 226 | print("-----") 227 | landmarks = self.getLandmarkXYZ(X) 228 | for key, value in landmarks.iteritems(): 229 | print(str(key)+" "), 230 | print(value) 231 | 232 | 233 | """ 234 | return Landmark (X,Y,Z) 235 | """ 236 | def getLandmarkXYZ(self,X): 237 | allLandmarks = {} 238 | # calc sum of landmark XYZ 239 | for x in X: 240 | for landmarkId, landmark in x.landmarks.iteritems(): 241 | xyz = landmark.getXYZ() 242 | if(allLandmarks.has_key(landmarkId) == False): 243 | allLandmarks[landmarkId] = xyz 244 | else: 245 | allLandmarks[landmarkId] += xyz 246 | # calc average of landamrk XYZ 247 | for key, value in allLandmarks.iteritems(): 248 | value /= float(self.M) 249 | return allLandmarks 250 | 251 | 252 | """ 253 | print (X,Y,Z) of particles 254 | """ 255 | def printXYZ(self,X): 256 | print("-----") 257 | for x in X: 258 | x.printXYZ() 259 | 260 | 261 | 262 | """ 263 | save (X,Y,Z) of particles as CSV file 264 | """ 265 | def saveXYZasCSV(self,X,appendix): 266 | x = [] 267 | for X_ in X: 268 | x.append(X_.x) 269 | date = datetime.datetime.now() 270 | #datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000) 271 | #np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',') 272 | datestr = date.strftime("%Y%m%d_%H%M%S_") 273 | np.savetxt('./data/output/particle_'+datestr+str(self.count)+'_'+appendix+'.csv', x, delimiter=',') 274 | 275 | 276 | """ 277 | create covariance matrix of position from particle set 278 | """ 279 | def createPositionCovarianceMatrixFromParticle(self, X): 280 | x = [] 281 | for X_ in X: 282 | if(len(x)==0): 283 | x = X_.x 284 | else: 285 | x = np.vstack((x,X_.x)) 286 | P = np.cov(x.T) 287 | return P 288 | 289 | 290 | """ 291 | return estimated state vector of position and orientation 292 | """ 293 | def getPositionAndOrientation(self): 294 | x = [] 295 | o = [] 296 | for X_ in self.X: 297 | x.append(X_.x) 298 | o.append(X_.o) 299 | return np.mean(x, axis=0),np.mean(o, axis=0) 300 | 301 | 302 | """ 303 | This method is called from "Main.py" 304 | return estimated state vector 305 | """ 306 | def getState(self): 307 | x = [] 308 | v = [] 309 | a = [] 310 | o = [] 311 | for X_ in self.X: 312 | x.append(X_.x) 313 | v.append(X_.v) 314 | a.append(X_.a) 315 | o.append(X_.o) 316 | #print(np.var(x, axis=0)) 317 | return np.mean(x, axis=0),np.mean(v, axis=0),np.mean(a, axis=0),np.mean(o, axis=0) -------------------------------------------------------------------------------- /state_coplanarity.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | state_coplanarity.py 5 | 6 | author: Keita Nagara　永良慶太 (University of Tokyo) 7 | 8 | State and estimation model of Coplanarity (IMU with Kalman Filter & Camera with Particle Filter. Observation model is coplanarity. State vector is device state only). 9 | 10 | This class is generated from "state.py". 11 | 12 | """ 13 | 14 | import sys 15 | import time 16 | import copy 17 | import math 18 | import datetime 19 | import cv2 as cv 20 | import numpy as np 21 | import matplotlib.pyplot as plt 22 | from mpl_toolkits.mplot3d.axes3d import Axes3D 23 | import KF 24 | from particle_filter import ParticleFilter 25 | from particle import Particle 26 | import Util 27 | 28 | class StateCoplanarity: 29 | 30 | def __init__(self): 31 | 32 | # ----- Set parameters here! ----- # 33 | self.M = 512 # total number of particles パーティクルの数 34 | self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px] 35 | # Kalman Filter 36 | self.noise_x_sys = 0.015 # system noise of position (SD)　位置のシステムノイズ（標準偏差） 37 | self.noise_a_sys = 0.1 # system noise of acceleration (SD)　加速度のシステムノイズ（標準偏差） 38 | self.noise_g_sys = 0.01 # system noise of orientation (SD)　角度のシステムノイズ（標準偏差） 39 | self.noise_a_obs = 0.001 # observation noise of acceleration (SD)　加速度の観測ノイズ（標準偏差） 40 | self.noise_g_obs = 0.0001 # observation noise of orientation (SD)　角度の観測ノイズ（標準偏差） 41 | # Particle Filter 42 | self.PFnoise_coplanarity_obs = 0.01 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差） 43 | # ----- Set parameters here! ----- # 44 | 45 | self.init() 46 | 47 | 48 | def init(self): 49 | self.isFirstTimeIMU = True 50 | self.isFirstTimeCamera = True 51 | self.lock = False 52 | self.step = 1 53 | 54 | self.t = 0.0 55 | self.t1 = 0.0 56 | self.t_camera = 0.0 57 | self.t1_camera = 0.0 58 | 59 | self.accel1 = np.array([0.0, 0.0, 0.0]) 60 | self.accel2 = np.array([0.0, 0.0, 0.0]) 61 | self.accel3 = np.array([0.0, 0.0, 0.0]) 62 | 63 | self.initKalmanFilter() 64 | self.initParticleFilter() 65 | 66 | 67 | def initKalmanFilter(self): 68 | self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]) 69 | self.mu1 = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]) 70 | self.sigma = np.zeros([12,12]) 71 | self.C = np.array([ [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0], 72 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0], 73 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0], 74 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0], 75 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0], 76 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]]) 77 | self.Q = np.diag([self.noise_x_sys**2,self.noise_x_sys**2,self.noise_x_sys**2,0.0,0.0,0.0,self.noise_a_sys**2,self.noise_a_sys**2,self.noise_a_sys**2,self.noise_g_sys**2,self.noise_g_sys**2,self.noise_g_sys**2]) # sys noise 78 | self.R = np.diag([self.noise_a_obs**2,self.noise_a_obs**2,self.noise_a_obs**2,self.noise_g_obs**2,self.noise_g_obs**2,self.noise_g_obs**2]) # obs noise 79 | 80 | 81 | def initParticleFilter(self): 82 | self.pf = ParticleFilter().getParticleFilterClass("Coplanarity") 83 | self.pf.setFocus(self.f) 84 | self.pf.setParameter(self.noise_x_sys, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise) 85 | self.X = [] # パーティクルセット set of particles 86 | self.loglikelihood = 0.0 87 | self.count = 0 88 | 89 | 90 | 91 | """ 92 | This method is called from Sensor class when new IMU sensor data are arrived. 93 | time_ : time (sec) 94 | accel : acceleration in global coordinates 95 | ori : orientaion 96 | """ 97 | def setSensorData(self, time_, accel, ori): 98 | 99 | # Count 100 | self.count+=1 101 | 102 | # If process is locked by Image Particle Filter, do nothing 103 | if(self.lock): 104 | print("locked") 105 | return 106 | 107 | # Get current time 108 | self.t1 = self.t 109 | self.t = time_ 110 | self.dt = self.t - self.t1 111 | 112 | if(self.isFirstTimeIMU): 113 | #init mu 114 | self.mu = np.array([0.0,0.0,0.0, 115 | 0.0,0.0,0.0, 116 | accel[0],accel[1],accel[2], 117 | ori[0],ori[1],ori[2]]) 118 | else: 119 | #observation 120 | Y = np.array([accel[0],accel[1],accel[2], 121 | ori[0],ori[1],ori[2]]) 122 | dt2 = 0.5 * self.dt * self.dt 123 | #dt3 = (1.0 / 6.0) * dt2 * self.dt 124 | A = np.array([[1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0,0.0], 125 | [0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0], 126 | [0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0], 127 | [0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0,0.0], 128 | [0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0], 129 | [0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0], 130 | [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0], 131 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0], 132 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0], 133 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0], 134 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0], 135 | [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]]) 136 | #Qt = np.diag([dt2,dt2,dt2,self.dt,self.dt,self.dt,1.0,1.0,1.0,self.dt,self.dt,self.dt]) 137 | #Q = Qt.dot(self.Q) 138 | 139 | """ 140 | self.accel3 = copy.deepcopy(self.accel2) 141 | self.accel2 = copy.deepcopy(self.accel1) 142 | self.accel1 = copy.deepcopy(accel) 143 | if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False): 144 | self.mu[3] = 0.0 145 | if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False): 146 | self.mu[4] = 0.0 147 | if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False): 148 | self.mu[5] = 0.0 149 | """ 150 | 151 | self.mu, self.sigma = KF.execKF1Simple(Y,self.mu,self.sigma,A,self.C,self.Q,self.R) 152 | 153 | if(self.isFirstTimeIMU): 154 | self.isFirstTimeIMU = False 155 | 156 | 157 | 158 | """ 159 | This method is called from Image class when new camera image data are arrived. 160 | time_ : time (sec) 161 | keypointPairs : list of KeyPointPair class objects 162 | """ 163 | def setImageData(self, time_, keypointPairs): 164 | 165 | # If IMU data has not been arrived yet, do nothing 166 | if(self.isFirstTimeIMU): 167 | return 168 | 169 | # Count 170 | self.count+=1 171 | 172 | ######################## 173 | print("===================================") 174 | print("step "+str(self.step)) 175 | ########################### 176 | 177 | # If first time, save mu and don't do anything else 178 | if(self.isFirstTimeCamera): 179 | self.isFirstTimeCamera = False 180 | self.mu1 = copy.deepcopy(self.mu) # save mu[t] as mu[t-1] 181 | self.t_camera = time_ 182 | self.step += 1 183 | return 184 | 185 | # Lock IMU process 186 | self.lock = True 187 | 188 | # Get current time 189 | self.t1 = self.t 190 | self.t = time_ 191 | self.dt = self.t - self.t1 192 | 193 | self.t1_camera = self.t_camera 194 | self.t_camera = time_ 195 | dt_camera = self.t_camera - self.t1_camera 196 | 197 | # create particle from state vector 198 | self.X = self.createParticleFromStateVector(self.mu, self.sigma) 199 | 200 | # create X1 from mu[t-1] 201 | X1 = Particle() 202 | X1.initWithMu(self.mu1) 203 | 204 | self.saveXYZasCSV(self.X,"1") ############################## 205 | 206 | # exec particle filter 207 | self.X = self.pf.pf_step(self.X, X1, self.dt, dt_camera, keypointPairs, self.M) 208 | 209 | self.saveXYZasCSV(self.X,"2") ############################## 210 | 211 | # create state vector from particle set 212 | self.mu, self.sigma = self.createStateVectorFromParticle(self.X) 213 | 214 | # save mu[t] as mu[t-1] 215 | self.mu1 = copy.deepcopy(self.mu) 216 | 217 | # Step (camera only observation step) 218 | self.step += 1 219 | 220 | # Unlock IMU process 221 | self.lock = False 222 | 223 | 224 | 225 | """ 226 | save (X,Y,Z) of particles as CSV file 227 | """ 228 | def saveXYZasCSV(self,X,appendix): 229 | x = [] 230 | for X_ in X: 231 | x.append(X_.x) 232 | date = datetime.datetime.now() 233 | #datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000) 234 | #np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',') 235 | datestr = date.strftime("%Y%m%d_%H%M%S_") 236 | np.savetxt('./data/plot3d/'+datestr+str(self.count)+'_xyz_'+appendix+'.csv', x, delimiter=',') 237 | 238 | 239 | """ 240 | create particle set from state vector 241 | """ 242 | def createParticleFromStateVector(self, mu, sigma): 243 | X = [] 244 | for i in range(self.M): 245 | particle = Particle() 246 | particle.initWithStateVector(mu, sigma) 247 | X.append(particle) 248 | return X 249 | 250 | 251 | """ 252 | create state vector from particle set 253 | """ 254 | def createStateVectorFromParticle(self, X): 255 | x = [] 256 | v = [] 257 | a = [] 258 | o = [] 259 | for X_ in X: 260 | x.append(X_.x) 261 | v.append(X_.v) 262 | a.append(X_.a) 263 | o.append(X_.o) 264 | x_mu = np.mean(x, axis=0) 265 | v_mu = np.mean(v, axis=0) 266 | a_mu = np.mean(a, axis=0) 267 | o_mu = np.mean(o, axis=0) 268 | x_var = np.var(x, axis=0) 269 | v_var = np.var(v, axis=0) 270 | a_var = np.var(a, axis=0) 271 | o_var = np.var(o, axis=0) 272 | mu = np.array([x_mu[0],x_mu[1],x_mu[2],v_mu[0],v_mu[1],v_mu[2],a_mu[0],a_mu[1],a_mu[2],o_mu[0],o_mu[1],o_mu[2]]) 273 | sigma = np.diag([x_var[0],x_var[1],x_var[2],v_var[0],v_var[1],v_var[2],a_var[0],a_var[1],a_var[2],o_var[0],o_var[1],o_var[2]]) 274 | return mu, sigma 275 | 276 | 277 | """ 278 | This method is called from "Main.py" 279 | return estimated state vector 280 | """ 281 | def getState(self): 282 | x = np.array([self.mu[0],self.mu[1],self.mu[2]]) 283 | v = np.array([self.mu[3],self.mu[4],self.mu[5]]) 284 | a = np.array([self.mu[6],self.mu[7],self.mu[8]]) 285 | o = np.array([self.mu[9],self.mu[10],self.mu[11]]) 286 | return x,v,a,o 287 | 288 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | # coding: utf-8 2 | import numpy as np 3 | import Util 4 | from math import * 5 | 6 | def main(): 7 | 8 | # 画像サイズ 9 | width = 720 10 | height = 1280 11 | # 焦点距離 12 | f = 924.1770935 13 | # 主点位置 14 | cx = - 6.361694 15 | cy = - 22.962158 16 | 17 | # ランドマーク座標 18 | landmark1 = np.array([0.6, -5.0, -1.0]) 19 | landmark2 = np.array([0.0, -5.0, 1.0]) 20 | landmark3 = np.array([1.2, -5.0, 1.0]) 21 | landmark4 = np.array([-0.2, -5.0, -0.0]) 22 | landmark5 = np.array([1.4, -5.0, -0.0]) 23 | 24 | landmark6 = np.array([0.6, -3.0, -0.5]) #途中から追加する 25 | landmark7 = np.array([0.6, -8.0, -0.5]) #途中から追加する 26 | 27 | # 1 座標 28 | x1 = np.array([0.0,0.0,0.0]) 29 | 30 | # 1 回転角 31 | ox1 = - pi / 2.0 32 | oy1 = 0.0 33 | oz1 = 0.0 34 | 35 | # 1 回転行列 W -> C 36 | rotXinv = Util.rotationMatrixX(-ox1) 37 | rotYinv = Util.rotationMatrixY(-oy1) 38 | rotZinv = Util.rotationMatrixZ(-oz1) 39 | R1 = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 40 | 41 | # 1 画像座標 42 | landmark1_1 = R1.dot(landmark1 - x1) 43 | u11 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 44 | v11 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 45 | print("(u1,v1)1 = "+str(u11)+":"+str(v11)) 46 | landmark2_1 = R1.dot(landmark2 - x1) 47 | u21 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 48 | v21 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 49 | print("(u2,v2)1 = "+str(u21)+":"+str(v21)) 50 | landmark3_1 = R1.dot(landmark3 - x1) 51 | u31 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 52 | v31 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 53 | print("(u3,v3)1 = "+str(u31)+":"+str(v31)) 54 | landmark4_1 = R1.dot(landmark4 - x1) 55 | u41 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 56 | v41 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 57 | print("(u4,v4)1 = "+str(u41)+":"+str(v41)) 58 | landmark5_1 = R1.dot(landmark5 - x1) 59 | u51 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 60 | v51 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 61 | print("(u5,v5)1 = "+str(u51)+":"+str(v51)) 62 | print(" ") 63 | 64 | # 2 座標 65 | x2 = np.array([0.11,-0.011,0.0]) 66 | 67 | # 2 回転角 68 | ox = - pi / 2.0 69 | oy = 0.0 70 | oz = 0.01 71 | 72 | # 2 回転行列 W -> C 73 | rotXinv = Util.rotationMatrixX(-ox) 74 | rotYinv = Util.rotationMatrixY(-oy) 75 | rotZinv = Util.rotationMatrixZ(-oz) 76 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 77 | 78 | # 2 画像座標 79 | landmark1_1 = R.dot(landmark1 - x2) 80 | u12 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 81 | v12 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 82 | print("(u1,v1)2 = "+str(u12)+":"+str(v12)) 83 | landmark2_1 = R.dot(landmark2 - x2) 84 | u22 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 85 | v22 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 86 | print("(u2,v2)2 = "+str(u22)+":"+str(v22)) 87 | landmark3_1 = R.dot(landmark3 - x2) 88 | u32 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 89 | v32 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 90 | print("(u3,v3)2 = "+str(u32)+":"+str(v32)) 91 | landmark4_1 = R.dot(landmark4 - x2) 92 | u42 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 93 | v42 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 94 | print("(u4,v4)2 = "+str(u42)+":"+str(v42)) 95 | landmark5_1 = R.dot(landmark5 - x2) 96 | u52 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 97 | v52 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 98 | print("(u5,v5)2 = "+str(u52)+":"+str(v52)) 99 | landmark6_1 = R.dot(landmark6 - x2) 100 | u62 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 101 | v62 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 102 | print("(u6,v6)2 = "+str(u62)+":"+str(v62)) 103 | landmark7_1 = R.dot(landmark7 - x2) 104 | u72 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 105 | v72 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 106 | print("(u7,v7)2 = "+str(u72)+":"+str(v72)) 107 | print(" ") 108 | 109 | # 3 座標 110 | x3 = np.array([0.21,-0.021,0.0]) 111 | 112 | # 3 回転角 113 | ox = - pi / 2.0 114 | oy = 0.0 115 | oz = 0.02 116 | 117 | # 3 回転行列 W -> C 118 | rotXinv = Util.rotationMatrixX(-ox) 119 | rotYinv = Util.rotationMatrixY(-oy) 120 | rotZinv = Util.rotationMatrixZ(-oz) 121 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 122 | 123 | # 3 画像座標 124 | landmark1_1 = R.dot(landmark1 - x3) 125 | u13 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 126 | v13 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 127 | print("(u1,v1)3 = "+str(u13)+":"+str(v13)) 128 | landmark2_1 = R.dot(landmark2 - x3) 129 | u23 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 130 | v23 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 131 | print("(u2,v2)3 = "+str(u23)+":"+str(v23)) 132 | landmark3_1 = R.dot(landmark3 - x3) 133 | u33 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 134 | v33 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 135 | print("(u3,v3)3 = "+str(u33)+":"+str(v33)) 136 | landmark4_1 = R.dot(landmark4 - x3) 137 | u43 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 138 | v43 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 139 | print("(u4,v4)3 = "+str(u43)+":"+str(v43)) 140 | landmark5_1 = R.dot(landmark5 - x3) 141 | u53 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 142 | v53 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 143 | print("(u5,v5)3 = "+str(u53)+":"+str(v53)) 144 | landmark6_1 = R.dot(landmark6 - x3) 145 | u63 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 146 | v63 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 147 | print("(u6,v6)3 = "+str(u63)+":"+str(v63)) 148 | landmark7_1 = R.dot(landmark7 - x3) 149 | u73 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 150 | v73 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 151 | print("(u7,v7)3 = "+str(u73)+":"+str(v73)) 152 | print(" ") 153 | 154 | # 4 座標 155 | x4 = np.array([0.31,-0.031,0.0]) 156 | 157 | # 4 回転角 158 | ox = - pi / 2.0 159 | oy = 0.0 160 | oz = 0.03 161 | 162 | # 4 回転行列 W -> C 163 | rotXinv = Util.rotationMatrixX(-ox) 164 | rotYinv = Util.rotationMatrixY(-oy) 165 | rotZinv = Util.rotationMatrixZ(-oz) 166 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 167 | 168 | # 4 画像座標 169 | landmark1_1 = R.dot(landmark1 - x4) 170 | u14 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 171 | v14 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 172 | print("(u1,v1)4 = "+str(u14)+":"+str(v14)) 173 | landmark2_1 = R.dot(landmark2 - x4) 174 | u24 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 175 | v24 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 176 | print("(u2,v2)4 = "+str(u24)+":"+str(v24)) 177 | landmark3_1 = R.dot(landmark3 - x4) 178 | u34 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 179 | v34 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 180 | print("(u3,v3)4 = "+str(u34)+":"+str(v34)) 181 | landmark4_1 = R.dot(landmark4 - x4) 182 | u44 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 183 | v44 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 184 | print("(u4,v4)4 = "+str(u44)+":"+str(v44)) 185 | landmark5_1 = R.dot(landmark5 - x4) 186 | u54 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 187 | v54 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 188 | print("(u5,v5)4 = "+str(u54)+":"+str(v54)) 189 | landmark6_1 = R.dot(landmark6 - x4) 190 | u64 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 191 | v64 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 192 | print("(u6,v6)4 = "+str(u64)+":"+str(v64)) 193 | landmark7_1 = R.dot(landmark7 - x4) 194 | u74 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 195 | v74 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 196 | print("(u7,v7)4 = "+str(u74)+":"+str(v74)) 197 | print(" ") 198 | 199 | # 5 座標 200 | x5 = np.array([0.41,-0.041,0.0]) 201 | 202 | # 5 回転角 203 | ox = - pi / 2.0 204 | oy = 0.0 205 | oz = 0.04 206 | 207 | # 5 回転行列 W -> C 208 | rotXinv = Util.rotationMatrixX(-ox) 209 | rotYinv = Util.rotationMatrixY(-oy) 210 | rotZinv = Util.rotationMatrixZ(-oz) 211 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 212 | 213 | # 5 画像座標 214 | landmark1_1 = R.dot(landmark1 - x5) 215 | u15 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 216 | v15 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 217 | print("(u1,v1)5 = "+str(u15)+":"+str(v15)) 218 | landmark2_1 = R.dot(landmark2 - x5) 219 | u25 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 220 | v25 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 221 | print("(u2,v2)5 = "+str(u25)+":"+str(v25)) 222 | landmark3_1 = R.dot(landmark3 - x5) 223 | u35 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 224 | v35 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 225 | print("(u3,v3)5 = "+str(u35)+":"+str(v35)) 226 | landmark4_1 = R.dot(landmark4 - x5) 227 | u45 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 228 | v45 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 229 | print("(u4,v4)5 = "+str(u45)+":"+str(v45)) 230 | landmark5_1 = R.dot(landmark5 - x5) 231 | u55 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 232 | v55 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 233 | print("(u5,v5)5 = "+str(u55)+":"+str(v55)) 234 | landmark6_1 = R.dot(landmark6 - x5) 235 | u65 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 236 | v65 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 237 | print("(u6,v6)5 = "+str(u65)+":"+str(v65)) 238 | landmark7_1 = R.dot(landmark7 - x5) 239 | u75 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 240 | v75 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 241 | print("(u7,v7)5 = "+str(u75)+":"+str(v75)) 242 | print(" ") 243 | 244 | # 6 座標 245 | x6 = np.array([0.51,-0.051,0.0]) 246 | 247 | # 6 回転角 248 | ox = - pi / 2.0 249 | oy = 0.0 250 | oz = 0.05 251 | 252 | # 6 回転行列 W -> C 253 | rotXinv = Util.rotationMatrixX(-ox) 254 | rotYinv = Util.rotationMatrixY(-oy) 255 | rotZinv = Util.rotationMatrixZ(-oz) 256 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 257 | 258 | # 6 画像座標 259 | landmark1_1 = R.dot(landmark1 - x6) 260 | u16 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 261 | v16 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 262 | print("(u1,v1)6 = "+str(u16)+":"+str(v16)) 263 | landmark2_1 = R.dot(landmark2 - x6) 264 | u26 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 265 | v26 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 266 | print("(u2,v2)6 = "+str(u26)+":"+str(v26)) 267 | landmark3_1 = R.dot(landmark3 - x6) 268 | u36 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 269 | v36 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 270 | print("(u3,v3)6 = "+str(u36)+":"+str(v36)) 271 | landmark4_1 = R.dot(landmark4 - x6) 272 | u46 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 273 | v46 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 274 | print("(u4,v4)6 = "+str(u46)+":"+str(v46)) 275 | landmark5_1 = R.dot(landmark5 - x6) 276 | u56 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 277 | v56 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 278 | print("(u5,v5)6 = "+str(u56)+":"+str(v56)) 279 | landmark6_1 = R.dot(landmark6 - x6) 280 | u66 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 281 | v66 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 282 | print("(u6,v6)6 = "+str(u66)+":"+str(v66)) 283 | landmark7_1 = R.dot(landmark7 - x6) 284 | u76 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 285 | v76 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 286 | print("(u7,v7)6 = "+str(u76)+":"+str(v76)) 287 | print(" ") 288 | 289 | # 7 座標 290 | x7 = np.array([0.61,-0.061,0.0]) 291 | 292 | # 7 回転角 293 | ox = - pi / 2.0 294 | oy = 0.0 295 | oz = 0.06 296 | 297 | # 7 回転行列 W -> C 298 | rotXinv = Util.rotationMatrixX(-ox) 299 | rotYinv = Util.rotationMatrixY(-oy) 300 | rotZinv = Util.rotationMatrixZ(-oz) 301 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 302 | 303 | # 7 画像座標 304 | landmark1_1 = R.dot(landmark1 - x7) 305 | u17 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 306 | v17 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 307 | print("(u1,v1)7 = "+str(u17)+":"+str(v17)) 308 | landmark2_1 = R.dot(landmark2 - x7) 309 | u27 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 310 | v27 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 311 | print("(u2,v2)7 = "+str(u27)+":"+str(v27)) 312 | landmark3_1 = R.dot(landmark3 - x7) 313 | u37 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 314 | v37 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 315 | print("(u3,v3)7 = "+str(u37)+":"+str(v37)) 316 | landmark4_1 = R.dot(landmark4 - x7) 317 | u47 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 318 | v47 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 319 | print("(u4,v4)7 = "+str(u47)+":"+str(v47)) 320 | landmark5_1 = R.dot(landmark5 - x7) 321 | u57 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 322 | v57 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 323 | print("(u5,v5)7 = "+str(u57)+":"+str(v57)) 324 | landmark6_1 = R.dot(landmark6 - x7) 325 | u67 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 326 | v67 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 327 | print("(u6,v6)7 = "+str(u67)+":"+str(v67)) 328 | landmark7_1 = R.dot(landmark7 - x7) 329 | u77 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 330 | v77 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 331 | print("(u7,v7)7 = "+str(u77)+":"+str(v77)) 332 | print(" ") 333 | 334 | # 8 座標 335 | x8 = np.array([0.71,-0.071,0.0]) 336 | 337 | # 8 回転角 338 | ox = - pi / 2.0 339 | oy = 0.0 340 | oz = 0.07 341 | 342 | # 8 回転行列 W -> C 343 | rotXinv = Util.rotationMatrixX(-ox) 344 | rotYinv = Util.rotationMatrixY(-oy) 345 | rotZinv = Util.rotationMatrixZ(-oz) 346 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 347 | 348 | # 8 画像座標 349 | landmark1_1 = R.dot(landmark1 - x8) 350 | u18 = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 351 | v18 = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 352 | print("(u1,v1)8 = "+str(u18)+":"+str(v18)) 353 | landmark2_1 = R.dot(landmark2 - x8) 354 | u28 = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 355 | v28 = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 356 | print("(u2,v2)8 = "+str(u28)+":"+str(v28)) 357 | landmark3_1 = R.dot(landmark3 - x8) 358 | u38 = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 359 | v38 = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 360 | print("(u3,v3)8 = "+str(u38)+":"+str(v38)) 361 | landmark4_1 = R.dot(landmark4 - x8) 362 | u48 = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 363 | v48 = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 364 | print("(u4,v4)8 = "+str(u48)+":"+str(v48)) 365 | landmark5_1 = R.dot(landmark5 - x8) 366 | u58 = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 367 | v58 = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 368 | print("(u5,v5)8 = "+str(u58)+":"+str(v58)) 369 | landmark6_1 = R.dot(landmark6 - x8) 370 | u68 = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 371 | v68 = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 372 | print("(u6,v6)8 = "+str(u68)+":"+str(v68)) 373 | landmark7_1 = R.dot(landmark7 - x8) 374 | u78 = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 375 | v78 = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 376 | print("(u7,v7)8 = "+str(u78)+":"+str(v78)) 377 | print(" ") 378 | 379 | """ 380 | # ■ 座標 381 | x■ = np.array([0.★1,-0.0★1,0.0]) 382 | 383 | # ■ 回転角 384 | ox = - pi / 2.0 385 | oy = 0.0 386 | oz = 0.0★ 387 | 388 | # ■ 回転行列 W -> C 389 | rotXinv = Util.rotationMatrixX(-ox) 390 | rotYinv = Util.rotationMatrixY(-oy) 391 | rotZinv = Util.rotationMatrixZ(-oz) 392 | R = np.dot(rotXinv, np.dot(rotYinv, rotZinv)) 393 | 394 | # ■ 画像座標 395 | landmark1_1 = R.dot(landmark1 - x■) 396 | u1■ = - (f * landmark1_1[0] / landmark1_1[2]) + width/2.0 + cx 397 | v1■ = f * landmark1_1[1] / landmark1_1[2] + height/2.0 + cy 398 | print("(u1,v1)■ = "+str(u1■)+":"+str(v1■)) 399 | landmark2_1 = R.dot(landmark2 - x■) 400 | u2■ = - (f * landmark2_1[0] / landmark2_1[2]) + width/2.0 + cx 401 | v2■ = f * landmark2_1[1] / landmark2_1[2] + height/2.0 + cy 402 | print("(u2,v2)■ = "+str(u2■)+":"+str(v2■)) 403 | landmark3_1 = R.dot(landmark3 - x■) 404 | u3■ = - (f * landmark3_1[0] / landmark3_1[2]) + width/2.0 + cx 405 | v3■ = f * landmark3_1[1] / landmark3_1[2] + height/2.0 + cy 406 | print("(u3,v3)■ = "+str(u3■)+":"+str(v3■)) 407 | landmark4_1 = R.dot(landmark4 - x■) 408 | u4■ = - (f * landmark4_1[0] / landmark4_1[2]) + width/2.0 + cx 409 | v4■ = f * landmark4_1[1] / landmark4_1[2] + height/2.0 + cy 410 | print("(u4,v4)■ = "+str(u4■)+":"+str(v4■)) 411 | landmark5_1 = R.dot(landmark5 - x■) 412 | u5■ = - (f * landmark5_1[0] / landmark5_1[2]) + width/2.0 + cx 413 | v5■ = f * landmark5_1[1] / landmark5_1[2] + height/2.0 + cy 414 | print("(u5,v5)■ = "+str(u5■)+":"+str(v5■)) 415 | landmark6_1 = R.dot(landmark6 - x■) 416 | u6■ = - (f * landmark6_1[0] / landmark6_1[2]) + width/2.0 + cx 417 | v6■ = f * landmark6_1[1] / landmark6_1[2] + height/2.0 + cy 418 | print("(u6,v6)■ = "+str(u6■)+":"+str(v6■)) 419 | landmark7_1 = R.dot(landmark7 - x■) 420 | u7■ = - (f * landmark7_1[0] / landmark7_1[2]) + width/2.0 + cx 421 | v7■ = f * landmark7_1[1] / landmark7_1[2] + height/2.0 + cy 422 | print("(u7,v7)■ = "+str(u7■)+":"+str(v7■)) 423 | print(" ") 424 | """ 425 | 426 | # MQTT形式に変換 427 | print("SLAM/input/camera%120$1:1:"+str(u11)+":"+str(v11)+":"+str(u12)+":"+str(v12)+"&2:2:"+str(u21)+":"+str(v21)+":"+str(u22)+":"+str(v22)+"&3:3:"+str(u31)+":"+str(v31)+":"+str(u32)+":"+str(v32)+"&4:4:"+str(u41)+":"+str(v41)+":"+str(u42)+":"+str(v42)+"&5:5:"+str(u51)+":"+str(v51)+":"+str(u52)+":"+str(v52)+"&") 428 | print("SLAM/input/camera%220$1:1:"+str(u12)+":"+str(v12)+":"+str(u13)+":"+str(v12)+"&2:2:"+str(u22)+":"+str(v22)+":"+str(u23)+":"+str(v23)+"&3:3:"+str(u32)+":"+str(v32)+":"+str(u33)+":"+str(v33)+"&4:4:"+str(u42)+":"+str(v42)+":"+str(u43)+":"+str(v43)+"&5:5:"+str(u52)+":"+str(v52)+":"+str(u53)+":"+str(v53)+"&6:6:"+str(u62)+":"+str(v62)+":"+str(u63)+":"+str(v63)+"&7:7:"+str(u72)+":"+str(v72)+":"+str(u73)+":"+str(v73)+"&") 429 | print("SLAM/input/camera%320$1:1:"+str(u13)+":"+str(v13)+":"+str(u14)+":"+str(v14)+"&2:2:"+str(u23)+":"+str(v23)+":"+str(u24)+":"+str(v24)+"&3:3:"+str(u33)+":"+str(v33)+":"+str(u34)+":"+str(v34)+"&4:4:"+str(u43)+":"+str(v43)+":"+str(u44)+":"+str(v44)+"&5:5:"+str(u53)+":"+str(v53)+":"+str(u54)+":"+str(v54)+"&6:6:"+str(u63)+":"+str(v63)+":"+str(u64)+":"+str(v64)+"&7:7:"+str(u73)+":"+str(v73)+":"+str(u74)+":"+str(v74)+"&") 430 | print("SLAM/input/camera%420$1:1:"+str(u14)+":"+str(v14)+":"+str(u15)+":"+str(v15)+"&2:2:"+str(u24)+":"+str(v24)+":"+str(u25)+":"+str(v25)+"&3:3:"+str(u34)+":"+str(v34)+":"+str(u35)+":"+str(v35)+"&4:4:"+str(u44)+":"+str(v44)+":"+str(u45)+":"+str(v45)+"&5:5:"+str(u54)+":"+str(v54)+":"+str(u55)+":"+str(v55)+"&6:6:"+str(u64)+":"+str(v64)+":"+str(u65)+":"+str(v65)+"&7:7:"+str(u74)+":"+str(v74)+":"+str(u75)+":"+str(v75)+"&") 431 | print("SLAM/input/camera%520$1:1:"+str(u15)+":"+str(v15)+":"+str(u16)+":"+str(v16)+"&2:2:"+str(u25)+":"+str(v25)+":"+str(u26)+":"+str(v26)+"&3:3:"+str(u35)+":"+str(v35)+":"+str(u36)+":"+str(v36)+"&4:4:"+str(u45)+":"+str(v45)+":"+str(u46)+":"+str(v46)+"&5:5:"+str(u55)+":"+str(v55)+":"+str(u56)+":"+str(v56)+"&6:6:"+str(u65)+":"+str(v65)+":"+str(u66)+":"+str(v66)+"&7:7:"+str(u75)+":"+str(v75)+":"+str(u76)+":"+str(v76)+"&") 432 | print("SLAM/input/camera%620$1:1:"+str(u16)+":"+str(v16)+":"+str(u17)+":"+str(v17)+"&2:2:"+str(u26)+":"+str(v26)+":"+str(u27)+":"+str(v27)+"&3:3:"+str(u36)+":"+str(v36)+":"+str(u37)+":"+str(v37)+"&4:4:"+str(u46)+":"+str(v46)+":"+str(u47)+":"+str(v47)+"&5:5:"+str(u56)+":"+str(v56)+":"+str(u57)+":"+str(v57)+"&6:6:"+str(u66)+":"+str(v66)+":"+str(u67)+":"+str(v67)+"&7:7:"+str(u76)+":"+str(v76)+":"+str(u77)+":"+str(v77)+"&") 433 | print("SLAM/input/camera%720$1:1:"+str(u17)+":"+str(v17)+":"+str(u18)+":"+str(v18)+"&2:2:"+str(u27)+":"+str(v27)+":"+str(u28)+":"+str(v28)+"&3:3:"+str(u37)+":"+str(v37)+":"+str(u38)+":"+str(v38)+"&4:4:"+str(u47)+":"+str(v47)+":"+str(u48)+":"+str(v48)+"&5:5:"+str(u57)+":"+str(v57)+":"+str(u58)+":"+str(v58)+"&6:6:"+str(u67)+":"+str(v67)+":"+str(u68)+":"+str(v68)+"&7:7:"+str(u77)+":"+str(v77)+":"+str(u78)+":"+str(v78)+"&") 434 | 435 | """ 436 | print("SLAM/input/camera%■20$1:1:"+str(u1★)+":"+str(v1★)+":"+str(u1■)+":"+str(v1■)+"&2:2:"+str(u2★)+":"+str(v2★)+":"+str(u2■)+":"+str(v2■)+"&3:3:"+str(u3★)+":"+str(v3★)+":"+str(u3■)+":"+str(v3■)+"&4:4:"+str(u4★)+":"+str(v4★)+":"+str(u4■)+":"+str(v4■)+"&5:5:"+str(u5★)+":"+str(v5★)+":"+str(u5■)+":"+str(v5■)+"&6:6:"+str(u6★)+":"+str(v6★)+":"+str(u6■)+":"+str(v6■)+"&7:7:"+str(u7★)+":"+str(v7★)+":"+str(u7■)+":"+str(v7■)+"&") 437 | """ 438 | 439 | 440 | if __name__ == '__main__': 441 | main() --------------------------------------------------------------------------------