├── .gitignore ├── README.md ├── dataset ├── image_list_02.txt ├── image_list_02.txt.s └── kitti_image_00.txt ├── feature_sparity.py ├── result ├── after_reject.png ├── before_reject.png ├── depth.png ├── depth_vs_v.png ├── kitti_00_path.png ├── kitti_00_path_filter_10.png ├── kitti_00_path_remove_outlier_with_gt.pdf ├── kitti_00_path_with_gt.png ├── kitti_00_scale_filter_10.png ├── kitti_00_scale_remove_outlier_with_gt.pdf ├── kitti_00_x_filter_10.png ├── kitti_00_y_filter_10.png ├── kitti_00_z_filter_10.png ├── kitti_02_path_remove_outlier_with_gt.pdf ├── kitti_02_path_with_gt.png ├── kitti_02_scale_remove_outlier_with_gt.pdf ├── path_00.txt ├── path_00_correct_R.txt ├── path_02.txt └── pcl.png ├── run.sh ├── script ├── calculate_mean.py ├── calculate_speed.py ├── change_scale.py ├── data_check.py ├── evaluate_scale.py ├── evaluate_vo.py ├── plot.sh ├── plot_path.py ├── score_calculation.py └── transformation.py ├── sequence.txt ├── src ├── calculate_height_pitch.py ├── calculate_height_pitch_eval.py ├── calculate_height_pitch_eval_line.py ├── detector.py ├── estimate_road_norm.py ├── feature_sparsity.py ├── graph.py ├── main.py ├── main_offline.py ├── param.py ├── reconstruct.py ├── rescale.py ├── rescale.py.origin ├── rescale_test.py ├── rescale_test.py.origin ├── scale_calculator.py ├── thirdparty │ ├── MonocularVO │ │ ├── README.md │ │ ├── map.png │ │ ├── test.py │ │ └── visual_odometry.py │ └── Ransac │ │ ├── .gitignore │ │ ├── LICENSE │ │ ├── README.md │ │ ├── __init__.py │ │ ├── line_fitting.py │ │ ├── plane_fitting.py │ │ └── ransac.py └── triangle_batch.py ├── tesh.sh ├── tesh_off_line.sh ├── test.sh └── test_off_line.sh /.gitignore: -------------------------------------------------------------------------------- 1 | result/* 2 | dataset/* 3 | *.txt* 4 | *.png 5 | log* 6 | *.sw[po] 7 | */*.sw[po] 8 | */*/*.sw[po] 9 | __pycache__/* 10 | */__pycache__/* 11 | */*/__pycache__/* 12 | */*/*/__pycache__/* 13 | *.DS* 14 | */*.DS* 15 | */*/*.DS* 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MVOScaleRecovery 2 | Recover the scale of monocular visual odometry 3 | 4 | # RUN 5 | 1. save your image name in path\_to\_image\_list by `find path/| sort >path_to_image_list` 6 | 2. modify the `src/param.py` based on your dataset 7 | 3. run 8 | `python3 src/main.py path_to_image_list` 9 | 10 | # Note 11 | this is a scale recoery for a simple monocular VO, the accuracy is degraded. Current error of KITTI 00 by [KITTI benchmark](https://github.com/TimingSpace/EvaluateVisualOdometryKITTI) is 2.17% (ave every 800m) 12 | 13 | ## Current result 14 | 1. KITTI 00 15 | ![kitti_00](result/kitti_00_path_filter_10.png) 16 | ![kitti_00](result/kitti_00_x_filter_10.png) 17 | ![kitti_00](result/kitti_00_z_filter_10.png) 18 | ![kitti_00](result/kitti_00_y_filter_10.png) 19 | ![kitti_00_scale](result/kitti_00_scale_filter_10.png) 20 | 21 | 2. KITTI 02 22 | ![kitti_02](result/kitti_02_path_remove_outlier_with_gt.pdf) 23 | ![kitti_02_scale](result/kitti_02_scale_remove_outlier_with_gt.pdf) 24 | 25 | 3. Initial Triangles before rance 26 | ![triangles](result/before_reject.png) 27 | ![triangles_o](result/after_reject.png) 28 | 4. depth and reconstruct 29 | ![triangles](result/depth.png) 30 | ![triangles_o](result/pcl.png) 31 | 32 | -------------------------------------------------------------------------------- /dataset/image_list_02.txt.s: -------------------------------------------------------------------------------- 1 | /Volumes/Matrix/Dataset/KITTI/02/image_2/ 2 | /Volumes/Matrix/Dataset/KITTI/02/image_2//000000.png 3 | /Volumes/Matrix/Dataset/KITTI/02/image_2//000001.png 4 | -------------------------------------------------------------------------------- /feature_sparity.py: -------------------------------------------------------------------------------- 1 | #coding:utf-8 2 | import random 3 | import numpy as np 4 | import cv2 5 | import matplotlib.pyplot as plt 6 | img = cv2.imread('test2.png',0) 7 | delete_freq=5 　　　　　　　　 8 | i=0 9 | akaze = cv2.AKAZE_create(threshold=0.0007) 　　 #smaller, more points 10 | kp_akaze = akaze.detect(img,None)　　　　　　　　　　　　 ＃keypoints of akaze 11 | img_akaze = cv2.drawKeypoints(img,kp_akaze,img,color=(255,0,0)) 12 | cv2.imshow('AKAZE',img_akaze) 13 | cv2.waitKey(０) 14 | 15 | pts=cv2.KeyPoint_convert(kp_akaze)　　　　　　　　　　　 #positions of keypoints 16 | while i < len(kp_akaze)-6: 17 | cv2.circle(img,(pts[i][0],pts[i][1]),2, (255, 0, 0), thickness =１) #draw pts[i][0] circle in the image 18 | i=i+5 19 | cv2.imshow('AKAZE_2',img) 20 | cv2.waitKey(0) 21 | -------------------------------------------------------------------------------- /result/after_reject.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/after_reject.png -------------------------------------------------------------------------------- /result/before_reject.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/before_reject.png -------------------------------------------------------------------------------- /result/depth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/depth.png -------------------------------------------------------------------------------- /result/depth_vs_v.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/depth_vs_v.png -------------------------------------------------------------------------------- /result/kitti_00_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_path.png -------------------------------------------------------------------------------- /result/kitti_00_path_filter_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_path_filter_10.png -------------------------------------------------------------------------------- /result/kitti_00_path_remove_outlier_with_gt.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_path_remove_outlier_with_gt.pdf -------------------------------------------------------------------------------- /result/kitti_00_path_with_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_path_with_gt.png -------------------------------------------------------------------------------- /result/kitti_00_scale_filter_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_scale_filter_10.png -------------------------------------------------------------------------------- /result/kitti_00_scale_remove_outlier_with_gt.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_scale_remove_outlier_with_gt.pdf -------------------------------------------------------------------------------- /result/kitti_00_x_filter_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_x_filter_10.png -------------------------------------------------------------------------------- /result/kitti_00_y_filter_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_y_filter_10.png -------------------------------------------------------------------------------- /result/kitti_00_z_filter_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_00_z_filter_10.png -------------------------------------------------------------------------------- /result/kitti_02_path_remove_outlier_with_gt.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_02_path_remove_outlier_with_gt.pdf -------------------------------------------------------------------------------- /result/kitti_02_path_with_gt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_02_path_with_gt.png -------------------------------------------------------------------------------- /result/kitti_02_scale_remove_outlier_with_gt.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/kitti_02_scale_remove_outlier_with_gt.pdf -------------------------------------------------------------------------------- /result/pcl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/result/pcl.png -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | for d in $(cat $1) ; do 3 | echo $d 4 | python src/main.py $d 5 | done 6 | -------------------------------------------------------------------------------- /script/calculate_mean.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | data = np.loadtxt(sys.argv[1]) 4 | print(np.mean(data,0)) 5 | -------------------------------------------------------------------------------- /script/calculate_speed.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | from transformation import * 4 | 5 | name = sys.argv[1] 6 | name = name.split('.')[0] 7 | data = np.loadtxt(sys.argv[1]) 8 | motion = pose2motion(data) 9 | motion_trans = motion[:,3:12:4] 10 | square = np.sum(motion_trans*motion_trans,1) 11 | speed = np.sqrt(square) 12 | np.savetxt(name+'_speed.txt',speed) 13 | -------------------------------------------------------------------------------- /script/change_scale.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | from transformation import * 4 | 5 | 6 | def main(): 7 | tag = sys.argv[3] 8 | file_name = sys.argv[1] 9 | file_name = file_name.split('.')[-3] 10 | data = np.loadtxt(sys.argv[1]) 11 | scale= np.loadtxt(sys.argv[2]) 12 | motion = pose2motion(data) 13 | motion_trans = motion[:,3:12:4] 14 | square = np.sum(motion_trans*motion_trans,1) 15 | speed = np.sqrt(square) 16 | motion_trans = (scale*motion_trans.transpose()/(speed+0.00001)).transpose() 17 | motion[:,3:12:4] = motion_trans 18 | pose = motion2pose(motion) 19 | np.savetxt(file_name+'_gt.txt'+tag,pose) 20 | 21 | if __name__ =='__main__': 22 | main() 23 | -------------------------------------------------------------------------------- /script/data_check.py: -------------------------------------------------------------------------------- 1 | from mpl_toolkits.mplot3d import Axes3D 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import sys 5 | 6 | def vis_3d(points): 7 | fig = plt.figure() 8 | ax = fig.add_subplot(211, projection='3d') 9 | select_id = np.array(points[:,1]>0) & np.array(points[:,1]<2) 10 | data_x = points[select_id,0] 11 | data_y = points[select_id,2] 12 | data_z = -points[select_id,1] 13 | ax.scatter(data_x,data_y,data_z) 14 | ax.set_xlabel('X Label') 15 | ax.set_ylabel('Y Label') 16 | ax.set_zlabel('Z Label') 17 | 18 | plt.show() 19 | def depth_check(data): 20 | select_id = np.array(data[:,1]>0) 21 | depth = data[select_id,2] 22 | v = data[select_id,1] 23 | v_n = v/depth 24 | matrix = [] 25 | for i in range(v_n.shape[0]): 26 | v_minus = v_n-v_n[i] 27 | d_minus = depth-depth[i] 28 | matrix.append(d_minus/(v_minus+0.000001)) 29 | matrix = np.array(matrix) 30 | bins = np.array(list(range(-40,20)))*50 31 | plt.hist(matrix.reshape(-1),bins) 32 | print(np.max(matrix)) 33 | #plt.imshow(matrix) 34 | plt.show() 35 | def vis_2d(points,ax2,color='b'): 36 | select_id = np.array(points[:,1]>0) & np.array(points[:,1]<10) 37 | data_x = points[select_id,0] 38 | data_y = points[select_id,2] 39 | data_z = -points[select_id,1] 40 | ax2.plot(data_y,data_z,'.'+color) 41 | def main(): 42 | fig = plt.figure() 43 | ax2 = fig.add_subplot(111) 44 | points_all = np.loadtxt(sys.argv[1]) 45 | points_left = np.loadtxt(sys.argv[2]) 46 | vis_2d(points_all,ax2) 47 | vis_2d(points_left,ax2,'g') 48 | plt.show() 49 | #depth_check(points) 50 | if __name__ == '__main__': 51 | main() 52 | 53 | -------------------------------------------------------------------------------- /script/evaluate_scale.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import sys 4 | def evaluate_scale(gt,re,vis=True): 5 | re_l = re.shape[0] 6 | er_ = np.abs((gt[:re_l]-re)) 7 | print(np.mean(er_),np.max(er_),1-np.sum(er_>0.1)/re_l,1-np.sum(er_>0.2)/re_l,1-np.sum(er_>0.3)/re_l,1-np.sum(er_>0.5)/re_l) 8 | er_s = gt[:re_l]-re 9 | er_all = [] 10 | for window in [10,20,50,100,200,300,400,500,600,700,800]: 11 | er_ = patch(er_s,window,10) 12 | er_all.append(np.mean(er_)) 13 | print(er_all) 14 | 15 | def evaluate(vis=True): 16 | gt = np.loadtxt(sys.argv[1]) # ground truth 17 | re = np.loadtxt(sys.argv[2]) # result 18 | evaluate_scale(gt,re) 19 | def patch(data,window=10,step=2): 20 | data_new = [] 21 | for i in range(0,data.shape[0]-window,step): 22 | data_new.append(np.sum(data[i:i+window])) 23 | return np.abs(np.array(data_new))/window 24 | 25 | def filter(data,window=10): 26 | data_new = [data[0]] 27 | for i in range(1,data.shape[0]): 28 | data_new.append(np.median(data[max(i-window+1,0):i+1])) 29 | return np.array(data_new) 30 | def main(): 31 | evaluate() 32 | 33 | if __name__ == '__main__': 34 | main() 35 | -------------------------------------------------------------------------------- /script/evaluate_vo.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | import matplotlib.pyplot as plt 4 | 5 | def trajectory_distances(poses): 6 | distances = [] 7 | distances.append(0) 8 | for i in range(1,poses.shape[0]): 9 | p1 = poses[i-1] 10 | p2 = poses[i] 11 | delta = p1[3:12:4] - p2[3:12:4] 12 | distances.append(distances[i-1]+np.linalg.norm(delta)) 13 | return distances 14 | 15 | def last_frame_from_segment_length(dist,first_frame,length): 16 | for i in range(first_frame,len(dist)): 17 | if dist[i]>dist[first_frame]+length: 18 | return i 19 | return -1 20 | 21 | def rotation_error(pose_error): 22 | a = pose_error[0,0] 23 | b = pose_error[1,1] 24 | c = pose_error[2,2] 25 | d = 0.5*(a+b+c-1) 26 | rot_error = np.arccos(max(min(d,1.0),-1.0)) 27 | return rot_error 28 | 29 | def translation_error(pose_error): 30 | dx = pose_error[0,3] 31 | dy = pose_error[1,3] 32 | dz = pose_error[2,3] 33 | return np.sqrt(dx*dx+dy*dy+dz*dz) 34 | 35 | def line2matrix(pose_line): 36 | pose_line = np.array(pose_line) 37 | pose_m = np.matrix(np.eye(4)) 38 | pose_m[0:3,:] = pose_line.reshape(3,4) 39 | return pose_m 40 | def calculate_sequence_error(poses_gt,poses_result): 41 | # error_vetor 42 | errors = [] 43 | 44 | # paramet 45 | step_size = 10; # every second 46 | lengths = [100,200,300,400,500,600,700,800] 47 | num_lengths = len(lengths) 48 | 49 | # pre-compute distances (from ground truth as reference) 50 | dist = trajectory_distances(poses_gt) 51 | # for all start positions do 52 | for first_frame in range(0,poses_gt.shape[0],step_size): 53 | # for all segment lengths do 54 | for i in range(0,num_lengths): 55 | # current length 56 | length = lengths[i]; 57 | 58 | # compute last frame 59 | last_frame = last_frame_from_segment_length(dist,first_frame,length); 60 | # continue, if sequence not long enough 61 | if (last_frame==-1): 62 | continue; 63 | 64 | # compute rotational and translational errors 65 | pose_delta_gt = line2matrix(poses_gt[first_frame]).I*line2matrix(poses_gt[last_frame]) 66 | pose_delta_result = line2matrix(poses_result[first_frame]).I*line2matrix(poses_result[last_frame]) 67 | pose_error = pose_delta_result.I*pose_delta_gt; 68 | r_err = rotation_error(pose_error); 69 | t_err = translation_error(pose_error); 70 | 71 | # compute speed 72 | num_frames = (float)(last_frame-first_frame+1); 73 | speed = length/(0.1*num_frames); 74 | 75 | # write to file 76 | error = [first_frame,r_err/length,t_err/length,length,speed] 77 | errors.append(error) 78 | # return error vector 79 | return errors 80 | def calculate_ave_errors(errors): 81 | lengths = [100,200,300,400,500,600,700,800] 82 | rot_errors=[] 83 | tra_errors=[] 84 | tra_errors_all = [] 85 | for length in lengths: 86 | rot_error_each_length =[] 87 | tra_error_each_length =[] 88 | for error in errors: 89 | if abs(error[3]-length)<1: 90 | rot_error_each_length.append(error[1]) 91 | tra_error_each_length.append(error[2]) 92 | tra_errors_all.append(tra_error_each_length) 93 | if len(rot_error_each_length)>0: 94 | rot_errors.append(sum(rot_error_each_length)/len(rot_error_each_length)) 95 | tra_errors.append(sum(tra_error_each_length)/len(tra_error_each_length)) 96 | return np.array(rot_errors)*180/np.pi,tra_errors,tra_errors_all 97 | def main(): 98 | # usage: python main.py path_to_ground_truth path_to_predict_pose 99 | # load and preprocess data 100 | ground_truth_data = np.loadtxt(sys.argv[1]) 101 | predict_pose__data = np.loadtxt(sys.argv[2]) 102 | errors = calculate_sequence_error(ground_truth_data,predict_pose__data) 103 | 104 | rot,tra,tra_all_1 = calculate_ave_errors(errors) 105 | errors_array = np.array(errors) 106 | print(tra,rot) 107 | print(np.mean(tra),np.mean(rot)) 108 | 109 | if __name__ == "__main__": 110 | main() 111 | 112 | 113 | -------------------------------------------------------------------------------- /script/plot.sh: -------------------------------------------------------------------------------- 1 | python script/plot_path.py result/kitti_raw/2011_09_26_drive_$1_sync_path.txt dataset/kitti_raw_gt/2011_09_26_drive_$1_sync_gps.txt 2 | -------------------------------------------------------------------------------- /script/plot_path.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import sys 4 | def plot_path(): 5 | label=['ground truth','path 1','path 2'] 6 | name = str(sys.argv[1]).split('.')[-2].split('/')[-1] 7 | print(name) 8 | for i in range(1, len(sys.argv)): 9 | path_vo = np.loadtxt(sys.argv[i]) 10 | #plt.plot(path_vo[:,3],path_vo[:,7]) 11 | plt.plot(path_vo[:,3],path_vo[:,11],label=label[i-1]) 12 | min_x = np.min(path_vo[:,3:12:4],0) 13 | max_x = np.max(path_vo[:,3:12:4],0) 14 | mean_x= (min_x+max_x)/2 15 | diff_x = max_x -min_x 16 | max_diff = np.max(diff_x) 17 | print(min_x,max_x) 18 | plt.xlabel('x/m') 19 | plt.xlim(mean_x[0]-max_diff/2-0.1*max_diff,mean_x[0]+max_diff/2+0.1*max_diff) 20 | plt.ylim(mean_x[2]-max_diff/2-0.1*max_diff,mean_x[2]+max_diff/2+0.1*max_diff) 21 | plt.ylabel('y/m') 22 | plt.title('PATH') 23 | plt.legend() 24 | plt.savefig('result/'+name+'.pdf') 25 | plt.show() 26 | 27 | def plot_scale_filter(window=20): 28 | for i in range(1, min(len(sys.argv),3)): 29 | data = np.loadtxt(sys.argv[i]) 30 | data_new = [] 31 | for i in range(0,data.shape[0]-window): 32 | data_new.append(np.mean(data[i:i+window])) 33 | plt.plot(data_new) 34 | plt.xlabel('frame') 35 | plt.ylabel('scale') 36 | plt.title('SCALE') 37 | plt.show() 38 | 39 | 40 | def plot_scale(): 41 | label=['ground truth','path 1','path 2'] 42 | for i in range(1, min(len(sys.argv),4)): 43 | data = np.loadtxt(sys.argv[i]) 44 | data_sum = [data[0]] 45 | for j in range(1,data.shape[0]): 46 | data_sum.append(data_sum[-1]+data[j]) 47 | 48 | plt.plot(data,label=label[i-1]) 49 | plt.xlabel('frame') 50 | plt.ylabel('scale') 51 | plt.legend() 52 | plt.title('SCALE') 53 | plt.show() 54 | 55 | def plot_motion(): 56 | import transformation as tf 57 | color=['r','g','b','y'] 58 | label=['ground truth','path 1','path 2'] 59 | for i in range(1, len(sys.argv)): 60 | path_vo = np.loadtxt(sys.argv[i]) 61 | motion_vo = tf.pose2motion(path_vo) 62 | se_vo = tf.SEs2ses(path_vo) 63 | #plt.plot(path_vo[:,3],path_vo[:,7]) 64 | #plt.plot(se_vo[:,3]) 65 | #plt.plot(path_vo[:,7]/1000) 66 | #plt.plot(motion_vo[:,7]) 67 | #plt.plot(motion_vo[:,11]/10) 68 | plt.plot(path_vo[:,11],color[i-1],label=label[i-1]) 69 | plt.xlabel('frame') 70 | plt.ylabel('z/m') 71 | plt.title('PATH') 72 | plt.legend() 73 | plt.show() 74 | def plot_corr(): 75 | gt = np.loadtxt(sys.argv[1]) 76 | re = np.loadtxt(sys.argv[2]) 77 | re = re[1:] 78 | re_l = re.shape[0] 79 | er_ = np.abs((gt[:re_l]-re)) 80 | valid_1 = er_ >0.0 81 | er = np.loadtxt(sys.argv[3]) 82 | er = er 83 | er = er[1:] 84 | valid = (er <100) & valid_1 85 | corr = np.corrcoef(er,er_) 86 | print(corr) 87 | plt.plot(er[valid],'g',label='variance') 88 | plt.plot(er_[valid],'r',label='error') 89 | plt.legend() 90 | plt.show() 91 | 92 | def main(): 93 | if len(np.loadtxt(sys.argv[1]).shape)>1: 94 | plot_motion() 95 | plot_path() 96 | else: 97 | plot_scale() 98 | 99 | if __name__ == '__main__': 100 | main() 101 | -------------------------------------------------------------------------------- /script/score_calculation.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | def get_rpe(file_name): 4 | file_data = open(file_name) 5 | data= file_data.read() 6 | rpe_800 = data.split(',')[-7] 7 | return float(rpe_800) 8 | 9 | def get_scale(file_name): 10 | file_data = open(file_name) 11 | data= file_data.read() 12 | scale_800 = data.split(',')[-1][:-1] 13 | return float(rpe_800) 14 | def main(): 15 | if sys.argv[2]=='v': 16 | rpes = [] 17 | for i in range(0,10): 18 | file_name = sys.argv[1]+str(i) 19 | rpe = get_rpe(file_name) 20 | rpes.append(rpe) 21 | rpes =np.sort(rpes) 22 | print(np.mean(rpes[1:-1])) 23 | else: 24 | scales=[] 25 | for i in range(0,10): 26 | file_name = sys.argv[1]+str(i) 27 | scale = get_scale(file_name) 28 | scales.append(scale) 29 | scales =np.sort(scales) 30 | print(np.mean(scales[1:-1])) 31 | 32 | 33 | if __name__ == '__main__': 34 | main() 35 | -------------------------------------------------------------------------------- /script/transformation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | #import cv2 3 | #import pyrr 4 | from scipy.spatial.transform import Rotation as R 5 | 6 | def line2mat(line_data): 7 | mat = np.eye(4) 8 | mat[0:3,:] = line_data.reshape(3,4) 9 | return np.matrix(mat) 10 | def motion2pose(data): 11 | data_size = data.shape[0] 12 | all_pose = np.zeros((data_size+1,12)) 13 | temp = np.eye(4,4).reshape(1,16) 14 | all_pose[0,:] = temp[0,0:12] 15 | pose = np.matrix(np.eye(4,4)) 16 | for i in range(0,data_size): 17 | data_mat = line2mat(data[i,:]) 18 | pose = pose*data_mat 19 | pose_line = np.array(pose[0:3,:]).reshape(1,12) 20 | all_pose[i+1,:] = pose_line 21 | return all_pose 22 | 23 | def pose2motion(data): 24 | data_size = data.shape[0] 25 | all_motion = np.zeros((data_size-1,12)) 26 | for i in range(0,data_size-1): 27 | pose_curr = line2mat(data[i,:]) 28 | pose_next = line2mat(data[i+1,:]) 29 | motion = pose_curr.I*pose_next 30 | motion_line = np.array(motion[0:3,:]).reshape(1,12) 31 | all_motion[i,:] = motion_line 32 | return all_motion 33 | 34 | def SE2se(SE_data): 35 | result = np.zeros((6)) 36 | result[0:3] = np.array(SE_data[0:3,3].T) 37 | result[3:6] = SO2so(SE_data[0:3,0:3]).T 38 | return result 39 | def SO2so(SO_data): 40 | return R.from_dcm(SO_data).as_rotvec() 41 | 42 | def so2SO(so_data): 43 | return R.from_rotvec(so_data).as_dcm() 44 | 45 | def se2SE(se_data): 46 | result_mat = np.matrix(np.eye(4)) 47 | result_mat[0:3,0:3] = so2SO(se_data[3:6]) 48 | result_mat[0:3,3] = np.matrix(se_data[0:3]).T 49 | return result_mat 50 | ### can get wrong result 51 | def se_mean(se_datas): 52 | all_SE = np.matrix(np.eye(4)) 53 | for i in range(se_datas.shape[0]): 54 | se = se_datas[i,:] 55 | SE = se2SE(se) 56 | all_SE = all_SE*SE 57 | all_se = SE2se(all_SE) 58 | mean_se = all_se/se_datas.shape[0] 59 | return mean_se 60 | 61 | def ses_mean(se_datas): 62 | se_datas = np.array(se_datas) 63 | se_datas = np.transpose(se_datas.reshape(se_datas.shape[0],se_datas.shape[1],se_datas.shape[2]*se_datas.shape[3]),(0,2,1)) 64 | se_result = np.zeros((se_datas.shape[0],se_datas.shape[2])) 65 | for i in range(0,se_datas.shape[0]): 66 | mean_se = se_mean(se_datas[i,:,:]) 67 | se_result[i,:] = mean_se 68 | return se_result 69 | 70 | def ses2poses(data): 71 | data_size = data.shape[0] 72 | all_pose = np.zeros((data_size+1,12)) 73 | temp = np.eye(4,4).reshape(1,16) 74 | all_pose[0,:] = temp[0,0:12] 75 | pose = np.matrix(np.eye(4,4)) 76 | for i in range(0,data_size): 77 | data_mat = se2SE(data[i,:]) 78 | pose = pose*data_mat 79 | pose_line = np.array(pose[0:3,:]).reshape(1,12) 80 | all_pose[i+1,:] = pose_line 81 | return all_pose 82 | 83 | def SEs2ses(motion_data): 84 | data_size = motion_data.shape[0] 85 | ses = np.zeros((data_size,6)) 86 | for i in range(0,data_size): 87 | SE = np.matrix(np.eye(4)) 88 | SE[0:3,:] = motion_data[i,:].reshape(3,4) 89 | ses[i,:] = SE2se(SE) 90 | return ses 91 | 92 | def so2quat(so_data): 93 | so_data = np.array(so_data) 94 | theta = np.sqrt(np.sum(so_data*so_data)) 95 | axis = so_data/theta 96 | quat=np.zeros(4) 97 | quat[0:3] = np.sin(theta/2)*axis 98 | quat[3] = np.cos(theta/2) 99 | return quat 100 | 101 | def quat2so(quat_data): 102 | quat_data = np.array(quat_data) 103 | sin_half_theta = np.sqrt(np.sum(quat_data[0:3]*quat_data[0:3])) 104 | axis = quat_data[0:3]/sin_half_theta 105 | cos_half_theta = quat_data[3] 106 | theta = 2*np.arctan2(sin_half_theta,cos_half_theta) 107 | so = theta*axis 108 | return so 109 | 110 | # input so_datas batch*channel*height*width 111 | # return quat_datas batch*numner*channel 112 | def sos2quats(so_datas,mean_std=[[1],[1]]): 113 | so_datas = np.array(so_datas) 114 | so_datas = so_datas.reshape(so_datas.shape[0],so_datas.shape[1],so_datas.shape[2]*so_datas.shape[3]) 115 | so_datas = np.transpose(so_datas,(0,2,1)) 116 | quat_datas = np.zeros((so_datas.shape[0],so_datas.shape[1],4)) 117 | for i_b in range(0,so_datas.shape[0]): 118 | for i_p in range(0,so_datas.shape[1]): 119 | so_data = so_datas[i_b,i_p,:] 120 | quat_data = so2quat(so_data) 121 | quat_datas[i_b,i_p,:] = quat_data 122 | return quat_datas 123 | 124 | def quat2SO(quat_data): 125 | return R.from_quat(quat_data).as_dcm() 126 | 127 | 128 | def pos_quat2SE(quat_data): 129 | SO = R.from_quat(quat_data[3:7]).as_dcm() 130 | SE = np.matrix(np.eye(4)) 131 | SE[0:3,0:3] = np.matrix(SO) 132 | SE[0:3,3] = np.matrix(quat_data[0:3]).T 133 | SE = np.array(SE[0:3,:]).reshape(1,12) 134 | return SE 135 | 136 | 137 | def pos_quats2SEs(quat_datas): 138 | data_len = quat_datas.shape[0] 139 | SEs = np.zeros((data_len,12)) 140 | for i_data in range(0,data_len): 141 | SE = pos_quat2SE(quat_datas[i_data,:]) 142 | SEs[i_data,:] = SE 143 | return SEs 144 | 145 | 146 | -------------------------------------------------------------------------------- /sequence.txt: -------------------------------------------------------------------------------- 1 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0002_sync.txt 2 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0009_sync.txt 3 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0013_sync.txt 4 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0014_sync.txt 5 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0015_sync.txt 6 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0019_sync.txt 7 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0022_sync.txt 8 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0036_sync.txt 9 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0070_sync.txt 10 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0084_sync.txt 11 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0096_sync.txt 12 | /data_sdd/xiangwei/kitti_data/kitti_2011_09_26/2011_09_26_drive_0101_sync.txt 13 | -------------------------------------------------------------------------------- /src/calculate_height_pitch.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial import Delaunay 3 | import cv2 4 | import math 5 | import sys 6 | 7 | import time 8 | from estimate_road_norm import get_pitch 9 | from estimate_road_norm import get_pitch_ransac 10 | from estimate_road_norm import get_pitch_line_ransac 11 | from estimate_road_norm import get_inliers 12 | from estimate_road_norm import get_norm_svd 13 | from estimate_road_norm import estimate 14 | 15 | camera_focus=718.856 16 | camera_cx=607.1928 17 | camera_cy=185.2157 18 | 19 | print 'python calculate_height_pitch.py image_name_list_file feature_pos_path motion_path pose_path' 20 | #initialization 21 | image_name_list_file = sys.argv[1] 22 | feature_pos_path = sys.argv[2] 23 | camera_motion_path = sys.argv[3] 24 | camera_pose_path = sys.argv[4] 25 | image_name_list = open(image_name_list_file) 26 | image_name = image_name_list.readline() 27 | 28 | camera_motions = np.loadtxt(camera_motion_path) 29 | camera_motion_ts = camera_motions[:,3::4] 30 | 31 | camera_poses = np.loadtxt(camera_pose_path) 32 | camera_pose_ts = camera_poses[:,3::4] 33 | #flag 34 | flag_save_images = False 35 | flag_save_data = False 36 | flag_demo = False 37 | 38 | ransac_camera_heights = [] 39 | refined_camera_height_means = [] 40 | refined_camera_height_stds = [] 41 | refined_pitchs=[] 42 | refined_inlier_number=[] 43 | refined_camera_height_t_means = [] 44 | inlier_numbers=[] 45 | norm_prev = np.array((3,1),np.float) 46 | norm_norm = 1 47 | norm_norm_prev = 1 48 | pitch_deg_prev = 1 49 | height_prev = 1 50 | frame_count = 1 51 | while frame_count<4541: 52 | print '**********',frame_count,'*****************' 53 | # load image and feature points 54 | start_time = time.time() 55 | image_name = image_name_list.readline() 56 | image_name=image_name[:-1] 57 | img = cv2.imread(image_name) 58 | img_inlier_points = img.copy() 59 | feature_pos_name = feature_pos_path+"/"+str(frame_count)+".txt" 60 | points3d = np.loadtxt(feature_pos_path+str(frame_count)+".txt") 61 | 62 | estimated_pitch = get_pitch(camera_motion_ts[0:frame_count+1,0:3]) 63 | estimated_pitch_deg = estimated_pitch*180/3.1415926 64 | # triangulation 65 | points = points3d[:,0:2] 66 | points_3d_array = np.array(points3d) 67 | points_3d_array[:,0] = points_3d_array[:,2]*(points_3d_array [:,0]-camera_cx)/camera_focus 68 | points_3d_array[:,1] = points_3d_array[:,2]*(points_3d_array [:,1]-camera_cy)/camera_focus 69 | tri = Delaunay(points) 70 | triangle_ids = tri.simplices 71 | b_matrix = np.matrix(np.ones((3,1),np.float)) 72 | print("triangluation--- %s seconds ---" % (time.time() - start_time)) 73 | start_time = time.time() 74 | data = [] 75 | # road detection 76 | # calculate angles norm and height 77 | for triangle_id in triangle_ids: 78 | point_selected = points3d[triangle_id] 79 | a_array = np.array(point_selected) 80 | a_array[:,0] = a_array[:,2]*(a_array[:,0]-camera_cx)/camera_focus 81 | a_array[:,1] = a_array[:,2]*(a_array[:,1]-camera_cy)/camera_focus 82 | a_matrix = np.matrix(a_array) 83 | a_matrix_inv = a_matrix.I 84 | norm = a_matrix_inv*b_matrix 85 | norm_norm_2 = norm.T*norm#the square norm of norm 86 | height = 1/math.sqrt(norm_norm_2) 87 | if norm[1,0]<0: 88 | norm = -norm 89 | height = -height 90 | pitch = math.asin(-norm[1,0]/math.sqrt(norm_norm_2[0,0])) 91 | pitch_deg = pitch*180/3.1415926 92 | pitch_height = [norm[1,0]/math.sqrt(norm_norm_2[0,0]),pitch_deg,height] 93 | data.append(pitch_height) 94 | data = np.array(data) 95 | 96 | print("triangle calculate--- %s seconds ---" % (time.time() - start_time)) 97 | start_time = time.time() 98 | if flag_save_data: 99 | np.savetxt('result/data'+str(frame_count)+'.txt',data) 100 | # initial select by prior information 101 | data_sub = data[data[:,1]>estimated_pitch_deg-95]#>80 deg 102 | data_sub = data[data[:,1]80 deg 103 | data_sub = data_sub[data_sub[:,2]>0]#under 104 | data_id = [] 105 | # collect suitable points and triangle 106 | for i in range(0,triangle_ids.shape[0]): 107 | triangle_id = triangle_ids[i] 108 | pitch_deg = data[i,1] 109 | height = data[i,2] 110 | triangle_points = np.array(points[triangle_id],np.int32) 111 | if(pitch_deg>estimated_pitch_deg-95 and pitch_deg0): 113 | if(1): 114 | data_id.append(triangle_id[0]) 115 | data_id.append(triangle_id[1]) 116 | data_id.append(triangle_id[2]) 117 | pts = triangle_points.reshape((-1,1,2)) 118 | #cv2.polylines(img,[pts],True,(0,255,0)) 119 | #cv2.fillPoly(img,[pts],(0,255,0)) 120 | else: 121 | pts = triangle_points.reshape((-1,1,2)) 122 | #cv2.polylines(img,[pts],True,(0,255,255)) 123 | #cv2.fillPoly(img,[pts],(0,255,255)) 124 | else: 125 | pts = triangle_points.reshape((-1,1,2)) 126 | #cv2.polylines(img,[pts],True,(0,0,255)) 127 | #cv2.fillPoly(img,[pts],(0,0,255)) 128 | else: 129 | pts = triangle_points.reshape((-1,1,2)) 130 | #cv2.polylines(img,[pts],True,(255,0,0)) 131 | point_selected = points3d[data_id] 132 | if flag_save_data: 133 | np.savetxt('result/selected_points'+str(frame_count)+'.txt',point_selected) 134 | print 'suitable triangle',data_sub.shape[0] 135 | print 'suitable point :' ,point_selected.shape[0] 136 | 137 | print("select triangle--- %s seconds ---" % (time.time() - start_time)) 138 | start_time = time.time() 139 | # initial ransac 140 | if(point_selected.shape[0]>=12): 141 | # ransac 142 | a_array = np.array(point_selected) 143 | a_array[:,0] = a_array[:,2]*(a_array[:,0]-camera_cx)/camera_focus 144 | a_array[:,1] = a_array[:,2]*(a_array[:,1]-camera_cy)/camera_focus 145 | m,b = get_pitch_ransac(a_array,500,0.005) 146 | m_2,b_2 = get_pitch_line_ransac(a_array[:,1:],500,0.005) 147 | print 'road model ransac',m_2 148 | print 'inlier number',b_2 149 | inlier_id = get_inliers(m,points_3d_array[:,:],0.01) 150 | inliers = points_3d_array[inlier_id,:] 151 | inliers_2d = points[inlier_id,:] 152 | outliers_2d = points[inlier_id==False,:] 153 | 154 | road_model_ransac = np.matrix(m) 155 | norm = road_model_ransac[0,0:-1] 156 | h_bar = -road_model_ransac[0,-1] 157 | if norm[0,1]<0: 158 | norm = -norm 159 | h_bar = -h_bar 160 | norm_norm_2 = norm*norm.T 161 | norm_norm = math.sqrt(norm_norm_2)/h_bar 162 | norm = norm/h_bar 163 | else: 164 | norm = norm_prev.copy() 165 | norm_norm = norm_norm_prev 166 | ransac_camera_height = 1/norm_norm 167 | pitch = math.asin(norm[0,1]/norm_norm) 168 | pitch_deg = abs(pitch)*180/3.1415926 169 | 170 | ransac_camera_heights.append(ransac_camera_height) 171 | 172 | inlier_number = inliers.shape[0] 173 | inlier_numbers.append(inlier_number) 174 | 175 | print("ransac--- %s seconds ---" % (time.time() - start_time)) 176 | start_time = time.time() 177 | # refine n 178 | refined_norm = estimate(inliers) 179 | refined_norm = refined_norm[0:-1] 180 | if refined_norm[1]<0: 181 | refined_norm = -refined_norm 182 | refined_norm_matrix_temp = np.matrix(refined_norm) 183 | refined_norm_norm_2 = refined_norm_matrix_temp*refined_norm_matrix_temp.T 184 | refined_norm_norm=np.sqrt(refined_norm_norm_2) 185 | refined_norm = refined_norm/refined_norm_norm 186 | refined_norm_matrix = np.matrix(refined_norm) 187 | print 'refined norm', refined_norm 188 | refined_pitch = math.asin(refined_norm[0,1]) 189 | inliers_matrix = np.matrix(inliers) 190 | refined_pitchs.append(refined_pitch) 191 | # refine h 192 | refined_hs = inliers_matrix*refined_norm.T 193 | 194 | refined_h_mean = np.mean(refined_hs) 195 | refined_h_std = np.std(refined_hs) 196 | refined_camera_height_means.append(refined_h_mean) 197 | refined_camera_height_stds.append(refined_h_std) 198 | 199 | print("refine--- %s seconds ---" % (time.time() - start_time)) 200 | start_time = time.time() 201 | 202 | refined_h_ts = inliers[:,2]*math.sin(estimated_pitch)+inliers[:,1]*math.cos(estimated_pitch) 203 | refined_h_t_mean = np.mean(refined_h_ts) 204 | refined_camera_height_t_means.append(refined_h_t_mean) 205 | #demo 206 | if flag_demo: 207 | for point2d in inliers_2d: 208 | cv2.circle(img_inlier_points,(int(point2d[0]),int(point2d[1])),3,(0,255,0),-1) 209 | for point2d in outliers_2d: 210 | cv2.circle(img_inlier_points,(int(point2d[0]),int(point2d[1])),3,(0,0,255),-1) 211 | # cv2.putText(img,'Road Model :'+str(round(norm[0,0],3))+'X+'+str(round(norm[0,1],4))+'Y+'+str(round(norm[0,2],3))+'Z-1=0',(50,50),1,1,(200,128,0),2) 212 | cv2.putText(img,'Camera Height :'+str(round(refined_h_t_mean,3)),(50,80),1,1,(200,128,0),2) 213 | cv2.putText(img,'Pitch :'+str(round(refined_pitch*180/3.1415926,3)),(50,110),1,1,(200,128,0),2) 214 | cv2.putText(img,'Frame counter :'+str(round(frame_count,3)),(50,130),1,1,(200,128,0),2) 215 | 216 | cv2.imshow("image",img) 217 | cv2.imshow("image_inlier",img_inlier_points) 218 | cv2.waitKey(1) 219 | if flag_save_images: 220 | cv2.imwrite("result/result"+str(frame_count)+".png",img) 221 | 222 | #system 223 | frame_count = frame_count+1 224 | norm_prev = norm.copy() 225 | norm_norm_prev = norm_norm 226 | height_prev = refined_h_mean 227 | #save data 228 | ransac_camera_heights = np.array(ransac_camera_heights) 229 | np.savetxt('result_heights_line_ransac.txt',ransac_camera_heights) 230 | 231 | refined_camera_height_means = np.array(refined_camera_height_means) 232 | np.savetxt('refined_camera_height_means.txt',refined_camera_height_means) 233 | 234 | refined_camera_height_stds =np.array(refined_camera_height_stds) 235 | np.savetxt('refined_camera_height_stds.txt',refined_camera_height_stds) 236 | 237 | refined_camera_height_t_means = np.array(refined_camera_height_t_means) 238 | np.savetxt('refined_camera_height_t_means.txt',refined_camera_height_t_means) 239 | 240 | refined_pitchs = np.array(refined_pitchs) 241 | np.savetxt('refined_pitch.txt',refined_pitchs) 242 | 243 | inlier_numbers = np.array(inlier_numbers) 244 | np.savetxt('inlier_numbers.txt',inlier_numbers) 245 | -------------------------------------------------------------------------------- /src/calculate_height_pitch_eval.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial import Delaunay 3 | import cv2 4 | import math 5 | import sys 6 | 7 | import time 8 | from estimate_road_norm import get_pitch 9 | from estimate_road_norm import get_pitch_ransac 10 | from estimate_road_norm import get_pitch_line_ransac 11 | from estimate_road_norm import get_inliers 12 | from estimate_road_norm import get_norm_svd 13 | from estimate_road_norm import estimate 14 | 15 | camera_focus=718.856 16 | camera_cx=607.1928 17 | camera_cy=185.2157 18 | #camera_focus=707.0912 19 | #camera_cx=601.8873 20 | #camera_cy=183.1104 21 | 22 | print 'python calculate_height_pitch.py image_name_list_file feature_pos_path motion_path pose_path' 23 | #initialization 24 | input_id = sys.argv[1] 25 | input_date = sys.argv[2] 26 | frame_number = int(sys.argv[3]) 27 | iter_number = int(sys.argv[4]) 28 | #image_name_list_file = 'input/kitti_image_'+input_id+'_list.txt' 29 | feature_pos_path = 'result/kitti_'+input_id+'/kitti_'+input_id+'_feature_'+input_date+'/' 30 | camera_motion_path = 'result/kitti_'+input_id+'/kitti_'+input_id+'_motion_'+input_date+'.txt' 31 | camera_pose_path = 'result/kitti_'+input_id+'/kitti_'+input_id+'_pose_'+input_date+'.txt' 32 | #image_name_list = open(image_name_list_file) 33 | #image_name = image_name_list.readline() 34 | 35 | begin_time = time.time() 36 | for case_i in range(0,10): 37 | camera_motions = np.loadtxt(camera_motion_path) 38 | camera_motion_ts = camera_motions[:,3::4] 39 | 40 | camera_poses = np.loadtxt(camera_pose_path) 41 | camera_pose_ts = camera_poses[:,3::4] 42 | #flag 43 | flag_save_images = False 44 | flag_save_data = False 45 | flag_demo = False 46 | 47 | ransac_camera_heights = [] 48 | refined_camera_height_means = [] 49 | refined_camera_height_stds = [] 50 | refined_pitchs=[] 51 | refined_inlier_number=[] 52 | refined_camera_height_t_means = [] 53 | inlier_numbers=[] 54 | norm_prev = np.array((3,1),np.float) 55 | norm_norm = 1 56 | norm_norm_prev = 1 57 | pitch_deg_prev = 1 58 | height_prev = 1 59 | frame_count = 1 60 | while frame_countestimated_pitch_deg-95]#>80 deg 120 | data_sub = data[data[:,1]80 deg 121 | data_sub = data_sub[data_sub[:,2]>0]#under 122 | data_id = [] 123 | # collect suitable points and triangle 124 | for i in range(0,triangle_ids.shape[0]): 125 | triangle_id = triangle_ids[i] 126 | pitch_deg = data[i,1] 127 | height = data[i,2] 128 | triangle_points = np.array(points[triangle_id],np.int32) 129 | if(pitch_deg>estimated_pitch_deg-95 and pitch_deg0): 131 | if(1): 132 | data_id.append(triangle_id[0]) 133 | data_id.append(triangle_id[1]) 134 | data_id.append(triangle_id[2]) 135 | pts = triangle_points.reshape((-1,1,2)) 136 | #cv2.polylines(img,[pts],True,(0,255,0)) 137 | #cv2.fillPoly(img,[pts],(0,255,0)) 138 | else: 139 | pts = triangle_points.reshape((-1,1,2)) 140 | #cv2.polylines(img,[pts],True,(0,255,255)) 141 | #cv2.fillPoly(img,[pts],(0,255,255)) 142 | else: 143 | pts = triangle_points.reshape((-1,1,2)) 144 | #cv2.polylines(img,[pts],True,(0,0,255)) 145 | #cv2.fillPoly(img,[pts],(0,0,255)) 146 | else: 147 | pts = triangle_points.reshape((-1,1,2)) 148 | #cv2.polylines(img,[pts],True,(255,0,0)) 149 | point_selected = points3d[data_id] 150 | if flag_save_data: 151 | np.savetxt('result/selected_points'+str(frame_count)+'.txt',point_selected) 152 | #print 'suitable triangle',data_sub.shape[0] 153 | #print 'suitable point :' ,point_selected.shape[0] 154 | 155 | #print("select triangle--- %s seconds ---" % (time.time() - start_time)) 156 | start_time = time.time() 157 | # initial ransac 158 | if(point_selected.shape[0]>=12): 159 | # ransac 160 | a_array = np.array(point_selected) 161 | a_array[:,0] = a_array[:,2]*(a_array[:,0]-camera_cx)/camera_focus 162 | a_array[:,1] = a_array[:,2]*(a_array[:,1]-camera_cy)/camera_focus 163 | m,b = get_pitch_ransac(a_array,iter_number,0.005) 164 | #m_2,b_2 = get_pitch_line_ransac(a_array[:,1:],500,0.005) 165 | #print 'road model ransac',m_2 166 | #print 'inlier number',b_2 167 | inlier_id = get_inliers(m,a_array,0.01) 168 | #print inlier_id 169 | inliers = a_array[inlier_id,:] 170 | inlier_id_all = get_inliers(m,points_3d_array,0.01) 171 | inliers_2d = points[inlier_id_all,:] 172 | outliers_2d = points[inlier_id_all==False,:] 173 | 174 | road_model_ransac = np.matrix(m) 175 | norm = road_model_ransac[0,0:-1] 176 | h_bar = -road_model_ransac[0,-1] 177 | if norm[0,1]<0: 178 | norm = -norm 179 | h_bar = -h_bar 180 | norm_norm_2 = norm*norm.T 181 | norm_norm = math.sqrt(norm_norm_2)/h_bar 182 | norm = norm/h_bar 183 | else: 184 | norm = norm_prev.copy() 185 | norm_norm = norm_norm_prev 186 | ransac_camera_height = 1/norm_norm 187 | pitch = math.asin(norm[0,1]/norm_norm) 188 | pitch_deg = abs(pitch)*180/3.1415926 189 | 190 | ransac_camera_heights.append(ransac_camera_height) 191 | 192 | inlier_number = inliers.shape[0] 193 | inlier_numbers.append(inlier_number) 194 | 195 | #print("ransac--- %s seconds ---" % (time.time() - start_time)) 196 | start_time = time.time() 197 | # refine n 198 | refined_norm = estimate(inliers) 199 | refined_norm = refined_norm[0:-1] 200 | if refined_norm[1]<0: 201 | refined_norm = -refined_norm 202 | refined_norm_matrix_temp = np.matrix(refined_norm) 203 | refined_norm_norm_2 = refined_norm_matrix_temp*refined_norm_matrix_temp.T 204 | refined_norm_norm=np.sqrt(refined_norm_norm_2) 205 | refined_norm = refined_norm/refined_norm_norm 206 | refined_norm_matrix = np.matrix(refined_norm) 207 | #print 'refined norm', refined_norm 208 | refined_pitch = math.asin(refined_norm[0,1]) 209 | inliers_matrix = np.matrix(inliers) 210 | refined_pitchs.append(refined_pitch) 211 | # refine h 212 | refined_hs = inliers_matrix*refined_norm.T 213 | 214 | refined_h_mean = np.mean(refined_hs) 215 | refined_h_std = np.std(refined_hs) 216 | refined_camera_height_means.append(refined_h_mean) 217 | refined_camera_height_stds.append(refined_h_std) 218 | 219 | #print("refine--- %s seconds ---" % (time.time() - start_time)) 220 | start_time = time.time() 221 | 222 | refined_h_ts = inliers[:,2]*math.sin(estimated_pitch)+inliers[:,1]*math.cos(estimated_pitch) 223 | refined_h_t_mean = np.mean(refined_h_ts) 224 | refined_camera_height_t_means.append(refined_h_t_mean) 225 | #demo 226 | #if flag_demo: 227 | # for point2d in inliers_2d: 228 | # cv2.circle(img_inlier_points,(int(point2d[0]),int(point2d[1])),3,(0,255,0),-1) 229 | # for point2d in outliers_2d: 230 | # cv2.circle(img_inlier_points,(int(point2d[0]),int(point2d[1])),3,(0,0,255),-1) 231 | # cv2.putText(img,'Road Model :'+str(round(norm[0,0],3))+'X+'+str(round(norm[0,1],4))+'Y+'+str(round(norm[0,2],3))+'Z-1=0',(50,50),1,1,(200,128,0),2) 232 | #cv2.putText(img,'Camera Height :'+str(round(refined_h_t_mean,3)),(50,80),1,1,(200,128,0),2) 233 | #cv2.putText(img,'Pitch :'+str(round(refined_pitch*180/3.1415926,3)),(50,110),1,1,(200,128,0),2) 234 | #cv2.putText(img,'Frame counter :'+str(round(frame_count,3)),(50,130),1,1,(200,128,0),2) 235 | 236 | #cv2.imshow("image",img) 237 | #cv2.imshow("image_inlier",img_inlier_points) 238 | #cv2.waitKey(1) 239 | #if flag_save_images: 240 | #cv2.imwrite("result/result"+str(frame_count)+".png",img) 241 | 242 | #system 243 | frame_count = frame_count+1 244 | norm_prev = norm.copy() 245 | norm_norm_prev = norm_norm 246 | height_prev = refined_h_mean 247 | #save data 248 | 249 | ransac_camera_heights = np.array(ransac_camera_heights) 250 | np.savetxt('eval_ransac/result_heights_plane_ransac_'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt',ransac_camera_heights) 251 | 252 | refined_camera_height_means = np.array(refined_camera_height_means) 253 | np.savetxt('eval_ransac/refined_camera_height_means'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_camera_height_means) 254 | 255 | refined_camera_height_stds =np.array(refined_camera_height_stds) 256 | np.savetxt('eval_ransac/refined_camera_height_stds'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_camera_height_stds) 257 | 258 | refined_camera_height_t_means = np.array(refined_camera_height_t_means) 259 | np.savetxt('eval_ransac/refined_camera_height_t_means'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_camera_height_t_means) 260 | 261 | refined_pitchs = np.array(refined_pitchs) 262 | np.savetxt('eval_ransac/refined_pitch'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_pitchs) 263 | 264 | inlier_numbers = np.array(inlier_numbers) 265 | np.savetxt('eval_ransac/inlier_numbers'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',inlier_numbers) 266 | 267 | end_time = time.time() 268 | np.savetxt('time'+input_id++str(iter_number)+'.txt',np.array([(end_time-begin_time)/(frame_number*10)])) 269 | print 'ave_time', (end_time-begin_time)/frame_number 270 | -------------------------------------------------------------------------------- /src/calculate_height_pitch_eval_line.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial import Delaunay 3 | import cv2 4 | import math 5 | import sys 6 | 7 | import time 8 | from estimate_road_norm import get_pitch 9 | from estimate_road_norm import get_pitch_ransac 10 | from estimate_road_norm import get_pitch_line_ransac 11 | from estimate_road_norm import get_inliers 12 | from estimate_road_norm import get_norm_svd 13 | from estimate_road_norm import estimate 14 | from estimate_road_norm import estimate_line 15 | 16 | camera_focus=718.856 17 | camera_cx=607.1928 18 | camera_cy=185.2157 19 | #camera_focus=707.0912 20 | #camera_cx=601.8873 21 | #camera_cy=183.1104 22 | 23 | print 'python calculate_height_pitch.py image_name_list_file feature_pos_path motion_path pose_path' 24 | #initialization 25 | input_id = sys.argv[1] 26 | input_date = sys.argv[2] 27 | frame_number = int(sys.argv[3]) 28 | iter_number = int(sys.argv[4]) 29 | #image_name_list_file = 'input/kitti_image_'+input_id+'_list.txt' 30 | feature_pos_path = 'result/kitti_'+input_id+'/kitti_'+input_id+'_feature_'+input_date+'/' 31 | camera_motion_path = 'result/kitti_'+input_id+'/kitti_'+input_id+'_motion_'+input_date+'.txt' 32 | camera_pose_path = 'result/kitti_'+input_id+'/kitti_'+input_id+'_pose_'+input_date+'.txt' 33 | #image_name_list = open(image_name_list_file) 34 | #image_name = image_name_list.readline() 35 | 36 | begin_time = time.time() 37 | for case_i in range(0,10): 38 | camera_motions = np.loadtxt(camera_motion_path) 39 | camera_motion_ts = camera_motions[:,3::4] 40 | 41 | camera_poses = np.loadtxt(camera_pose_path) 42 | camera_pose_ts = camera_poses[:,3::4] 43 | #flag 44 | flag_save_images = False 45 | flag_save_data = False 46 | flag_demo = False 47 | 48 | ransac_camera_heights = [] 49 | refined_camera_height_means = [] 50 | refined_camera_height_stds = [] 51 | refined_pitchs=[] 52 | refined_inlier_number=[] 53 | refined_camera_height_t_means = [] 54 | inlier_numbers=[] 55 | norm_prev = np.array((3,1),np.float) 56 | norm_norm = 1 57 | norm_norm_prev = 1 58 | pitch_deg_prev = 1 59 | height_prev = 1 60 | frame_count = 1 61 | while frame_countestimated_pitch_deg-95]#>80 deg 120 | data_sub = data[data[:,1]80 deg 121 | data_sub = data_sub[data_sub[:,2]>0]#under 122 | data_id = [] 123 | # collect suitable points and triangle 124 | for i in range(0,triangle_ids.shape[0]): 125 | triangle_id = triangle_ids[i] 126 | pitch_deg = data[i,1] 127 | height = data[i,2] 128 | triangle_points = np.array(points[triangle_id],np.int32) 129 | if(pitch_deg>estimated_pitch_deg-95 and pitch_deg0): 131 | if(1): 132 | data_id.append(triangle_id[0]) 133 | data_id.append(triangle_id[1]) 134 | data_id.append(triangle_id[2]) 135 | pts = triangle_points.reshape((-1,1,2)) 136 | #cv2.polylines(img,[pts],True,(0,255,0)) 137 | #cv2.fillPoly(img,[pts],(0,255,0)) 138 | else: 139 | pts = triangle_points.reshape((-1,1,2)) 140 | #cv2.polylines(img,[pts],True,(0,255,255)) 141 | #cv2.fillPoly(img,[pts],(0,255,255)) 142 | else: 143 | pts = triangle_points.reshape((-1,1,2)) 144 | #cv2.polylines(img,[pts],True,(0,0,255)) 145 | #cv2.fillPoly(img,[pts],(0,0,255)) 146 | else: 147 | pts = triangle_points.reshape((-1,1,2)) 148 | #cv2.polylines(img,[pts],True,(255,0,0)) 149 | point_selected = points3d[data_id] 150 | if flag_save_data: 151 | np.savetxt('result/selected_points'+str(frame_count)+'.txt',point_selected) 152 | np.savetxt('result/selected_points'+str(frame_count)+'.txt',point_selected) 153 | #print 'suitable triangle',data_sub.shape[0] 154 | #print 'suitable point :' ,point_selected.shape[0] 155 | 156 | #print("select triangle--- %s seconds ---" % (time.time() - start_time)) 157 | start_time = time.time() 158 | # initial ransac 159 | if(point_selected.shape[0]>=12): 160 | # ransac 161 | a_array = np.array(point_selected) 162 | a_array[:,0] = a_array[:,2]*(a_array[:,0]-camera_cx)/camera_focus 163 | a_array[:,1] = a_array[:,2]*(a_array[:,1]-camera_cy)/camera_focus 164 | #m,b = get_pitch_ransac(a_array,iter_number,0.005) 165 | m_2,b_2 = get_pitch_line_ransac(a_array[:,1:],iter_number,0.005) 166 | #print 'road model ransac',m_2 167 | #print 'inlier number',b_2 168 | inlier_id = get_inliers(m_2,a_array[:,1:],0.01) 169 | #print inlier_id 170 | inliers = a_array[inlier_id,:] 171 | inlier_id_all = get_inliers(m_2,points_3d_array[:,1:],0.01) 172 | inliers_2d = points[inlier_id_all,:] 173 | outliers_2d = points[inlier_id_all==False,:] 174 | 175 | road_model_ransac = np.matrix(m_2) 176 | norm = road_model_ransac[0,0:-1] 177 | h_bar = -road_model_ransac[0,-1] 178 | if norm[0,1]<0: 179 | norm = -norm 180 | h_bar = -h_bar 181 | norm_norm_2 = norm*norm.T 182 | norm_norm = math.sqrt(norm_norm_2)/h_bar 183 | norm = norm/h_bar 184 | else: 185 | norm = norm_prev.copy() 186 | norm_norm = norm_norm_prev 187 | ransac_camera_height = 1/norm_norm 188 | pitch = math.asin(norm[0,1]/norm_norm) 189 | pitch_deg = abs(pitch)*180/3.1415926 190 | 191 | ransac_camera_heights.append(ransac_camera_height) 192 | 193 | inlier_number = inliers.shape[0] 194 | inlier_numbers.append(inlier_number) 195 | 196 | #print("ransac--- %s seconds ---" % (time.time() - start_time)) 197 | start_time = time.time() 198 | # refine n 199 | refined_norm = estimate_line(inliers[:,1:]) 200 | refined_norm = refined_norm[0:-1] 201 | if refined_norm[1]<0: 202 | refined_norm = -refined_norm 203 | refined_norm_matrix_temp = np.matrix(refined_norm) 204 | refined_norm_norm_2 = refined_norm_matrix_temp*refined_norm_matrix_temp.T 205 | refined_norm_norm=np.sqrt(refined_norm_norm_2) 206 | refined_norm = refined_norm/refined_norm_norm 207 | refined_norm_matrix = np.matrix(refined_norm) 208 | #print 'refined norm', refined_norm 209 | refined_pitch = math.asin(refined_norm[0,0]) 210 | inliers_matrix = np.matrix(inliers) 211 | refined_pitchs.append(refined_pitch) 212 | # refine h 213 | refined_hs = inliers_matrix[:,1:]*refined_norm.T 214 | 215 | refined_h_mean = np.mean(refined_hs) 216 | refined_h_std = np.std(refined_hs) 217 | refined_camera_height_means.append(refined_h_mean) 218 | refined_camera_height_stds.append(refined_h_std) 219 | 220 | #print("refine--- %s seconds ---" % (time.time() - start_time)) 221 | start_time = time.time() 222 | 223 | refined_h_ts = inliers[:,2]*math.sin(estimated_pitch)+inliers[:,1]*math.cos(estimated_pitch) 224 | refined_h_t_mean = np.mean(refined_h_ts) 225 | refined_camera_height_t_means.append(refined_h_t_mean) 226 | #demo 227 | #if flag_demo: 228 | # for point2d in inliers_2d: 229 | # cv2.circle(img_inlier_points,(int(point2d[0]),int(point2d[1])),3,(0,255,0),-1) 230 | # for point2d in outliers_2d: 231 | # cv2.circle(img_inlier_points,(int(point2d[0]),int(point2d[1])),3,(0,0,255),-1) 232 | # cv2.putText(img,'Road Model :'+str(round(norm[0,0],3))+'X+'+str(round(norm[0,1],4))+'Y+'+str(round(norm[0,2],3))+'Z-1=0',(50,50),1,1,(200,128,0),2) 233 | #cv2.putText(img,'Camera Height :'+str(round(refined_h_t_mean,3)),(50,80),1,1,(200,128,0),2) 234 | #cv2.putText(img,'Pitch :'+str(round(refined_pitch*180/3.1415926,3)),(50,110),1,1,(200,128,0),2) 235 | #cv2.putText(img,'Frame counter :'+str(round(frame_count,3)),(50,130),1,1,(200,128,0),2) 236 | 237 | #cv2.imshow("image",img) 238 | #cv2.imshow("image_inlier",img_inlier_points) 239 | #cv2.waitKey(1) 240 | #if flag_save_images: 241 | #cv2.imwrite("result/result"+str(frame_count)+".png",img) 242 | 243 | #system 244 | frame_count = frame_count+1 245 | norm_prev = norm.copy() 246 | norm_norm_prev = norm_norm 247 | height_prev = refined_h_mean 248 | #save data 249 | 250 | ransac_camera_heights = np.array(ransac_camera_heights) 251 | np.savetxt('eval_ransac_line/result_heights_plane_ransac_'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt',ransac_camera_heights) 252 | 253 | refined_camera_height_means = np.array(refined_camera_height_means) 254 | np.savetxt('eval_ransac_line/refined_camera_height_means'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_camera_height_means) 255 | 256 | refined_camera_height_stds =np.array(refined_camera_height_stds) 257 | np.savetxt('eval_ransac_line/refined_camera_height_stds'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_camera_height_stds) 258 | 259 | refined_camera_height_t_means = np.array(refined_camera_height_t_means) 260 | np.savetxt('eval_ransac_line/refined_camera_height_t_means'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_camera_height_t_means) 261 | 262 | refined_pitchs = np.array(refined_pitchs) 263 | np.savetxt('eval_ransac_line/refined_pitch'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',refined_pitchs) 264 | 265 | inlier_numbers = np.array(inlier_numbers) 266 | np.savetxt('eval_ransac_line/inlier_numbers'+input_id+'_'+input_date+'_'+str(iter_number)+'_'+str(case_i)+'.txt''.txt',inlier_numbers) 267 | 268 | end_time = time.time() 269 | print 'ave_time', (end_time-begin_time)/frame_number 270 | -------------------------------------------------------------------------------- /src/detector.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import sys 3 | import numpy as np 4 | lk_params = dict(winSize = (21, 21), 5 | #maxLevel = 3, 6 | criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)) 7 | 8 | class FeatureDetector: 9 | def __init__(self,threshold = 0.000007,bucket_size=30,density=2): 10 | self.akaze = cv2.AKAZE_create(threshold=threshold) 11 | self.bucket_size = bucket_size 12 | self.density = density 13 | def detect(self,image): 14 | kp_akaze = self.akaze.detect(image,None) 15 | px_cur = np.array([x.pt for x in kp_akaze], dtype=np.float32) 16 | return bucket(px_cur,self.bucket_size,self.density) 17 | 18 | def bucket(self,features,bucket_size=30,density=2): 19 | u_max,v_max = np.max(features,0) 20 | u_min,v_min = np.min(features,0) 21 | print(u_min,v_min,u_max,v_max) 22 | bucket_x = 1+(u_max )//bucket_size 23 | bucket_y = 1+(v_max )//bucket_size 24 | print(bucket_y) 25 | bucket = [] 26 | for i in range(int(bucket_y)): 27 | buc = [] 28 | for j in range(int(bucket_x)): 29 | buc.append([]) 30 | bucket.append(buc) 31 | print(len(bucket)) 32 | i_feature = 0 33 | for feature in features: 34 | u = int(feature[0])//bucket_size 35 | v = int(feature[1])//bucket_size 36 | bucket[v][u].append(i_feature) 37 | i_feature+=1 38 | 39 | #print(bucket) 40 | new_feature=[] 41 | for i in range(int(bucket_y)): 42 | for j in range(int(bucket_x)): 43 | feature_id = bucket[i][j] 44 | np.random.shuffle(feature_id) 45 | for k in range(min(density,len(feature_id))): 46 | new_feature.append(features[feature_id[k]]) 47 | 48 | return np.array(new_feature) 49 | 50 | 51 | def akaze(image): 52 | akaze = cv2.AKAZE_create(threshold=0.000007) 53 | kp_akaze = akaze.detect(image,None) 54 | px_cur = np.array([x.pt for x in kp_akaze], dtype=np.float32) 55 | return px_cur 56 | 57 | def fast(image): 58 | detector = cv2.FastFeatureDetector_create(10, nonmaxSuppression=True) 59 | kp_akaze = detector.detect(image,None) 60 | #img_akaze = cv2.drawKeypoints(image,kp_akaze,image,color=(255,0,0)) 61 | #cv2.imshow('AKAZE',img_akaze) 62 | #cv2.waitKey(0) 63 | 64 | # ref libviso 65 | def bucket(features,bucket_size=30,density=2): 66 | u_max,v_max = np.max(features,0) 67 | u_min,v_min = np.min(features,0) 68 | print(u_min,v_min,u_max,v_max) 69 | bucket_x = 1+(u_max )//bucket_size 70 | bucket_y = 1+(v_max )//bucket_size 71 | print(bucket_y) 72 | bucket = [] 73 | for i in range(int(bucket_y)): 74 | buc = [] 75 | for j in range(int(bucket_x)): 76 | buc.append([]) 77 | bucket.append(buc) 78 | print(len(bucket)) 79 | i_feature = 0 80 | for feature in features: 81 | u = int(feature[0])//bucket_size 82 | v = int(feature[1])//bucket_size 83 | bucket[v][u].append(i_feature) 84 | i_feature+=1 85 | 86 | #print(bucket) 87 | new_feature=[] 88 | for i in range(int(bucket_y)): 89 | for j in range(int(bucket_x)): 90 | feature_id = bucket[i][j] 91 | np.random.shuffle(feature_id) 92 | for k in range(min(density,len(feature_id))): 93 | new_feature.append(features[feature_id[k]]) 94 | 95 | return np.array(new_feature) 96 | 97 | def motion_estimarion(feature_ref,feature_target,fx,cx,cy): 98 | camera_matrix = np.eye(3) 99 | camera_matrix[0,0] = camera_matrix[1,1] = fx 100 | camera_matrix[0,2] = cx 101 | camera_matrix[1,2] = cy 102 | 103 | 104 | E, mask = cv2.findEssentialMat(feature_ref, feature_target,cameraMatrix = camera_matrix , method=cv2.RANSAC, prob=0.999, threshold=1.0) 105 | _, R, t, mask,points_3d = cv2.recoverPose(E, feature_ref,\ 106 | feature_target,cameraMatrix=camera_matrix,distanceThresh=100) 107 | print(t) 108 | mask_bool = np.array(mask>0).reshape(-1) 109 | points_3d_selected = points_3d[:,mask_bool].T 110 | 111 | def feature_tracking(image_ref, image_cur, px_ref): 112 | kp2, st, err = cv2.calcOpticalFlowPyrLK(image_ref, image_cur, px_ref, None, **lk_params) #shape: [k,2] [k,1] [k,1] 113 | st = st.reshape(st.shape[0]) 114 | kp1 = px_ref[st == 1] 115 | kp2 = kp2[st == 1] 116 | 117 | return kp1, kp2 118 | 119 | 120 | 121 | def feature_detection(image,image_show): 122 | features = akaze(image) 123 | fast(image) 124 | features = bucket(features) 125 | print(features.shape) 126 | draw_feature(image_show,features) 127 | return features 128 | 129 | def draw_feature(img,feature,color=(255,255,0)): 130 | for i in range(feature.shape[0]): 131 | cv2.circle(img,(int(feature[i,0]),int(feature[i,1])),3,color,-1) 132 | def main(): 133 | image_name_file = open(sys.argv[1]) 134 | image_name_file.readline() 135 | image_names = image_name_file.read().split('\n') 136 | begin_id = 312 137 | image_id = 0 138 | image_last=None 139 | detector = FeatureDetector() 140 | for image_name in image_names: 141 | if image_id< begin_id: 142 | image_id+=1 143 | continue 144 | print(image_name) 145 | image = cv2.imread(image_name) 146 | image_show = image.copy() 147 | #features = feature_detection(image,image_show) 148 | features = detector.detect(image) 149 | if(image_last is not None): 150 | feature_ref,feature_target = feature_tracking(image,image_last,features) 151 | draw_feature(image_show,feature_ref,(0,255,255)) 152 | draw_feature(image_last,feature_target,(0,255,255)) 153 | motion_estimarion(feature_ref,feature_target,716,607,189) 154 | print(len(feature_ref)) 155 | cv2.imshow('image_last',image_last) 156 | cv2.imshow('image',image_show) 157 | cv2.waitKey(0) 158 | image_last = image.copy() 159 | image_id+=1 160 | 161 | if __name__ == '__main__': 162 | main() 163 | -------------------------------------------------------------------------------- /src/estimate_road_norm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial import Delaunay 3 | import cv2 4 | import math 5 | import sys 6 | from thirdparty.Ransac.ransac import * 7 | 8 | def augment(xyzs): 9 | axyz = np.ones((len(xyzs), 4)) 10 | axyz[:, :3] = xyzs 11 | return axyz 12 | 13 | def estimate(xyzs): 14 | axyz = augment(xyzs[:3]) 15 | return np.linalg.svd(axyz)[-1][-1, :] 16 | 17 | def is_inlier(coeffs, xyz, threshold): 18 | return np.abs(coeffs.dot(augment([xyz]).T)) < threshold 19 | 20 | def get_norm_svd(camera_motion_ts): 21 | U, s, V = np.linalg.svd(camera_motion_ts.T, full_matrices=True) 22 | estimated_norm = U[:,2] 23 | if estimated_norm[1]<0: 24 | estimated_norm = -estimated_norm 25 | estimated_norm = np.matrix(estimated_norm) 26 | return estimated_norm 27 | 28 | def get_pitch_svd(camera_motion_ts): 29 | U, s, V = np.linalg.svd(camera_motion_ts.T, full_matrices=True) 30 | estimated_norm = U[:,2] 31 | if estimated_norm[1]<0: 32 | estimated_norm = -estimated_norm 33 | estimated_norm = np.matrix(estimated_norm) 34 | norm_norm_2 = estimated_norm*estimated_norm.T 35 | norm_norm=np.sqrt(norm_norm_2) 36 | estimated_pitch = math.asin(estimated_norm[0,1]/norm_norm_2) 37 | return estimated_pitch 38 | 39 | def augment_line(xys): 40 | axy = np.ones((len(xys), 3)) 41 | axy[:, :2] = xys 42 | return axy 43 | 44 | def estimate_line(xys): 45 | axy = augment_line(xys[:2]) 46 | return np.linalg.svd(axy)[-1][-1, :] 47 | 48 | def is_inlier_line(coeffs, xy, threshold): 49 | return np.abs(coeffs.dot(augment_line([xy]).T)) < threshold 50 | 51 | 52 | def get_pitch(camera_motion_ts): 53 | camera_motion = np.sum(camera_motion_ts,0) 54 | camera_motion = np.matrix(camera_motion) 55 | motion_norm_2 = camera_motion*camera_motion.T 56 | motion_norm=np.sqrt(motion_norm_2) 57 | estimated_pitch = math.asin(-camera_motion[0,1]/motion_norm_2) 58 | return estimated_pitch 59 | 60 | def get_pitch_line_ransac(road_points,max_iterations,threshold): 61 | points_number = road_points.shape[0] 62 | goal_inliers = points_number*0.8 63 | m, b = run_ransac(road_points, estimate_line, lambda x, y: is_inlier_line(x, y, threshold), 2, goal_inliers, max_iterations) 64 | return m,b 65 | 66 | def get_pitch_ransac(road_points,max_iterations,threshold): 67 | points_number = road_points.shape[0] 68 | goal_inliers = points_number*0.8 69 | m, b = run_ransac(road_points, estimate, lambda x, y: is_inlier(x, y, threshold), 3, goal_inliers, max_iterations) 70 | return m,b 71 | def get_inliers(parameter,data,threshold): 72 | parameter = np.matrix(parameter) 73 | data = np.matrix(data) 74 | error = data*(parameter[0,0:-1].T)+parameter[0,-1] 75 | error = np.abs(np.array(error)) 76 | error = error.T 77 | error=error[0] 78 | return error0.5) 36 | return np.array(valid_id) 37 | 38 | 39 | class GraphGrow: 40 | def __init__(self,threshold_angle=8): 41 | self.threshold_angle = threshold_angle 42 | self.threshold_height= 0.2 43 | self.graph = [] 44 | self.proposal=[] 45 | self.height_invs =[] 46 | self.angles =[] 47 | def graph_construction(self,triangle_ids): 48 | graph = [] 49 | graph_meta = [] 50 | max_id = np.max(triangle_ids) 51 | for i in range(0,max_id+1): 52 | graph_meta.append(set()) 53 | for i in range(0,triangle_ids.shape[0]): 54 | graph.append([]) 55 | for i in range(0,triangle_ids.shape[0]): 56 | a,b,c = triangle_ids[i] 57 | intersect = graph_meta[a].intersection(graph_meta[b]) 58 | if len(intersect)!=0: 59 | graph[i].append(list(intersect)[0]) 60 | graph[list(intersect)[0]].append(i) 61 | intersect = graph_meta[a].intersection(graph_meta[c]) 62 | if len(intersect)!=0: 63 | graph[i].append(list(intersect)[0]) 64 | graph[list(intersect)[0]].append(i) 65 | intersect = graph_meta[b].intersection(graph_meta[c]) 66 | if len(intersect)!=0: 67 | graph[i].append(list(intersect)[0]) 68 | graph[list(intersect)[0]].append(i) 69 | graph_meta[a].add(i) 70 | graph_meta[b].add(i) 71 | graph_meta[c].add(i) 72 | self.graph = graph 73 | def check(self,i,j): 74 | if np.abs(self.angles[i]-self.angles[j]) len(final_proposal): 102 | final_proposal=proposal[:] 103 | proposal.clear() 104 | print(final_proposal) 105 | print(len(self.height_invs),len(self.angles),len(triangle_ids)) 106 | print(self.height_invs[final_proposal],self.angles[final_proposal]) 107 | return final_proposal 108 | 109 | 110 | def triangle(edge_potential): 111 | triangle_potential = np.ones((8,8)) 112 | ep = np.array(edge_potential) 113 | for row in range(8): 114 | for col in range(8): 115 | row_i = [int(row&4!=0),int(row&2!=0),int(row&1!=0)] 116 | col_i = [int(col&4!=0),int(col&2!=0),int(col&1!=0)] 117 | p = 1 118 | p *= ep[row_i[0]*2+row_i[1],col_i[0]] 119 | p *= ep[row_i[1]*2+row_i[2],col_i[1]] 120 | p *= ep[row_i[0]*2+row_i[2],col_i[2]] 121 | triangle_potential[row,col] = p 122 | return triangle_potential 123 | 124 | def check_triangle(v,d): 125 | a = int((v[0]-v[1])*(d[0]-d[1])<0) 126 | b = int((v[1]-v[2])*(d[1]-d[2])<0) 127 | c = int((v[0]-v[2])*(d[0]-d[2])<0) 128 | index = a*4+b*2+c 129 | return index 130 | 131 | def get_assemble_probability(probs): 132 | return np.sum(probs>0.6)/len(probs) 133 | 134 | def get_probability(v,d,tp): 135 | range_ = np.array([0,1,2,3,4,5,6,7]) 136 | a_ = (range_&4)>0 137 | b_ = (range_&2)>0 138 | c_ = (range_&1)>0 139 | index = check_triangle(v,d) 140 | potential = tp[:,index] 141 | z = np.sum(potential) 142 | pa = np.sum(potential[a_])/z 143 | pb = np.sum(potential[b_])/z 144 | pc = np.sum(potential[c_])/z 145 | return [pa,pb,pc] 146 | 147 | 148 | def bool2id(flag): 149 | id_array = np.array(range(0,flag.shape[0])) 150 | ids = id_array[flag] 151 | return ids 152 | 153 | 154 | 155 | 156 | def main(): 157 | edge_potential = [[3,1],[2,2],[2,2],[0,4]] 158 | tp = triangle(edge_potential) 159 | print(tp) 160 | v = [0,1,2] 161 | d = [2,1,1] 162 | index = check_triangle(v,d) 163 | print(tp[:,index]) 164 | prob = get_probability(v,d,tp) 165 | print(prob) 166 | 167 | 168 | 169 | if __name__ == '__main__': 170 | main() 171 | 172 | -------------------------------------------------------------------------------- /src/main.py: -------------------------------------------------------------------------------- 1 | ''' 2 | this code is modified from monovo 3 | the processure is as follows: 4 | 1. read image(currently load offline images from the paths stored in sys.argv[1]) 5 | 2. initial the camera parameter 6 | 3. feature detection and tracking 7 | 4. calculate the camera motion and 3d coordinates of the tracked feature points 8 | 5. figure out which points is on the road and calculation the road model based on the points 9 | 6. got the scale parameter, and smooth it 10 | 11 | # 3+4 is done by vo.update 12 | # 5+6 is done by scale_estimator 13 | ''' 14 | import sys 15 | import numpy as np 16 | import cv2 17 | from thirdparty.MonocularVO.visual_odometry import PinholeCamera, VisualOdometry 18 | #from scale_calculator import ScaleEstimator 19 | #from rescale_test import ScaleEstimator 20 | from rescale import ScaleEstimator 21 | #from reconstruct import Reconstruct 22 | import param 23 | 24 | def main(): 25 | real_scale=None 26 | tag = '.test_001' 27 | images_path = sys.argv[1] 28 | print(images_path) 29 | seq = images_path.split('_')[-1][:2] 30 | print(seq) 31 | calib = open('dataset/'+str(seq)+'_calib.txt').read() 32 | calib = calib.split(' ') 33 | f = float(calib[1]) 34 | cx = float(calib[3]) 35 | cy = float(calib[7]) 36 | 37 | if len(sys.argv)>2: 38 | tag = sys.argv[2] 39 | # real_scale = np.loadtxt(sys.argv[2]) 40 | res_addr = 'result/'+images_path.split('.')[-2].split('/')[-1]+'_' 41 | print(res_addr,tag) 42 | images = open(images_path) 43 | image_name = images.readline() # first line is not pointing to a image, so we read and skip it 44 | image_names= images.read().split('\n') 45 | h,w,c = cv2.imread(image_names[0]).shape 46 | 47 | print(f,cx,cy,h,w,c) 48 | #cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) 49 | #cam = PinholeCamera(640.0, 480.0, 343.8560, 344.8560, 321.1928, 231.2157) 50 | #cam = PinholeCamera(1920.0, 1080.0, 960.0, 960.0, 960.0, 480.0) 51 | #cam = PinholeCamera(param.img_w, param.img_h, param.img_fx, param.img_fy, param.img_cx, param.img_cy) 52 | cam = PinholeCamera(w, h, f, f, cx, cy) 53 | intrinsic_m = np.array([[param.img_fx,0,param.img_cx],[0,param.img_fy,param.img_cy],[0,0,1]]) 54 | vo = VisualOdometry(cam) 55 | scale_estimator = ScaleEstimator(absolute_reference = param.camera_h,window_size=5) 56 | #reconstructer = Reconstruct(cam) 57 | image_id = 0 58 | path=[] 59 | scales=[] 60 | error =[] 61 | pitchs=[] 62 | motions=[] 63 | feature2ds = [] 64 | feature3ds = [] 65 | move_flags = [] 66 | scale = 1 67 | path.append([1,0,0,0,0,1,0,0,0,0,1,0]) 68 | scales.append(0) 69 | error.append(100) 70 | begin_id = 0 71 | 72 | end_id = None 73 | img_last = [] 74 | for image_name in image_names: 75 | if image_idend_id: 79 | break 80 | print(image_name) 81 | if len(image_name) == 0: 82 | break 83 | img = cv2.imread(image_name,0) 84 | img = cv2.resize(img,(cam.width,cam.height)) 85 | img_bgr = cv2.imread(image_name) 86 | #img_bgr = cv2.resize(img_bgr,(cam.width,cam.height)) 87 | move_flag = vo.update(img,image_id) 88 | print(move_flag) 89 | if (not move_flag) and image_id>1: 90 | path.append(path[-1]) 91 | motions.append([1,0,0,0,0,1,0,0,0,0,1,0]) 92 | scales.append(0) 93 | error.append(0) 94 | feature2ds.append([]) 95 | feature3ds.append([]) 96 | image_id+=1 97 | move_flags.append(move_flag) 98 | continue 99 | #print(vo.motion_t) 100 | #print(vo.motion_R) 101 | if image_id>begin_id: 102 | feature2d = vo.feature3d[:,0:2].copy() 103 | feature2d[:,0] = feature2d[:,0]*cam.fx/vo.feature3d[:,2]+cam.cx 104 | feature2d[:,1] = feature2d[:,1]*cam.fx/vo.feature3d[:,2]+cam.cy 105 | feature3ds.append(vo.feature3d.copy()) 106 | feature2ds.append(feature2d.copy()) 107 | move_flags.append(True) 108 | #np.savetxt('feature_3d.txt',vo.feature3d) 109 | print('feature mumber',vo.feature3d.shape) 110 | if vo.feature3d.shape[0]>param.minimum_feature_for_scale: 111 | pitch = scale_estimator.initial_estimation(vo.motion_t.reshape(-1)) 112 | pitchs.append(pitch) 113 | scale,std = scale_estimator.scale_calculation(vo.feature3d,feature2d,img_bgr) 114 | if real_scale is not None: 115 | print('predict,real,std',scale,real_scale[image_id-1],std) 116 | #if(np.abs(scale-real_scale[image_id-1])>0.3 and std<0.3): 117 | if(False):# and abs(real_scale[image_id-1]-scale)>0.3): 118 | scale_estimator.check_full_distribution(vo.feature3d.copy(),feature2d.copy(),real_scale[image_id-1],img_bgr) 119 | scale_estimator.plot_distribution(str(image_id),img_bgr) 120 | # uncomment to visualize the feature and triangle 121 | #scale_estimator.visualize_distance(vo.feature3d,feature2d,img_bgr) 122 | #scale_estimator.visualize(vo.feature3d,feature2d,img_bgr) 123 | #scale_estimator.visualize_distance(vo.feature3d,ref_warp,img_last) 124 | #re = reconstructer.visualize(vo.feature3d,feature2d,img_bgr) 125 | #if re==False: 126 | # break 127 | R,t = vo.get_current_state(scale) 128 | M = np.zeros((3,4)) 129 | M[:,0:3]=R 130 | M[:,3]=t.reshape(-1) 131 | M = M.reshape(-1) 132 | motion = np.zeros((3,4)) 133 | motion[:,0:3]=vo.motion_R 134 | motion[:,3]=vo.motion_t.reshape(-1) 135 | motion = motion.reshape(-1) 136 | path.append(M) 137 | motions.append(motion) 138 | scales.append(scale) 139 | error.append(std) 140 | else: 141 | path.append(path[-1]) 142 | motions.append(motions[-1]) 143 | scales.append(scales[-1]) 144 | error.append(error[-1]) 145 | print('id ', image_id,' scale ',scale) 146 | img_last = img_bgr.copy() 147 | image_id+=1 148 | #np.savetxt(res_addr+'features.txt',scale_estimator.all_features) 149 | data_to_save = {} 150 | data_to_save['motions'] = motions 151 | data_to_save['feature3ds'] = feature3ds 152 | data_to_save['feature2ds'] = feature2ds 153 | data_to_save['move_flags'] = move_flags 154 | np.save(res_addr+'result.npy'+tag,data_to_save) 155 | np.savetxt(res_addr+'path.txt'+tag,path) 156 | np.savetxt(res_addr+'motions.txt'+tag,motions) 157 | np.savetxt(res_addr+'scales.txt'+tag,scales[1:]) 158 | np.savetxt(res_addr+'error.txt'+tag,error) 159 | np.savetxt(res_addr+'pitch.txt'+tag,pitchs) 160 | 161 | 162 | if __name__ == '__main__': 163 | main() 164 | 165 | 166 | ''' 167 | # reprojection error 168 | ref_warp = intrinsic_m@(vo.motion_R@vo.feature3d.transpose()+vo.motion_t) 169 | ref_warp = (ref_warp/ref_warp[2,:]).transpose() 170 | ref_warp = ref_warp[:,0:2] 171 | reproject_error = np.sum(np.abs(ref_warp - vo.px_ref_selected))/ref_warp.shape[0] 172 | error.append(reproject_error) 173 | print(reproject_error) 174 | 175 | ''' 176 | -------------------------------------------------------------------------------- /src/main_offline.py: -------------------------------------------------------------------------------- 1 | ''' 2 | this code is modified from monovo 3 | the processure is as follows: 4 | 1. read image(currently load offline images from the paths stored in sys.argv[1]) 5 | 2. initial the camera parameter 6 | 3. feature detection and tracking 7 | 4. calculate the camera motion and 3d coordinates of the tracked feature points 8 | 5. figure out which points is on the road and calculation the road model based on the points 9 | 6. got the scale parameter, and smooth it 10 | 11 | # 3+4 is done by vo.update 12 | # 5+6 is done by scale_estimator 13 | ''' 14 | import sys 15 | import numpy as np 16 | import cv2 17 | from thirdparty.MonocularVO.visual_odometry import PinholeCamera, VisualOdometry 18 | #from scale_calculator import ScaleEstimator 19 | #from rescale_test import ScaleEstimator 20 | from rescale import ScaleEstimator 21 | #from reconstruct import Reconstruct 22 | import param 23 | 24 | def main(): 25 | real_scale=None 26 | data_path = sys.argv[1] 27 | np_load_old = np.load 28 | np.load = lambda *a,**k: np_load_old(*a, allow_pickle=True, **k) 29 | # do what you did like 30 | data = np.load(data_path) 31 | data = data.item() 32 | np.load = np_load_old 33 | tag = '.test_001' 34 | if len(sys.argv)>2: 35 | tag = sys.argv[2] 36 | # real_scale = np.loadtxt(sys.argv[2]) 37 | res_addr = 'evaluate_result/'+data_path.split('.')[-4].split('/')[-1]+'_' 38 | scale_estimator = ScaleEstimator(absolute_reference = param.camera_h,window_size=5) 39 | #reconstructer = Reconstruct(cam) 40 | image_id = 0 41 | path=[] 42 | scales=[] 43 | error =[] 44 | scale = 1 45 | path.append([1,0,0,0,0,1,0,0,0,0,1,0]) 46 | scales.append(0) 47 | error.append(100) 48 | begin_id = 0 49 | 50 | end_id = None 51 | img_last = [] 52 | motions = data['motions'] 53 | move_flags = data['move_flags'] 54 | feature2ds = data['feature2ds'] 55 | feature3ds = data['feature3ds'] 56 | print(len(motions),len(move_flags),len(feature2ds)) 57 | for move_flag in move_flags: 58 | if image_idend_id: 62 | break 63 | print(move_flag) 64 | if (not move_flag): 65 | scales.append(0) 66 | error.append(0) 67 | image_id+=1 68 | continue 69 | 70 | feature3d = feature3ds[image_id] 71 | feature2d = feature2ds[image_id] 72 | motion = motions[image_id] 73 | if feature3d.shape[0]>param.minimum_feature_for_scale: 74 | pitch = scale_estimator.initial_estimation(motion[3:12:4].reshape(-1)) 75 | scale,std = scale_estimator.scale_calculation(feature3d,feature2d) 76 | if real_scale is not None: 77 | print('predict,real,std',scale,real_scale[image_id-1],std) 78 | #if(np.abs(scale-real_scale[image_id-1])>0.3 and std<0.3): 79 | if(False):# and abs(real_scale[image_id-1]-scale)>0.3): 80 | scale_estimator.check_full_distribution(feature3d.copy(),feature2d.copy(),real_scale[image_id-1],img_bgr) 81 | scale_estimator.plot_distribution(str(image_id),img_bgr) 82 | scales.append(scale) 83 | error.append(std) 84 | else: 85 | scales.append(scales[-1]) 86 | error.append(error[-1]) 87 | print('id ', image_id,' scale ',scale) 88 | image_id+=1 89 | #np.savetxt(res_addr+'features.txt',scale_estimator.all_features) 90 | np.savetxt(res_addr+'scales.txt'+tag,scales[1:]) 91 | print(res_addr) 92 | poses = get_path(np.array(motions),np.array(scales[1:])) 93 | np.savetxt(res_addr+'path.txt'+tag,poses) 94 | 95 | def line2mat(line_data): 96 | mat = np.eye(4) 97 | mat[0:3,:] = line_data.reshape(3,4) 98 | return np.matrix(mat) 99 | 100 | def motion2pose(data): 101 | data_size = data.shape[0] 102 | all_pose = np.zeros((data_size+1,12)) 103 | temp = np.eye(4,4).reshape(1,16) 104 | all_pose[0,:] = temp[0,0:12] 105 | pose = np.matrix(np.eye(4,4)) 106 | for i in range(0,data_size): 107 | data_mat = line2mat(data[i,:]) 108 | pose = pose*data_mat 109 | pose_line = np.array(pose[0:3,:]).reshape(1,12) 110 | all_pose[i+1,:] = pose_line 111 | return all_pose 112 | 113 | 114 | def get_path(motions,scales): 115 | motion_trans = motions[:,3:12:4] 116 | motion_trans = (scales*motion_trans.transpose()).transpose() 117 | motions[:,3:12:4] = motion_trans 118 | pose = motion2pose(motions) 119 | return pose 120 | 121 | 122 | if __name__ == '__main__': 123 | main() 124 | 125 | 126 | ''' 127 | # reprojection error 128 | ref_warp = intrinsic_m@(vo.motion_R@vo.feature3d.transpose()+vo.motion_t) 129 | ref_warp = (ref_warp/ref_warp[2,:]).transpose() 130 | ref_warp = ref_warp[:,0:2] 131 | reproject_error = np.sum(np.abs(ref_warp - vo.px_ref_selected))/ref_warp.shape[0] 132 | error.append(reproject_error) 133 | print(reproject_error) 134 | 135 | ''' 136 | -------------------------------------------------------------------------------- /src/param.py: -------------------------------------------------------------------------------- 1 | 2 | ''' 3 | #kitti 11 4 | img_w = 1226.0 5 | img_h = 370.0 6 | img_fx= 707.0912 7 | img_fy= 707.0912 8 | img_cx= 601.8873 9 | img_cy= 183.1104 10 | camera_h = 1.75 11 | minimum_feature_for_scale = 100 12 | minimum_feature_for_tracking = 1500 13 | fast_threshold = 25 14 | 15 | ''' 16 | ''' 17 | #kitti 0926 18 | img_w = 1242.0 19 | img_h = 375.0 20 | img_fx= 721.5377 21 | img_fy= 721.5377 22 | img_cx= 609.5593 23 | img_cy= 172.8540 24 | camera_h = 1.75 25 | minimum_feature_for_scale = 100 26 | minimum_feature_for_tracking = 1500 27 | fast_threshold = 25 28 | ''' 29 | # kitti 00 30 | img_w = 1241.0 31 | img_h = 376.0 32 | img_fx= 718.856 33 | img_fy= 718.856 34 | img_cx= 607.1928 35 | img_cy= 185.2157 36 | camera_h = 1.75 37 | minimum_feature_for_scale = 100 38 | minimum_feature_for_tracking = 1500 39 | fast_threshold = 25 40 | ''' 41 | img_w = 640.0 42 | img_h = 360.0 43 | img_fx= 320.0 44 | img_fy= 320.0 45 | img_cx= 320.0 46 | img_cy= 180.0 47 | camera_h = 1.75 48 | minimum_feature_for_scale = 100 49 | minimum_feature_for_tracking = 10000 50 | fast_threshold = 30 51 | ''' 52 | 53 | -------------------------------------------------------------------------------- /src/reconstruct.py: -------------------------------------------------------------------------------- 1 | # recover ego-motion scale by road geometry information 2 | # @author xiangwei(wangxiangwei.cpp@gmail.com) and zhanghui() 3 | # 4 | 5 | # @function: calcualte the road pitch angle from motion matrix 6 | # @input: the tansformation matrix in SE(3) 7 | # @output:translation angle calculate by t in R^3 and 8 | # rotarion angle calculate from rotation matrix R in SO(3) 9 | 10 | 11 | 12 | from scipy.spatial import Delaunay 13 | from estimate_road_norm import * 14 | import numpy as np 15 | import math 16 | from collections import deque 17 | import matplotlib.pyplot as plt 18 | import open3d as o3d 19 | 20 | class Reconstruct: 21 | def __init__(self,cam): 22 | self.threshold = 1 23 | self.cam = cam 24 | pixel_u=np.array(list(range(0,cam.width))*cam.height).reshape(cam.height,cam.width) 25 | pixel_v=np.array(list(range(0,cam.height))*cam.width).reshape(cam.width,cam.height) 26 | pixel_v=pixel_v.transpose() 27 | pixel = np.stack((pixel_u,pixel_v)) 28 | pixel = pixel.transpose((1,2,0)).reshape(-1,2) 29 | self.pixel_ori = pixel.copy() 30 | 31 | pixel =pixel.astype(float) 32 | pixel[:,0] = (pixel[:,0]-self.cam.cx)/self.cam.fx 33 | d = (pixel[:,0]-self.cam.cx)/self.cam.fx 34 | 35 | pixel[:,1] = (pixel[:,1]-self.cam.cy)/self.cam.fy 36 | self.pixel = pixel.reshape(cam.height,cam.width,2) 37 | ''' 38 | check the three vertice in triangle whether satisfy 39 | (d_1-d_2)*(v_1-v_2)<=0 if not they are outlier 40 | True means inlier 41 | ''' 42 | def check_triangle(self,v,d): 43 | flag=[False,False,False] 44 | a = (v[0]-v[1])*(d[0]-d[1]) 45 | b = (v[0]-v[2])*(d[0]-d[2]) 46 | c = (v[1]-v[2])*(d[1]-d[2]) 47 | if a>0: 48 | flag[0]=True 49 | flag[1]=True 50 | if b>0: 51 | flag[0]=True 52 | flag[1]=True 53 | if c>0: 54 | flag[1]=True 55 | flag[2]=True 56 | return flag 57 | 58 | def find_outliers(self,feature3d,feature2d,triangle_ids): 59 | # suppose every is inlier 60 | outliers = np.ones((feature3d.shape[0])) 61 | for triangle_id in triangle_ids: 62 | data=[] 63 | depths = feature3d[triangle_id,2] 64 | pixel_vs = feature2d[triangle_id,1] 65 | flag = self.check_triangle(pixel_vs,depths) 66 | outlier = triangle_id[flag] 67 | outliers[outlier]-=np.ones(outliers[outlier].shape[0]) 68 | 69 | return outliers 70 | def triangle_model(self,feature3d,triangle_ids): 71 | datas =[] 72 | b_matrix = np.matrix(np.ones((3,1),np.float)) 73 | for triangle_id in triangle_ids: 74 | data=[] 75 | point_selected = feature3d[triangle_id] 76 | a_array = np.array(point_selected) 77 | a_matrix = np.matrix(a_array) 78 | a_matrix_inv = a_matrix.I 79 | norm = a_matrix_inv*b_matrix 80 | norm_norm_2 = norm.T*norm#the orm of norm 81 | height = 1/math.sqrt(norm_norm_2) 82 | norm = norm/math.sqrt(norm_norm_2) 83 | if norm[1,0]<0: 84 | norm = -norm 85 | height = -height 86 | data = [norm[0,0],norm[1,0],norm[2,0],height] 87 | #print(data) 88 | datas.append(data) 89 | datas = np.array(datas) 90 | return datas 91 | def depth_generate(self,tri,datas,img): 92 | depth_img = np.zeros((self.cam.height,self.cam.width,3)) 93 | pixel_tris = tri.find_simplex(self.pixel_ori) 94 | self.pixel_tris = pixel_tris.reshape(self.cam.height,self.cam.width) 95 | point_clouds =[] 96 | colors =[] 97 | for v in range(0,self.cam.height): 98 | for u in range(0,self.cam.width): 99 | tri_id = self.pixel_tris[v,u] 100 | if tri_id != -1: 101 | pixel = self.pixel[v,u] 102 | norm_tri = datas[tri_id,0:3] 103 | h_tri = datas[tri_id,3] 104 | depth = h_tri/(norm_tri[0]*pixel[0]+norm_tri[1]*pixel[1]+norm_tri[2]) 105 | if(depth)<0: 106 | print('there should be an error',depth,pixel,datas[tri_id]) 107 | depth_img[v,u] =depth 108 | point = [pixel[0]*depth,pixel[1]*depth,depth] 109 | point_clouds.append(point) 110 | colors.append(img[v,u,::-1]/255.0) 111 | depth_img = depth_img/np.max(depth_img) 112 | pcd = o3d.geometry.PointCloud() 113 | pcd.points = o3d.utility.Vector3dVector(np.array(point_clouds)) 114 | pcd.colors = o3d.utility.Vector3dVector(np.array(colors)) 115 | o3d.io.write_point_cloud("pointcloud.ply", pcd) 116 | cv2.imshow('img_depth',depth_img/np.max(depth_img)) 117 | print(np.min(depth_img),np.max(depth_img),np.mean(depth_img)) 118 | 119 | def visualize_triangle(self,feature2d,triangle_ids,datas,img): 120 | datas_80 = datas[datas[:,1]>0.9847,:] 121 | 122 | datas_x = datas[(datas[:,0]>0.9847)|(datas[:,0]<-0.9847),:] 123 | datas_z = datas[(datas[:,2]>0.9847)|(datas[:,2]<-0.9847),:] 124 | 125 | data_label = np.zeros(datas.shape[0]) 126 | data_label += 1*((datas[:,0]>0.9847)|(datas[:,0]<-0.9847)).astype(int) 127 | data_label += 2*((datas[:,1]>0.9847)|(datas[:,1]<-0.9847)).astype(int) 128 | data_label += 3*((datas[:,2]>0.9847)|(datas[:,2]<-0.9847)).astype(int) 129 | for i in range(0,triangle_ids.shape[0]): 130 | triangle_id = triangle_ids[i] 131 | triangle_points = np.array(feature2d[triangle_id],np.int32) 132 | pts = triangle_points.reshape((-1,1,2)) 133 | #color = list(np.abs(datas[i,0:3]*255).astype(np.int32)) 134 | pitch = math.asin(datas[i,1]) 135 | pitch_deg = pitch*180/3.1415926 136 | if data_label[i]==1: 137 | color=[255,0,0] 138 | color = (abs(int(color[0]*255)),abs(int(color[1]*255)),abs(int(color[2]*255))) 139 | cv2.polylines(img,[pts],True,color) 140 | #cv2.fillPoly(img,[pts],color) 141 | elif data_label[i]==2: 142 | color=[0,255,0] 143 | color = (abs(int(color[0]*255)),abs(int(color[1]*255)),abs(int(color[2]*255))) 144 | cv2.polylines(img,[pts],True,color) 145 | #cv2.fillPoly(img,[pts],color) 146 | elif data_label[i]==3: 147 | color=[0,0,255] 148 | color = (abs(int(color[0]*255)),abs(int(color[1]*255)),abs(int(color[2]*255))) 149 | cv2.polylines(img,[pts],True,color) 150 | else: 151 | color=[0,0,0] 152 | height = datas[i,3] 153 | color = (abs(int(color[0]*255)),abs(int(color[1]*255)),abs(int(color[2]*255))) 154 | cv2.polylines(img,[pts],True,color) 155 | #cv2.fillPoly(img,[pts],color) 156 | 157 | def visualize_feature(self,feature3d,feature2d,img): 158 | near = np.min(feature3d[:,2]) 159 | far = np.max(feature3d[:,2]) 160 | #print(near,far) 161 | for i in range(feature3d.shape[0]): 162 | pos_y_norm = (feature3d[i,2]-near)/(far-near) 163 | color=(255*pos_y_norm,0,255-255*pos_y_norm) 164 | cv2.circle(img,(int(feature2d[i,0]),int(feature2d[i,1])),3,color,-1) 165 | 166 | 167 | 168 | def visualize(self,feature3d,feature2d,img): 169 | print('feature total ',feature3d.shape[0]) 170 | img_c = img.copy() 171 | lower_label = feature3d[:,1]>0 172 | feature3d = feature3d[lower_label,:] 173 | feature2d = feature2d[lower_label,:] 174 | print('feature lower ',feature3d.shape[0]) 175 | tri = Delaunay(feature2d) 176 | triangle_ids = tri.simplices 177 | datas = self.triangle_model(feature3d,triangle_ids) 178 | self.visualize_triangle(feature2d,triangle_ids,datas,img) 179 | self.visualize_feature(feature3d,feature2d,img) 180 | 181 | cv2.imshow('img',img) 182 | outliers = self.find_outliers(feature3d,feature2d,triangle_ids) 183 | print('feature rejected ',np.sum(outliers<0)) 184 | print('feature left ',np.sum(outliers>=0)) 185 | #calculating the geometry model of each triangle 186 | feature2d = feature2d[outliers>=0,:] 187 | feature3d = feature3d[outliers>=0,:] 188 | tri = Delaunay(feature2d) 189 | triangle_ids = tri.simplices 190 | datas = self.triangle_model(feature3d,triangle_ids) 191 | #self.depth_generate(tri,datas,img_c) 192 | self.visualize_triangle(feature2d,triangle_ids,datas,img_c) 193 | self.visualize_feature(feature3d,feature2d,img_c) 194 | cv2.imshow('img_no_outlier',img_c) 195 | key = cv2.waitKey() 196 | if key&255 == ord('q'): 197 | return False 198 | return True 199 | 200 | 201 | def main(): 202 | # get initial motion pitch by motion matrix 203 | # triangle region norm and height 204 | # selected 205 | # calculate the road norm 206 | # filtering 207 | # scale recovery 208 | print('test') 209 | 210 | if __name__ == '__main__': 211 | main() 212 | -------------------------------------------------------------------------------- /src/rescale.py: -------------------------------------------------------------------------------- 1 | # recover ego-motion scale by road geometry information 2 | # @author xiangwei(wangxiangwei.cpp@gmail.com) and zhanghui() 3 | # 4 | 5 | # @function: calcualte the road pitch angle from motion matrix 6 | # @input: the tansformation matrix in SE(3) 7 | # @output:translation angle calculate by t in R^3 and 8 | # rotarion angle calculate from rotation matrix R in SO(3) 9 | 10 | 11 | 12 | from scipy.spatial import Delaunay 13 | from estimate_road_norm import * 14 | import numpy as np 15 | import math 16 | from collections import deque 17 | import matplotlib.pyplot as plt 18 | import scale_calculator as sc 19 | import graph 20 | import param 21 | 22 | class ScaleEstimator: 23 | def __init__(self,absolute_reference,window_size=6): 24 | self.absolute_reference = absolute_reference 25 | self.camera_pitch = 0 26 | self.scale = 1 27 | self.inliers = None 28 | self.scale_queue = deque() 29 | self.window_size = window_size 30 | self.vanish = 185 31 | self.sc = sc.ScaleEstimator(absolute_reference,window_size) 32 | self.gc = graph.GraphChecker([[3,1],[2,2],[2,2],[0,4]]) 33 | self.gs = graph.GraphGrow() 34 | self.img_w = param.img_w 35 | self.img_h = param.img_h 36 | def initial_estimation(self,motion_matrix): 37 | 38 | return 0 39 | 40 | ''' 41 | check the three vertice in triangle whether satisfy 42 | (d_1-d_2)*(v_1-v_2)<=0 if not they are outlier 43 | True means outlier 44 | ''' 45 | def check_triangle(self,v,d): 46 | flag=[False,False,False] 47 | a = (v[0]-v[1])*(d[0]-d[1]) 48 | b = (v[0]-v[2])*(d[0]-d[2]) 49 | c = (v[1]-v[2])*(d[1]-d[2]) 50 | if a>0: 51 | flag[0]=True 52 | flag[1]=True 53 | if b>0: 54 | flag[0]=True 55 | flag[1]=True 56 | if c>0: 57 | flag[1]=True 58 | flag[2]=True 59 | return flag 60 | 61 | def find_outliers(self,feature3d,feature2d,triangle_ids): 62 | # suppose every is inlier 63 | outliers = np.ones((feature3d.shape[0])) 64 | for triangle_id in triangle_ids: 65 | data=[] 66 | depths = feature3d[triangle_id,2] 67 | pixel_vs = feature2d[triangle_id,1] 68 | flag = self.check_triangle(pixel_vs,depths) 69 | outlier = triangle_id[flag] 70 | outliers[outlier]-=np.ones(outliers[outlier].shape[0]) 71 | 72 | return outliers 73 | 74 | 75 | def flat_selection(self,feature3d,triangle_ids): 76 | self.b_matrix = np.ones((3,1),np.float) 77 | #calculating the geometry model of each triangle 78 | triangles = np.array([np.matrix(feature3d[triangle_id]) for triangle_id in triangle_ids]) 79 | triangles_i = np.array([np.matrix(feature3d[triangle_id]).I for triangle_id in triangle_ids]) 80 | normals = (triangles_i@self.b_matrix).reshape(-1,3) 81 | normals_len = np.sqrt(np.sum(normals*normals,1)).reshape(-1,1) 82 | normals = normals/normals_len 83 | pitch_deg = np.arcsin(-normals[:,1])*180/np.pi #[-90,90] 84 | 85 | valid_pitch_id = pitch_deg<-80 86 | valid_pitch_id_tight = pitch_deg<-85 87 | 88 | print('triangle left ',np.sum(valid_pitch_id),'from',valid_pitch_id.shape[0]) 89 | heights = (1/normals_len).reshape(-1) 90 | unvalid_pitch_id = pitch_deg>=-80 91 | height_level = 0.9*(np.median(heights[valid_pitch_id])) 92 | self.height_level = height_level 93 | print('height level',height_level) 94 | valid_height_id = heights>height_level 95 | print(len(valid_height_id)) 96 | valid_id = valid_pitch_id_tight & valid_height_id 97 | print('triangle left final',np.sum(valid_id),'from',valid_id.shape[0]) 98 | #valid_id = valid_pitch_id 99 | #valid_id = self.gs.process(triangle_ids,heights,pitch_deg) 100 | #valid_points_id = np.unique(triangle_ids[valid_id].reshape(-1)) 101 | valid_points_id = triangle_ids[valid_id].reshape(-1) 102 | return list(valid_points_id),heights[valid_pitch_id] 103 | 104 | ''' 105 | @func:select features on road 106 | @input: feature3d nx3 features relative 3d coordinate 107 | @input: feature2d nx2 features pixel coordinate 108 | 1. remove feature above vanishing point 109 | 2. remove feature not fit (v1-v2)(d1-d2)<0 110 | 3. remove unflat feature and higher feature 111 | ''' 112 | 113 | def feature_selection(self,feature3d,feature2d): 114 | # step 1: remove feature above vanishing point 115 | lower_feature_ids = feature2d[:,1]>self.vanish 116 | feature2d = feature2d[lower_feature_ids,:] 117 | feature3d = feature3d[lower_feature_ids,:] 118 | 119 | #distance_level = np.median(feature3d[:,2]) 120 | #near_feature_ids = feature3d[:,2]<2*distance_level 121 | #feature2d = feature2d[near_feature_ids,:] 122 | #feature3d = feature3d[near_feature_ids,:] 123 | # step 2：remove feature by fit (v1-v2)(d1-d2)<0 124 | tri = Delaunay(feature2d) 125 | triangle_ids = tri.simplices 126 | #valid_id = self.sc.find_reliability_by_graph(feature3d,feature2d,triangle_ids) 127 | valid_id = self.gc.find_inliers(feature3d,feature2d,triangle_ids) 128 | #outliers = self.find_outliers(feature3d,feature2d,triangle_ids) 129 | #valid_id = (outliers>=0) 130 | #valid_id = [] 131 | print('feature rejected ',np.sum(valid_id==False)) 132 | print('feature left ',np.sum(valid_id)) 133 | if(np.sum(valid_id)>10): 134 | feature2d = feature2d[valid_id,:] 135 | feature3d = feature3d[valid_id,:] 136 | tri = Delaunay(feature2d) 137 | triangle_ids = tri.simplices 138 | # step 3 remove unflat and high 139 | data_id,h = self.flat_selection(feature3d,triangle_ids) 140 | point_selected = feature3d[data_id] 141 | ''' 142 | ax1 = plt.subplot(111) 143 | ax1.plot(feature3d[:,2],-1/feature3d[:,1],'.g') 144 | ax1.plot(point_selected[:,2],-1/point_selected[:,1],'.y') 145 | ax1.set_ylim(-2,0) 146 | plt.show() 147 | ''' 148 | return point_selected,h 149 | #return feature3d 150 | 151 | def scale_calculation_ransac(self,point_selected): 152 | if(point_selected.shape[0]>=12): 153 | # ransac 154 | a_array = np.array(point_selected) 155 | m,b = get_pitch_ransac(a_array,100,0.005) 156 | road_model_ransac = np.matrix(m) 157 | norm = road_model_ransac[0,0:-1] 158 | h_bar = -road_model_ransac[0,-1] 159 | if norm[0,1]<0: 160 | norm = -norm 161 | h_bar = -h_bar 162 | norm_norm_2 = norm*norm.T 163 | norm_norm = math.sqrt(norm_norm_2)/h_bar 164 | norm = norm/h_bar 165 | ransac_camera_height = 1/norm_norm 166 | pitch = math.asin(norm[0,1]/norm_norm) 167 | scale = self.absolute_reference/ransac_camera_height 168 | #0.3 is the max accellerate 169 | if scale - self.scale >0.3: 170 | self.scale += 0.3 171 | elif scale - self.scale<-0.3: 172 | self.scale -= 0.3 173 | else: 174 | self.scale = scale 175 | self.scale_queue.append(self.scale) 176 | if len(self.scale_queue)>self.window_size: 177 | self.scale_queue.popleft() 178 | return np.median(self.scale_queue),1 179 | def scale_calculation_static_tri(self,heights): 180 | 181 | if len(heights)>12: 182 | scale_norm,_,_ = self.sc.road_model_calculation_static_tri(heights) 183 | scale = scale_norm*self.absolute_reference 184 | self.scale =scale 185 | return scale,0 186 | else: 187 | return self.scale,0 188 | 189 | 190 | 191 | def scale_calculation(self,feature3d,feature2d,img=None): 192 | point_selected,h = self.feature_selection(feature3d,feature2d) 193 | return self.scale_calculation_ransac(point_selected) 194 | #return self.scale_calculation_static_tri(h) 195 | #return self.sc.scale_calculation_static(point_selected) 196 | # initial ransac 197 | def main(): 198 | # get initial motion pitch by motion matrix 199 | # triangle region norm and height 200 | # selected 201 | # calculate the road norm 202 | # filtering 203 | # scale recovery 204 | camera_height = 1.7 205 | scale_estimator = ScaleEstimator(camera_height) 206 | scale = scale_estimator.scale_calculation() 207 | 208 | if __name__ == '__main__': 209 | main() 210 | -------------------------------------------------------------------------------- /src/rescale.py.origin: -------------------------------------------------------------------------------- 1 | # recover ego-motion scale by road geometry information 2 | # @author xiangwei(wangxiangwei.cpp@gmail.com) and zhanghui() 3 | # 4 | 5 | # @function: calcualte the road pitch angle from motion matrix 6 | # @input: the tansformation matrix in SE(3) 7 | # @output:translation angle calculate by t in R^3 and 8 | # rotarion angle calculate from rotation matrix R in SO(3) 9 | 10 | 11 | 12 | from scipy.spatial import Delaunay 13 | from estimate_road_norm import * 14 | import numpy as np 15 | import math 16 | from collections import deque 17 | import matplotlib.pyplot as plt 18 | 19 | 20 | 21 | class ScaleEstimator: 22 | def __init__(self,absolute_reference,window_size=6): 23 | self.absolute_reference = absolute_reference 24 | self.camera_pitch = 0 25 | self.scale = 1 26 | self.inliers = None 27 | self.scale_queue = deque() 28 | self.window_size = window_size 29 | self.vanish = 185 30 | def initial_estimation(self,motion_matrix): 31 | 32 | return 0 33 | def visualize_distance(self,feature3d,feature2d,img): 34 | near = np.min(feature3d[:,2]) 35 | far = np.max(feature3d[:,2]) 36 | print(near,far) 37 | for i in range(feature3d.shape[0]): 38 | pos_y_norm = (feature3d[i,2]-near)/(far-near) 39 | cv2.circle(img,(int(feature2d[i,0]),int(feature2d[i,1])),3,(255*pos_y_norm,0,255-255*pos_y_norm),-1) 40 | cv2.imshow('img',img) 41 | cv2.waitKey() 42 | 43 | def visualize_feature(self,feature3d,feature2d,img): 44 | heighest = np.min(feature3d[:,1]) 45 | lowest = np.max(feature3d[:,1]) 46 | print(heighest,lowest) 47 | for i in range(feature3d.shape[0]): 48 | pos_y_norm = (feature3d[i,1]-heighest)/(lowest-heighest) 49 | if feature3d[i][1]>0: 50 | cv2.circle(img,(int(feature2d[i,0]),int(feature2d[i,1])),3,(int(255*pos_y_norm),0,int(255-255*pos_y_norm)),-1) 51 | else: 52 | cv2.circle(img,(int(feature2d[i,0]),int(feature2d[i,1])),3,(int(255*pos_y_norm),0,int(255-255*pos_y_norm)),-1) 53 | 54 | def visualize(self,feature3d,feature2d,img): 55 | #lower_label = feature3d[:,1]>0 56 | #feature3d = feature3d[lower_label,:] 57 | #feature2d = feature2d[lower_label,:] 58 | tri = Delaunay(feature2d) 59 | triangle_ids = tri.simplices 60 | b_matrix = np.matrix(np.ones((3,1),np.float)) 61 | #calculating the geometry model of each triangle 62 | datas =[] 63 | for triangle_id in triangle_ids: 64 | data=[] 65 | point_selected = feature3d[triangle_id] 66 | a_array = np.array(point_selected) 67 | a_matrix = np.matrix(a_array) 68 | a_matrix_inv = a_matrix.I 69 | norm = a_matrix_inv*b_matrix 70 | norm_norm_2 = norm.T*norm#the orm of norm 71 | height = 1/math.sqrt(norm_norm_2) 72 | norm = norm/math.sqrt(norm_norm_2) 73 | if norm[1,0]<0: 74 | norm = -norm 75 | height = -height 76 | data = [norm[0,0],norm[1,0],norm[2,0],height] 77 | #print(data) 78 | datas.append(data) 79 | datas = np.array(datas) 80 | datas_80 = datas[datas[:,1]>0.9847,:] 81 | 82 | print(np.median(datas_80[:,3])) 83 | precomput_h = 0.9*np.median(datas_80[:,3]) 84 | datas_low = datas_80[datas_80[:,3]>precomput_h,:] 85 | print(datas.shape[0],datas_80.shape[0],datas_low.shape[0]) 86 | 87 | #plt.hist(feature3d[feature3d[:,1]>0,1]) 88 | #np.savetxt('test_norm_height.txt',datas) 89 | #plt.show() 90 | for i in range(0,triangle_ids.shape[0]): 91 | triangle_id = triangle_ids[i] 92 | triangle_points = np.array(feature2d[triangle_id],np.int32) 93 | pts = triangle_points.reshape((-1,1,2)) 94 | #color = list(np.abs(datas[i,0:3]*255).astype(np.int32)) 95 | pitch = math.asin(datas[i,1]) 96 | pitch_deg = pitch*180/3.1415926 97 | color = datas[i,0:3] 98 | if pitch_deg>80 and datas[i,3]>precomput_h: 99 | color=[0,255,0] 100 | height = datas[i,3] 101 | color = (abs(int(color[0]*255)),abs(int(color[1]*255)),abs(int(color[2]*255))) 102 | cv2.polylines(img,[pts],True,color) 103 | #cv2.fillPoly(img,[pts],color) 104 | elif datas[i,3]0: 131 | flag[0]=True 132 | flag[1]=True 133 | if b>0: 134 | flag[0]=True 135 | flag[1]=True 136 | if c>0: 137 | flag[1]=True 138 | flag[2]=True 139 | return flag 140 | 141 | def find_outliers(self,feature3d,feature2d,triangle_ids): 142 | # suppose every is inlier 143 | outliers = np.ones((feature3d.shape[0])) 144 | for triangle_id in triangle_ids: 145 | data=[] 146 | depths = feature3d[triangle_id,2] 147 | pixel_vs = feature2d[triangle_id,1] 148 | flag = self.check_triangle(pixel_vs,depths) 149 | outlier = triangle_id[flag] 150 | outliers[outlier]-=np.ones(outliers[outlier].shape[0]) 151 | 152 | return outliers 153 | 154 | 155 | def scale_calculation(self,feature3d,feature2d,img=None): 156 | 157 | lower_feature_ids = feature2d[:,1]>self.vanish 158 | feature2d = feature2d[lower_feature_ids,:] 159 | feature3d = feature3d[lower_feature_ids,:] 160 | tri = Delaunay(feature2d) 161 | triangle_ids = tri.simplices 162 | outliers = self.find_outliers(feature3d,feature2d,triangle_ids) 163 | 164 | print('feature rejected ',np.sum(outliers<0)) 165 | print('feature left ',np.sum(outliers>=0)) 166 | if(np.sum(outliers==1)>10): 167 | feature2d = feature2d[outliers>=0,:] 168 | feature3d = feature3d[outliers>=0,:] 169 | tri = Delaunay(feature2d) 170 | triangle_ids = tri.simplices 171 | 172 | b_matrix = np.matrix(np.ones((3,1),np.float)) 173 | data = [] 174 | #calculating the geometry model of each triangle 175 | for triangle_id in triangle_ids: 176 | point_selected = feature3d[triangle_id] 177 | a_array = np.array(point_selected) 178 | a_matrix = np.matrix(a_array) 179 | a_matrix_inv = a_matrix.I 180 | norm = a_matrix_inv*b_matrix 181 | norm_norm_2 = norm.T*norm#the square norm of norm 182 | height = 1/math.sqrt(norm_norm_2) 183 | if norm[1,0]<0: 184 | norm = -norm 185 | height = -height 186 | pitch = math.asin(-norm[1,0]/math.sqrt(norm_norm_2[0,0])) 187 | pitch_deg = pitch*180/3.1415926 188 | pitch_height = [norm[1,0]/math.sqrt(norm_norm_2[0,0]),pitch_deg,height] 189 | data.append(pitch_height) 190 | data = np.array(data) # all data is saved here 191 | 192 | # initial select by prior information 193 | data_sub = data[data[:,1]>self.camera_pitch-95]#>80 deg 194 | data_sub = data_sub[data[:,1]80 deg 195 | data_sub = data_sub[data_sub[:,2]>0]#under 196 | precomput_h = 0.9*np.median(data_sub[:,2]) 197 | data_sub_low = data_sub[data_sub[:,2]>precomput_h,:] 198 | data_id = [] 199 | # collect suitable points and triangle 200 | for i in range(0,triangle_ids.shape[0]): 201 | triangle_id = triangle_ids[i] 202 | pitch_deg = data[i,1] 203 | height = data[i,2] 204 | triangle_points = np.array(feature2d[triangle_id],np.int32) 205 | if(pitch_deg>self.camera_pitch-95 and pitch_degprecomput_h): 207 | data_id.append(triangle_id[0]) 208 | data_id.append(triangle_id[1]) 209 | data_id.append(triangle_id[2]) 210 | pts = triangle_points.reshape((-1,1,2)) 211 | point_selected = feature3d[data_id] 212 | self.initial_points = feature2d[data_id] 213 | 214 | # initial ransac 215 | if(point_selected.shape[0]>=12): 216 | # ransac 217 | a_array = np.array(point_selected) 218 | m,b = get_pitch_ransac(a_array,30,0.005) 219 | inlier_id = get_inliers(m,feature3d[:,:],0.01) 220 | inliers = feature3d[inlier_id,:] 221 | inliers_2d = feature2d[inlier_id,:] 222 | self.inliers = inliers_2d 223 | outliers_2d = feature2d[inlier_id==False,:] 224 | print('inilier',inliers.shape[0]) 225 | road_model_ransac = np.matrix(m) 226 | norm = road_model_ransac[0,0:-1] 227 | h_bar = -road_model_ransac[0,-1] 228 | if norm[0,1]<0: 229 | norm = -norm 230 | h_bar = -h_bar 231 | norm_norm_2 = norm*norm.T 232 | norm_norm = math.sqrt(norm_norm_2)/h_bar 233 | norm = norm/h_bar 234 | ransac_camera_height = 1/norm_norm 235 | pitch = math.asin(norm[0,1]/norm_norm) 236 | scale = self.absolute_reference/ransac_camera_height 237 | #0.3 is the max accellerate 238 | if scale - self.scale >0.3: 239 | self.scale += 0.3 240 | elif scale - self.scale<-0.3: 241 | self.scale -= 0.3 242 | else: 243 | self.scale = scale 244 | self.scale_queue.append(self.scale) 245 | if len(self.scale_queue)>self.window_size: 246 | self.scale_queue.popleft() 247 | return np.median(self.scale_queue),1 248 | 249 | 250 | def main(): 251 | # get initial motion pitch by motion matrix 252 | # triangle region norm and height 253 | # selected 254 | # calculate the road norm 255 | # filtering 256 | # scale recovery 257 | camera_height = 1.7 258 | scale_estimator = ScaleEstimator(camera_height) 259 | scale = scale_estimator.scale_calculation() 260 | 261 | if __name__ == '__main__': 262 | main() 263 | -------------------------------------------------------------------------------- /src/rescale_test.py: -------------------------------------------------------------------------------- 1 | # recover ego-motion scale by road geometry information 2 | # @author xiangwei(wangxiangwei.cpp@gmail.com) and zhanghui() 3 | # 4 | 5 | # @function: calcualte the road pitch angle from motion matrix 6 | # @input: the tansformation matrix in SE(3) 7 | # @output:translation angle calculate by t in R^3 and 8 | # rotarion angle calculate from rotation matrix R in SO(3) 9 | 10 | 11 | 12 | from scipy.spatial import Delaunay 13 | from estimate_road_norm import * 14 | import numpy as np 15 | import math 16 | from collections import deque 17 | import matplotlib.pyplot as plt 18 | import scale_calculator as sc 19 | import graph 20 | import param 21 | 22 | class ScaleEstimator: 23 | def __init__(self,absolute_reference,window_size=6): 24 | self.absolute_reference = absolute_reference 25 | self.camera_pitch = 0 26 | self.scale = 1 27 | self.inliers = None 28 | self.scale_queue = deque() 29 | self.window_size = window_size 30 | self.vanish = 185 31 | self.sc = sc.ScaleEstimator(absolute_reference,window_size) 32 | self.gc = graph.GraphChecker([[3,1],[2,2],[2,2],[0,4]]) 33 | self.img_w = param.img_w 34 | self.img_h = param.img_h 35 | def initial_estimation(self,motion_matrix): 36 | 37 | return 0 38 | 39 | ''' 40 | check the three vertice in triangle whether satisfy 41 | (d_1-d_2)*(v_1-v_2)<=0 if not they are outlier 42 | True means outlier 43 | ''' 44 | def check_triangle(self,v,d): 45 | flag=[False,False,False] 46 | a = (v[0]-v[1])*(d[0]-d[1]) 47 | b = (v[0]-v[2])*(d[0]-d[2]) 48 | c = (v[1]-v[2])*(d[1]-d[2]) 49 | if a>0: 50 | flag[0]=True 51 | flag[1]=True 52 | if b>0: 53 | flag[0]=True 54 | flag[1]=True 55 | if c>0: 56 | flag[1]=True 57 | flag[2]=True 58 | return flag 59 | 60 | def find_outliers(self,feature3d,feature2d,triangle_ids): 61 | # suppose every is inlier 62 | outliers = np.ones((feature3d.shape[0])) 63 | for triangle_id in triangle_ids: 64 | data=[] 65 | depths = feature3d[triangle_id,2] 66 | pixel_vs = feature2d[triangle_id,1] 67 | flag = self.check_triangle(pixel_vs,depths) 68 | outlier = triangle_id[flag] 69 | outliers[outlier]-=np.ones(outliers[outlier].shape[0]) 70 | 71 | return outliers 72 | 73 | def feature_selection(self,feature3d,feature2d): 74 | lower_feature_ids = feature2d[:,1]>self.vanish 75 | feature2d = feature2d[lower_feature_ids,:] 76 | feature3d = feature3d[lower_feature_ids,:] 77 | tri = Delaunay(feature2d) 78 | triangle_ids = tri.simplices 79 | ''' 80 | ax1 = plt.subplot(311) 81 | ax2 = plt.subplot(312) 82 | ax3 = plt.subplot(313) 83 | valid_id_1 = self.sc.find_reliability_by_graph(feature3d,feature2d,triangle_ids) 84 | valid_id_2 = self.gc.find_inliers(feature3d,feature2d,triangle_ids) 85 | outliers = self.find_outliers(feature3d,feature2d,triangle_ids) 86 | valid_id_3 = (outliers>=0) 87 | valid_id = valid_id_1 88 | #valid_id =[] 89 | ax1.plot(feature3d[:,2],-feature3d[:,1],'.r') 90 | ax2.plot(feature3d[:,2],-feature3d[:,1],'.r') 91 | ax3.plot(feature3d[:,2],-feature3d[:,1],'.r') 92 | ax1.plot(feature3d[valid_id_1,2],-feature3d[valid_id_1,1],'.g') 93 | ax2.plot(feature3d[valid_id_2,2],-feature3d[valid_id_2,1],'.g') 94 | ax3.plot(feature3d[valid_id_3,2],-feature3d[valid_id_3,1],'.g') 95 | plt.show() 96 | ''' 97 | #valid_id = self.sc.find_reliability_by_graph(feature3d,feature2d,triangle_ids) 98 | valid_id = self.gc.find_inliers(feature3d,feature2d,triangle_ids) 99 | #outliers = self.find_outliers(feature3d,feature2d,triangle_ids) 100 | #valid_id = (outliers>=0) 101 | #valid_id = [] 102 | print('feature rejected ',np.sum(valid_id==False)) 103 | print('feature left ',np.sum(valid_id)) 104 | if(np.sum(valid_id)>10): 105 | feature2d = feature2d[valid_id,:] 106 | feature3d = feature3d[valid_id,:] 107 | tri = Delaunay(feature2d) 108 | triangle_ids = tri.simplices 109 | 110 | b_matrix = np.matrix(np.ones((3,1),np.float)) 111 | data = [] 112 | #calculating the geometry model of each triangle 113 | for triangle_id in triangle_ids: 114 | point_selected = feature3d[triangle_id] 115 | a_array = np.array(point_selected) 116 | a_matrix = np.matrix(a_array) 117 | a_matrix_inv = a_matrix.I 118 | norm = a_matrix_inv*b_matrix 119 | norm_norm_2 = norm.T*norm#the square norm of norm 120 | height = 1/math.sqrt(norm_norm_2) 121 | if norm[1,0]<0: 122 | norm = -norm 123 | height = -height 124 | #height= np.mean(point_selected[:,1]) 125 | pitch = math.asin(-norm[1,0]/math.sqrt(norm_norm_2[0,0])) 126 | pitch_deg = pitch*180/3.1415926 127 | pitch_height = [norm[1,0]/math.sqrt(norm_norm_2[0,0]),pitch_deg,height] 128 | data.append(pitch_height) 129 | data = np.array(data) # all data is saved here 130 | 131 | # initial select by prior information 132 | data_sub = data[data[:,1]>self.camera_pitch-95]#>80 deg 133 | data_sub = data_sub[data[:,1]80 deg 134 | data_sub = data_sub[data_sub[:,2]>0]#under 135 | precomput_h = 0.9*np.median(data_sub[:,2]) 136 | print(precomput_h) 137 | #unvalid_id = data[:,1]>=self.camera_pitch-80 138 | #precomput_h = np.mean(data[unvalid_id,2]) 139 | data_sub_low = data_sub[data_sub[:,2]>precomput_h,:] 140 | data_id = [] 141 | # collect suitable points and triangle 142 | for i in range(0,triangle_ids.shape[0]): 143 | triangle_id = triangle_ids[i] 144 | pitch_deg = data[i,1] 145 | height = data[i,2] 146 | triangle_points = np.array(feature2d[triangle_id],np.int32) 147 | if(pitch_deg>self.camera_pitch-95 and pitch_degprecomput_h): 149 | #if(True): 150 | if(height>precomput_h): 151 | data_id.append(triangle_id[0]) 152 | data_id.append(triangle_id[1]) 153 | data_id.append(triangle_id[2]) 154 | pts = triangle_points.reshape((-1,1,2)) 155 | #data_id = list(np.unique(data_id)) 156 | 157 | #data_id = (feature2d[:,0]>self.img_w*0.4) & (feature2d[:,0]self.img_h*0.666) 159 | point_selected = feature3d[data_id] 160 | ''' 161 | ax1 = plt.subplot(111) 162 | ax1.plot(feature3d[:,2],-feature3d[:,1],'.g') 163 | ax1.plot(point_selected[:,2],-point_selected[:,1],'.y') 164 | plt.show() 165 | ''' 166 | return point_selected 167 | #return feature3d 168 | def scale_calculation_ransac(self,point_selected): 169 | if(point_selected.shape[0]>=12): 170 | # ransac 171 | a_array = np.array(point_selected) 172 | m,b = get_pitch_ransac(a_array,100,0.005) 173 | road_model_ransac = np.matrix(m) 174 | norm = road_model_ransac[0,0:-1] 175 | h_bar = -road_model_ransac[0,-1] 176 | if norm[0,1]<0: 177 | norm = -norm 178 | h_bar = -h_bar 179 | norm_norm_2 = norm*norm.T 180 | norm_norm = math.sqrt(norm_norm_2)/h_bar 181 | norm = norm/h_bar 182 | ransac_camera_height = 1/norm_norm 183 | pitch = math.asin(norm[0,1]/norm_norm) 184 | scale = self.absolute_reference/ransac_camera_height 185 | #0.3 is the max accellerate 186 | if scale - self.scale >0.3: 187 | self.scale += 0.3 188 | elif scale - self.scale<-0.3: 189 | self.scale -= 0.3 190 | else: 191 | self.scale = scale 192 | self.scale_queue.append(self.scale) 193 | if len(self.scale_queue)>self.window_size: 194 | self.scale_queue.popleft() 195 | return np.median(self.scale_queue),1 196 | 197 | 198 | 199 | def scale_calculation(self,feature3d,feature2d,img=None): 200 | point_selected = self.feature_selection(feature3d,feature2d) 201 | return self.scale_calculation_ransac(point_selected) 202 | #return self.sc.scale_calculation_static(point_selected) 203 | # initial ransac 204 | def main(): 205 | # get initial motion pitch by motion matrix 206 | # triangle region norm and height 207 | # selected 208 | # calculate the road norm 209 | # filtering 210 | # scale recovery 211 | camera_height = 1.7 212 | scale_estimator = ScaleEstimator(camera_height) 213 | scale = scale_estimator.scale_calculation() 214 | 215 | if __name__ == '__main__': 216 | main() 217 | -------------------------------------------------------------------------------- /src/rescale_test.py.origin: -------------------------------------------------------------------------------- 1 | # recover ego-motion scale by road geometry information 2 | # @author xiangwei(wangxiangwei.cpp@gmail.com) and zhanghui() 3 | # 4 | 5 | # @function: calcualte the road pitch angle from motion matrix 6 | # @input: the tansformation matrix in SE(3) 7 | # @output:translation angle calculate by t in R^3 and 8 | # rotarion angle calculate from rotation matrix R in SO(3) 9 | 10 | 11 | 12 | from scipy.spatial import Delaunay 13 | from estimate_road_norm import * 14 | import numpy as np 15 | import math 16 | from collections import deque 17 | import matplotlib.pyplot as plt 18 | 19 | 20 | 21 | class ScaleEstimator: 22 | def __init__(self,absolute_reference,window_size=6): 23 | self.absolute_reference = absolute_reference 24 | self.camera_pitch = 0 25 | self.scale = 1 26 | self.inliers = None 27 | self.scale_queue = deque() 28 | self.window_size = window_size 29 | self.vanish = 185 30 | def initial_estimation(self,motion_matrix): 31 | 32 | return 0 33 | 34 | ''' 35 | check the three vertice in triangle whether satisfy 36 | (d_1-d_2)*(v_1-v_2)<=0 if not they are outlier 37 | True means inlier 38 | ''' 39 | def check_triangle(self,v,d): 40 | flag=[False,False,False] 41 | a = (v[0]-v[1])*(d[0]-d[1]) 42 | b = (v[0]-v[2])*(d[0]-d[2]) 43 | c = (v[1]-v[2])*(d[1]-d[2]) 44 | if a>0: 45 | flag[0]=True 46 | flag[1]=True 47 | if b>0: 48 | flag[0]=True 49 | flag[1]=True 50 | if c>0: 51 | flag[1]=True 52 | flag[2]=True 53 | return flag 54 | 55 | def find_outliers(self,feature3d,feature2d,triangle_ids): 56 | # suppose every is inlier 57 | outliers = np.ones((feature3d.shape[0])) 58 | for triangle_id in triangle_ids: 59 | data=[] 60 | depths = feature3d[triangle_id,2] 61 | pixel_vs = feature2d[triangle_id,1] 62 | flag = self.check_triangle(pixel_vs,depths) 63 | outlier = triangle_id[flag] 64 | outliers[outlier]-=np.ones(outliers[outlier].shape[0]) 65 | 66 | return outliers 67 | 68 | def feature_selection(self,feature3d,feature2d): 69 | lower_feature_ids = feature2d[:,1]>self.vanish 70 | feature2d = feature2d[lower_feature_ids,:] 71 | feature3d = feature3d[lower_feature_ids,:] 72 | tri = Delaunay(feature2d) 73 | triangle_ids = tri.simplices 74 | outliers = self.find_outliers(feature3d,feature2d,triangle_ids) 75 | 76 | print('feature rejected ',np.sum(outliers<0)) 77 | print('feature left ',np.sum(outliers>=0)) 78 | if(np.sum(outliers==1)>10): 79 | feature2d = feature2d[outliers>=0,:] 80 | feature3d = feature3d[outliers>=0,:] 81 | tri = Delaunay(feature2d) 82 | triangle_ids = tri.simplices 83 | 84 | b_matrix = np.matrix(np.ones((3,1),np.float)) 85 | data = [] 86 | #calculating the geometry model of each triangle 87 | for triangle_id in triangle_ids: 88 | point_selected = feature3d[triangle_id] 89 | a_array = np.array(point_selected) 90 | a_matrix = np.matrix(a_array) 91 | a_matrix_inv = a_matrix.I 92 | norm = a_matrix_inv*b_matrix 93 | norm_norm_2 = norm.T*norm#the square norm of norm 94 | height = 1/math.sqrt(norm_norm_2) 95 | if norm[1,0]<0: 96 | norm = -norm 97 | height = -height 98 | pitch = math.asin(-norm[1,0]/math.sqrt(norm_norm_2[0,0])) 99 | pitch_deg = pitch*180/3.1415926 100 | pitch_height = [norm[1,0]/math.sqrt(norm_norm_2[0,0]),pitch_deg,height] 101 | data.append(pitch_height) 102 | data = np.array(data) # all data is saved here 103 | 104 | # initial select by prior information 105 | data_sub = data[data[:,1]>self.camera_pitch-95]#>80 deg 106 | data_sub = data_sub[data[:,1]80 deg 107 | data_sub = data_sub[data_sub[:,2]>0]#under 108 | precomput_h = 0.9*np.median(data_sub[:,2]) 109 | data_sub_low = data_sub[data_sub[:,2]>precomput_h,:] 110 | data_id = [] 111 | # collect suitable points and triangle 112 | for i in range(0,triangle_ids.shape[0]): 113 | triangle_id = triangle_ids[i] 114 | pitch_deg = data[i,1] 115 | height = data[i,2] 116 | triangle_points = np.array(feature2d[triangle_id],np.int32) 117 | if(pitch_deg>self.camera_pitch-95 and pitch_degprecomput_h): 119 | data_id.append(triangle_id[0]) 120 | data_id.append(triangle_id[1]) 121 | data_id.append(triangle_id[2]) 122 | pts = triangle_points.reshape((-1,1,2)) 123 | point_selected = feature3d[data_id] 124 | self.initial_points = feature2d[data_id] 125 | return point_selected 126 | 127 | def scale_calculation(self,feature3d,feature2d,img=None): 128 | point_selected = self.feature_selection(feature3d,feature2d) 129 | # initial ransac 130 | if(point_selected.shape[0]>=12): 131 | # ransac 132 | a_array = np.array(point_selected) 133 | m,b = get_pitch_ransac(a_array,30,0.005) 134 | inlier_id = get_inliers(m,feature3d[:,:],0.01) 135 | inliers = feature3d[inlier_id,:] 136 | inliers_2d = feature2d[inlier_id,:] 137 | self.inliers = inliers_2d 138 | outliers_2d = feature2d[inlier_id==False,:] 139 | print('inilier',inliers.shape[0]) 140 | road_model_ransac = np.matrix(m) 141 | norm = road_model_ransac[0,0:-1] 142 | h_bar = -road_model_ransac[0,-1] 143 | if norm[0,1]<0: 144 | norm = -norm 145 | h_bar = -h_bar 146 | norm_norm_2 = norm*norm.T 147 | norm_norm = math.sqrt(norm_norm_2)/h_bar 148 | norm = norm/h_bar 149 | ransac_camera_height = 1/norm_norm 150 | pitch = math.asin(norm[0,1]/norm_norm) 151 | scale = self.absolute_reference/ransac_camera_height 152 | #0.3 is the max accellerate 153 | if scale - self.scale >0.3: 154 | self.scale += 0.3 155 | elif scale - self.scale<-0.3: 156 | self.scale -= 0.3 157 | else: 158 | self.scale = scale 159 | self.scale_queue.append(self.scale) 160 | if len(self.scale_queue)>self.window_size: 161 | self.scale_queue.popleft() 162 | return np.median(self.scale_queue),1 163 | 164 | 165 | def main(): 166 | # get initial motion pitch by motion matrix 167 | # triangle region norm and height 168 | # selected 169 | # calculate the road norm 170 | # filtering 171 | # scale recovery 172 | camera_height = 1.7 173 | scale_estimator = ScaleEstimator(camera_height) 174 | scale = scale_estimator.scale_calculation() 175 | 176 | if __name__ == '__main__': 177 | main() 178 | -------------------------------------------------------------------------------- /src/scale_calculator.py: -------------------------------------------------------------------------------- 1 | # recover ego-motion scale by road geometry information 2 | # @author xiangwei(wangxiangwei.cpp@gmail.com) and zhanghui() 3 | # 4 | 5 | # @function: calcualte the road pitch angle from motion matrix 6 | # @input: the tansformation matrix in SE(3) 7 | # @output:translation angle calculate by t in R^3 and 8 | # rotarion angle calculate from rotation matrix R in SO(3) 9 | 10 | 11 | 12 | from scipy.spatial import Delaunay 13 | from estimate_road_norm import * 14 | import numpy as np 15 | import math 16 | from collections import deque 17 | import matplotlib.pyplot as plt 18 | 19 | 20 | 21 | class ScaleEstimator: 22 | def __init__(self,absolute_reference,window_size=6,vanish=185,focus=718): 23 | self.absolute_reference = absolute_reference 24 | self.camera_pitch = -0.5*np.pi/180 25 | self.scale = None 26 | self.inliers = None 27 | self.scale_queue = deque() 28 | self.motion_queue= deque() 29 | self.window_size = window_size 30 | self.vanish = vanish 31 | self.focus = focus 32 | self.b_matrix = np.ones((3,1),np.float) 33 | self.all_features=[] 34 | self.correct_distance_features=[] 35 | self.flat_features=[] 36 | self.all_feature=[] 37 | self.correct_distance_feature=[] 38 | self.flat_feature=[] 39 | self.flat_feature_2d=[] 40 | self.img =None 41 | def initial_estimation(self,motion_t): 42 | pitch = np.arcsin(motion_t[1])*180/np.pi 43 | print('initial pitch',pitch) 44 | self.motion_queue.append(motion_t.reshape(-1)) 45 | #self.pitch = pitch 46 | return pitch 47 | 48 | # only work when vehicle move forward \frac{fX}{z*(z+1)}>1 49 | def check_distance(self,feature3d): 50 | flag_x = (feature3d[:,2]*(feature3d[:,2]+1) - np.abs(self.focus*feature3d[:,0]))<0 51 | flag_y = (feature3d[:,2]*(feature3d[:,2]+1) - np.abs(self.focus*feature3d[:,1]))<0 52 | flag = flag_x | flag_y 53 | print(np.sum(flag),flag.shape) 54 | return flag 55 | 56 | def triangle2region_graph(self,triangle_ids): 57 | graph = [] 58 | graph_meta = [] 59 | max_id = np.max(triangle_ids) 60 | for i in range(0,max_id+1): 61 | graph_meta.append(set()) 62 | for i in range(0,triangle_ids.shape[0]): 63 | graph.append([]) 64 | for i in range(0,triangle_ids.shape[0]): 65 | a,b,c = triangle_ids[i] 66 | intersect = graph_meta[a].intersection(graph_meta[b]) 67 | if len(intersect)!=0: 68 | graph[i].append(list(intersect)[0]) 69 | graph[list(intersect)[0]].append(i) 70 | intersect = graph_meta[a].intersection(graph_meta[c]) 71 | if len(intersect)!=0: 72 | graph[i].append(list(intersect)[0]) 73 | graph[list(intersect)[0]].append(i) 74 | intersect = graph_meta[b].intersection(graph_meta[c]) 75 | if len(intersect)!=0: 76 | graph[i].append(list(intersect)[0]) 77 | graph[list(intersect)[0]].append(i) 78 | graph_meta[a].add(i) 79 | graph_meta[b].add(i) 80 | graph_meta[c].add(i) 81 | return graph 82 | 83 | 84 | # @input shape n*3 n triangles 85 | # @output array of set 86 | def triangle2graph(self,triangle_ids): 87 | graph=[] 88 | max_id = np.max(triangle_ids) 89 | for i in range(0,max_id+1): 90 | graph.append([]) 91 | for triangle_id in triangle_ids: 92 | triangle_id = np.sort(triangle_id) 93 | if triangle_id[1] not in graph[triangle_id[0]]: 94 | graph[triangle_id[0]].append(triangle_id[1]) 95 | if triangle_id[2] not in graph[triangle_id[0]]: 96 | graph[triangle_id[0]].append(triangle_id[2]) 97 | if triangle_id[2] not in graph[triangle_id[1]]: 98 | graph[triangle_id[1]].append(triangle_id[2]) 99 | return graph 100 | ''' 101 | check the three vertice in triangle whether satisfy 102 | (d_1-d_2)*(v_1-v_2)<=0 if not they are outlier 103 | True means inlier 104 | ''' 105 | def check_triangle(self,v,d): 106 | flag=[False,False,False] 107 | a = (v[0]-v[1])*(d[0]-d[1]) 108 | b = (v[0]-v[2])*(d[0]-d[2]) 109 | c = (v[1]-v[2])*(d[1]-d[2]) 110 | if a>0: 111 | flag[0]=True 112 | flag[1]=True 113 | if b>0: 114 | flag[0]=True 115 | flag[1]=True 116 | if c>0: 117 | flag[1]=True 118 | flag[2]=True 119 | return np.array(flag) 120 | 121 | def check_depth(self,v,d): 122 | if (v[0]-v[1])*(d[0]-d[1])>0: 123 | return True 124 | else: 125 | return False 126 | 127 | def find_reliability_by_graph(self,feature3d,feature2d,triangle_ids): 128 | feature_graph = self.triangle2graph(triangle_ids) 129 | reliability = 0.8*np.ones((feature3d.shape[0])) 130 | for i in range(0,len(feature_graph)): 131 | for j in feature_graph[i]: 132 | a = reliability[i]*reliability[j] 133 | b = (1-reliability[i])*reliability[j] 134 | c = (1-reliability[j])*reliability[i] 135 | d = (1-reliability[i])*(1-reliability[j]) 136 | # abmormal 137 | if self.check_depth(feature2d[[i,j],1],feature3d[[i,j],2]): 138 | reliability[i] = (0.25*c)/(0.25*(b+c)+0.5*d) 139 | reliability[j] = (0.25*b)/(0.25*(b+c)+0.5*d) 140 | else: 141 | #normal 142 | reliability[i] = (a+0.25*c)/(a+0.25*(b+c)+0.5*d) 143 | reliability[j] = (a+0.25*b)/(a+0.25*(b+c)+0.5*d) 144 | 145 | valid_id = reliability>0.8 146 | print('reliability',np.min(reliability),np.median(reliability),np.max(reliability)) 147 | print('feature rejected ',np.sum(valid_id==False)) 148 | print('feature left ',np.sum(valid_id)) 149 | return valid_id 150 | 151 | def find_outliers(self,feature3d,feature2d,triangle_ids): 152 | # suppose every is inlier 153 | outliers = np.ones((feature3d.shape[0])) 154 | 155 | for triangle_id in triangle_ids: 156 | data=[] 157 | depths = feature3d[triangle_id,2] 158 | pixel_vs = feature2d[triangle_id,1] 159 | flag = self.check_triangle(pixel_vs,depths) 160 | outlier = triangle_id[flag] 161 | inlier = triangle_id[~flag] 162 | outliers[outlier]-=np.ones(outliers[outlier].shape[0]) 163 | outliers[inlier]+=np.ones(outliers[inlier].shape[0]) 164 | print('feature rejected ',np.sum(outliers<0)) 165 | print('feature left ',np.sum(outliers>=0)) 166 | valid_id = (outliers>=0) 167 | return valid_id 168 | 169 | def compare(self,a,b,threshold=0.1): 170 | if a - b< -threshold: 171 | return -1 172 | elif a-b> threshold: 173 | return 1 174 | else: 175 | return 0 176 | 177 | def feature_selection_by_tri_graph(self,feature3d,triangle_ids): 178 | graph = self.triangle2region_graph(triangle_ids) 179 | #calculating the geometry model of each triangle 180 | triangles = np.array([np.matrix(feature3d[triangle_id]) for triangle_id in triangle_ids]) 181 | triangles_i = np.array([np.matrix(feature3d[triangle_id]).I for triangle_id in triangle_ids]) 182 | normals = (triangles_i@self.b_matrix).reshape(-1,3) 183 | normals_len = np.sqrt(np.sum(normals*normals,1)).reshape(-1,1) 184 | normals = normals/normals_len 185 | pitch_deg = np.arcsin(-normals[:,1])*180/np.pi #[-90,90] 186 | heights = np.mean(triangles[:,:,1],1) 187 | 188 | p_road = (-70 - pitch_deg)/20-0.2 189 | p_road[p_road<0] = 0 190 | valid_pitch_id = pitch_deg<-80 191 | unvalid_pitch_id = pitch_deg>=-80 192 | valid_triangle_ids = bool2id(valid_pitch_id) 193 | observation_matrix = np.array([[0.33,0.33,0.33],[0.03,0.07,0.90],[0.90,0.07,0.03],[0.05,0.9,0.05]]) 194 | for valid_triangle_id in valid_triangle_ids: 195 | mb_ids = np.array(graph[valid_triangle_id]) 196 | 197 | #plt.plot(heights[mb_ids],'.-g') 198 | #plt.plot(p_road[mb_ids],'.-r') 199 | #plt.plot(heights[valid_triangle_id],'g*') 200 | #plt.plot(p_road[valid_triangle_id],'r*') 201 | ha = heights[valid_triangle_id] 202 | pa = p_road[valid_triangle_id] 203 | prab=[] 204 | #print('initial prab',pa) 205 | for mb_id in mb_ids: 206 | hc = heights[mb_id] 207 | pc = p_road[mb_id] 208 | cr = self.compare(hc,ha) 209 | potential_matrix = np.array([(1-pa)*(1-pc),(1-pa)*pc,pa*(1-pc),pa*pc]) 210 | pa = observation_matrix[2:4,cr+1]@potential_matrix[2:4] /(observation_matrix[:,cr+1]@potential_matrix) 211 | prab.append(pa) 212 | #print(pa) 213 | p_road[valid_triangle_id] = pa 214 | #plt.ylim() 215 | print('triangle left ',np.sum(valid_pitch_id),'from',valid_pitch_id.shape[0]) 216 | height_level = (np.mean(heights[unvalid_pitch_id])) 217 | self.height_level = height_level 218 | print('height level',height_level) 219 | valid_id = p_road>0.5 #valid_id = valid_pitch_id 220 | print('triangle left final',np.sum(valid_id),'from',valid_id.shape[0]) 221 | valid_points_id = np.unique(triangle_ids[valid_id].reshape(-1)) 222 | return valid_points_id 223 | 224 | 225 | def feature_selection_by_tri(self,feature3d,triangle_ids): 226 | 227 | #calculating the geometry model of each triangle 228 | triangles = np.array([np.matrix(feature3d[triangle_id]) for triangle_id in triangle_ids]) 229 | triangles_i = np.array([np.matrix(feature3d[triangle_id]).I for triangle_id in triangle_ids]) 230 | normals = (triangles_i@self.b_matrix).reshape(-1,3) 231 | normals_len = np.sqrt(np.sum(normals*normals,1)).reshape(-1,1) 232 | normals = normals/normals_len 233 | pitch_deg = np.arcsin(-normals[:,1])*180/np.pi #[-90,90] 234 | 235 | valid_pitch_id = pitch_deg<-80 236 | 237 | print('triangle left ',np.sum(valid_pitch_id),'from',valid_pitch_id.shape[0]) 238 | heights = np.mean(triangles[:,:,1],1) 239 | unvalid_pitch_id = pitch_deg>=-80 240 | height_level = (np.mean(heights[unvalid_pitch_id])) 241 | self.height_level = height_level 242 | print('height level',height_level) 243 | valid_height_id = heights>height_level 244 | valid_id = valid_pitch_id & valid_height_id 245 | print('triangle left final',np.sum(valid_id),'from',valid_id.shape[0]) 246 | #valid_id = valid_pitch_id 247 | valid_points_id = np.unique(triangle_ids[valid_id].reshape(-1)) 248 | return valid_points_id 249 | 250 | def feature_selection(self,feature3d,feature2d): 251 | # 1 select feature below vanish point 252 | lower_feature_ids = feature2d[:,1]>self.vanish 253 | feature2d = feature2d[lower_feature_ids,:] 254 | feature3d = feature3d[lower_feature_ids,:] 255 | #self.distribution(feature3d) 256 | # 2. select feature with wrong depth estimation 257 | tri = Delaunay(feature2d) 258 | triangle_ids = tri.simplices 259 | #valid_id = self.find_reliability_by_graph(feature3d,feature2d,triangle_ids) 260 | valid_id = self.find_outliers(feature3d,feature2d,triangle_ids) 261 | 262 | 263 | if(valid_id.shape[0]>3): 264 | feature2d = feature2d[valid_id,:] 265 | feature3d = feature3d[valid_id,:] 266 | tri = Delaunay(feature2d) 267 | triangle_ids = tri.simplices 268 | else: 269 | print('no enough feature for triangulation') 270 | return None 271 | 272 | # 3. select feature with similar model with road 273 | selected_id = self.feature_selection_by_tri(feature3d,triangle_ids) 274 | if(len(selected_id)>0): 275 | self.flat_feature_2d = feature2d[selected_id] 276 | return feature3d[selected_id] 277 | else: 278 | print('no enough flat feature') 279 | return None 280 | 281 | def road_model_calculation(self,feature3d): 282 | return self.road_model_calculation_static(feature3d) 283 | 284 | def remove_single(self,feature3d,dis,bins): 285 | flag_single = (dis==1) 286 | bins_single = bins[1:][flag_single] 287 | if np.sum(bins_single)>0: 288 | bin_single = bins_single[0] 289 | unvalid_flag = (feature3d[:,1]>=bin_single-0.1)&(feature3d[:,1]<=bin_single) 290 | for bin_single in bins_single[1:]: 291 | unvalid_flag |= (feature3d[:,1]>bin_single-0.1)&(feature3d[:,1]<=bin_single) 292 | feature3d=feature3d[unvalid_flag==False,:] 293 | return feature3d 294 | def road_model_calculation_static_tri(self,heights): 295 | print('flat features',len(heights)) 296 | heights_inv = 1/heights 297 | dis,bins =np.histogram(heights_inv,bins = np.array(range(0,20))*0.1) 298 | #heights_inv = self.remove_single(heights_inv,dis,bins) 299 | dis[dis==1]=0 300 | modes = self.check_mode(dis,bins) 301 | print('mode number',len(modes)) 302 | if len(modes)==0: 303 | return np.median(heights_inv),0,1 304 | else: 305 | reverse_modes = self.check_reverse_mode(dis) 306 | mode_right = int(modes[0][-1]*10) 307 | mode_left = int(modes[0][0]*10) 308 | mode = (mode_left+mode_right)/2 309 | skewness = 0 310 | return mode/10,0,1; 311 | if len(modes)>1: 312 | skewness = self.check_skewness(heights_inv,mode=mode/10) 313 | print('skewness',skewness,'\n') 314 | else: 315 | skewness = self.check_skewness(heights_inv,mode=mode/10,method='p2') 316 | print('skewness',skewness,'\n') 317 | if skewness>-0.3: 318 | return mode/10,0,1 319 | else: 320 | reverse_modes_left = reverse_modes[:mode_left] 321 | left = bins[1:mode_left+1][reverse_modes_left][-1] 322 | return left,0,1 323 | 324 | def road_model_calculation_static(self,feature3d): 325 | print('flat features',feature3d.shape) 326 | dis,bins =np.histogram(feature3d[:,1],bins = np.array(range(0,170))*0.1) 327 | feature3d = self.remove_single(feature3d,dis,bins) 328 | dis[dis==1]=0 329 | modes = self.check_mode(dis,bins) 330 | print('mode number',len(modes)) 331 | if len(modes)==0: 332 | if feature3d.shape[0]>0: 333 | return np.median(feature3d[:,1]),0,1 334 | else: 335 | return self.height_level,0,1 336 | else: 337 | reverse_modes = self.check_reverse_mode(dis) 338 | mode_right = int(modes[-1][-1]*10) 339 | mode_left = int(modes[-1][0]*10) 340 | mode = (mode_left+mode_right)/2 341 | reverse_modes_left = reverse_modes[:mode_left] 342 | reverse_modes_right = reverse_modes[mode_right:] 343 | left = bins[1:mode_left+1][reverse_modes_left][-1] 344 | right = bins[mode_right+1:][reverse_modes_right][0] 345 | 346 | skewness = self.check_skewness(feature3d[:,1],mode=mode/10) 347 | print('skewness',skewness) 348 | if skewness>0.3: 349 | # unimodel or multi-model 350 | left = right 351 | right = 15.1 352 | return left,0,1 353 | else: 354 | return mode/10,0,1 355 | ''' 356 | print('mode',mode_left,mode_right,'left',left,'right',right) 357 | valid_id = (feature3d[:,1]>(left)) & (feature3d[:,1]0: 360 | return np.median(feature3d[valid_id,1]),0,np.std(feature3d[valid_id,1]+1/np.sum(valid_id)) 361 | else: 362 | return left,0,1 363 | ''' 364 | 365 | 366 | def road_model_calculation_ransac(self,feature3d): 367 | # ransac 368 | a_array = np.array(feature3d) 369 | m,b = get_pitch_ransac(a_array,30,0.005) 370 | inlier_id = get_inliers(m,feature3d[:,:],0.01) 371 | inliers = feature3d[inlier_id,:] 372 | road_model_ransac = np.array(m) 373 | normal = road_model_ransac[0:-1] 374 | h_bar = -road_model_ransac[-1] 375 | if normal[1]<0: 376 | normal = -normal 377 | h_bar = -h_bar 378 | 379 | 380 | normal_len = np.sqrt(np.sum(normal*normal)) 381 | ransac_camera_height = h_bar/normal_len 382 | #ransac_camera_height = np.median(feature3d[:,1]) 383 | pitch = np.arcsin(-normal[1]/normal_len) 384 | return ransac_camera_height,pitch,inliers 385 | 386 | def distribution(self,feature3d): 387 | dis,bins =np.histogram(feature3d[:,1],bins = np.array(range(0,100))*0.1) 388 | plt.plot(dis) 389 | 390 | def feature_remap(self,feature3d): 391 | y=feature3d[:,1]*np.cos(self.camera_pitch)-feature3d[:,2]*np.sin(self.camera_pitch) 392 | z=feature3d[:,1]*np.sin(self.camera_pitch)+feature3d[:,2]*np.cos(self.camera_pitch) 393 | feature3d[:,1]=y 394 | feature3d[:,2]=z 395 | 396 | def scale_filtering(self,scale): 397 | self.scale_queue.append(scale) 398 | if len(self.scale_queue)>self.window_size: 399 | self.scale_queue.popleft() 400 | return np.median(self.scale_queue) 401 | def scale_calculation_static(self,point_selected): 402 | self.feature_remap(point_selected) 403 | if(point_selected is not None): 404 | height,pitch,std= self.road_model_calculation(point_selected) 405 | scale = self.absolute_reference/height 406 | else: 407 | scale = self.absolute_reference/self.height_level 408 | print('no enough feature on road') 409 | return self.scale_filtering(scale),std 410 | 411 | def scale_calculation(self,feature3d,feature2d,img=None): 412 | scale = 0 413 | std = 100 414 | self.feature_remap(feature3d) 415 | point_selected = self.feature_selection(feature3d,feature2d) 416 | self.flat_feature = point_selected 417 | if(point_selected is not None): 418 | height,pitch,std= self.road_model_calculation(point_selected) 419 | scale = self.absolute_reference/height 420 | else: 421 | scale = self.absolute_reference/self.height_level 422 | print('no enough feature on road') 423 | return self.scale_filtering(scale),std 424 | 425 | 426 | #return scale,std 427 | 428 | def check_reverse_mode(self,dis_data): 429 | dis_data = np.array(dis_data) 430 | flag = np.zeros(dis_data.shape[0]) 431 | min_data = np.min(dis_data) 432 | 433 | if dis_data[0] == min_data: 434 | flag[0]=1 435 | 436 | if dis_data[-1] == min_data: 437 | flag[-1]=1 438 | flag_l = dis_data[1:-1] <=dis_data[0:-2] 439 | flag_r = dis_data[1:-1] <= dis_data[2:] 440 | flag_n = (dis_data[1:-1] == dis_data[2:]) & (dis_data[1:-1] == dis_data[0:-2]) 441 | 442 | flag[1:-1] = flag_l & flag_r &(~flag_n) 443 | return flag>0.5 444 | 445 | 446 | def check_mode(self,dis_data,bins): 447 | dis_data = np.array(dis_data) 448 | flag = np.zeros(dis_data.shape[0]) 449 | max_data = np.max(dis_data) 450 | 451 | if max_data<=2: 452 | return [] 453 | 454 | if dis_data[0] == max_data: 455 | flag[0]=1 456 | 457 | if dis_data[-1] == max_data: 458 | flag[-1]=1 459 | flag_l = dis_data[1:-1] >= dis_data[0:-2] 460 | flag_r = dis_data[1:-1] >= dis_data[2:] 461 | flag_v = dis_data[1:-1] >= 0.33* max_data 462 | flag_n = dis_data[1:-1] >=2 463 | flag[1:-1] = flag_l & flag_r & flag_v & flag_n 464 | 465 | modes_flag = flag>0.5 466 | modes = bins[1:][modes_flag] 467 | print(modes) 468 | modes_final = [] 469 | mode_final=[] 470 | if np.sum(modes_flag)==1: 471 | modes_final.append([modes]) 472 | elif np.sum(modes_flag)>1: 473 | for mode in modes: 474 | if len(mode_final)==0 or mode - mode_final[-1]<0.11: 475 | mode_final.append(mode) 476 | else: 477 | modes_final.append(mode_final[:]) 478 | mode_final.clear() 479 | mode_final.append(mode) 480 | if len(mode_final)>0: 481 | modes_final.append(mode_final) 482 | 483 | return modes_final 484 | 485 | 486 | def check_skewness(self,data,mode=None,method='p1'): 487 | print('mean,meidan,std',np.mean(data),np.median(data),np.std(data)) 488 | if method=='p2': 489 | skew = 3*(np.mean(data) - np.median(data))/np.std(data) 490 | elif method =='p1': 491 | if mode is None: 492 | dis,bins =np.histogram(data,bins = np.array(range(0,170))*0.1) 493 | dis[dis==1]=0 494 | flag = (dis == np.max(dis)) 495 | mode = np.mean(bins[1:][flag]) 496 | skew = (np.mean(data) - mode)/np.std(data) 497 | return skew 498 | def skewness_analysis(self): 499 | #all_skew = self.check_skewness(self.all_feature[:,1]) 500 | #cor_skew = self.check_skewness(self.correct_distance_feature[:,1]) 501 | flat_skew= self.check_skewness(self.flat_feature[:,1]) 502 | print('skewness',flat_skew) 503 | return flat_skew 504 | 505 | def mode_analysis(self): 506 | dis,bins =np.histogram(self.flat_feature[:,1],bins = np.array(range(0,100))*0.1,density=True) 507 | return self.check_mode(dis,bins) 508 | 509 | def plot_distribution(self,label,img,scale=1): 510 | 511 | ''' 512 | ax = plt.subplot(111) 513 | all_features = np.array(self.all_features) 514 | dis,bins =np.histogram(all_features[:,1],bins = np.array(range(0,30))*0.1,density=True) 515 | ax.plot(bins[:-1],0.1*dis,'r',label='All Features') 516 | correct_features = np.array(self.correct_distance_features) 517 | dis,bins =np.histogram(correct_features[:,1],bins = np.array(range(0,30))*0.1,density=True) 518 | ax.plot(bins[:-1],0.1*dis,'g',label='Depth-correct Features') 519 | flat_features = np.array(self.flat_features) 520 | dis,bins =np.histogram(flat_features[:,1],bins = np.array(range(0,30))*0.1,density=True) 521 | #ax.plot(bins[:-1],dis,'y') 522 | plt.title('Feature Points Vertical Distribution') 523 | plt.legend() 524 | ''' 525 | ax2 = plt.subplot(221) 526 | if self.all_feature is not None and len(self.all_feature)>0: 527 | all_feature = np.array(self.all_feature) 528 | dis,bins =np.histogram(all_feature[:,1],bins = np.array(range(0,50))*0.1) 529 | #ax2.plot(bins[:-1],0.1*dis,'r-*',label='All Features') 530 | if len(self.correct_distance_feature)>0: 531 | correct_feature = np.array(self.correct_distance_feature) 532 | dis,bins =np.histogram(correct_feature[:,1],bins = np.array(range(0,50))*0.1) 533 | #ax2.plot(bins[:-1],0.1*dis,'g-*',label='Depth-correct Features') 534 | if self.flat_feature is not None and len(self.flat_feature)>0: 535 | flat_feature = np.array(self.flat_feature) 536 | dis,bins =np.histogram(flat_feature[:,1],bins = np.array(range(0,50))*0.1,density=True) 537 | ax2.plot(bins[:-1],0.1*dis,'y-*',label='Selected Features') 538 | ax2.set_title('vertical distribution') 539 | ax2.set_xlabel('y') 540 | ax2.set_ylabel('') 541 | 542 | #ax2.legend(loc='upper left') 543 | 544 | 545 | ax3 = plt.subplot(222) 546 | ax3.plot(self.all_feature[:,2],-self.all_feature[:,1],'.r',label='All Features') 547 | ax3.plot(self.correct_distance_feature[:,2],-self.correct_distance_feature[:,1],'.g',label='Depth-correct Features') 548 | ax3.set_xlabel('z') 549 | ax3.set_ylabel('-y') 550 | ax3.set_title('feature projection') 551 | if self.flat_feature is not None and len(self.flat_feature)>0: 552 | ax3.plot(self.flat_feature[:,2],-self.flat_feature[:,1],'.y',label='Selected Features') 553 | #ax3.legend() 554 | ax4 = plt.subplot(212) 555 | #draw_feature(img,self.flat_feature_2d,) 556 | #draw_feature(img,self.flat_feature_2d[self.flat_feature[:,2]>60,:],(255,0,0)) 557 | ax4.imshow(self.img) 558 | #plt.savefig('case'+label+'.pdf') 559 | 560 | plt.show() 561 | 562 | def check_full_distribution(self,feature3d,feature2d,scale,img): 563 | #self.feature_remap(feature3d) 564 | lower_feature_ids = feature2d[:,1]>self.vanish 565 | feature3d = feature3d[lower_feature_ids,:] 566 | feature2d = feature2d[lower_feature_ids,:] 567 | self.all_feature = feature3d.copy() 568 | for feature in feature3d: 569 | self.all_features.append(feature*scale) 570 | 571 | draw_feature(img,feature2d,(255,0,0)) 572 | tri = Delaunay(feature2d) 573 | triangle_ids = tri.simplices 574 | #valid_id = self.find_outliers(feature3d,feature2d,triangle_ids) 575 | valid_id = self.find_reliability_by_graph(feature3d,feature2d,triangle_ids) 576 | if(valid_id.shape[0]>3): 577 | feature2d = feature2d[valid_id,:] 578 | feature3d = feature3d[valid_id,:] 579 | draw_feature(img,feature2d,(0,255,0)) 580 | self.correct_distance_feature =feature3d.copy() 581 | for feature in feature3d: 582 | self.correct_distance_features.append(feature*scale) 583 | tri = Delaunay(feature2d) 584 | triangle_ids = tri.simplices 585 | else: 586 | return 587 | 588 | selected_id = self.feature_selection_by_tri(feature3d,triangle_ids) 589 | #selected_id = self.feature_selection_by_tri_graph(feature3d,triangle_ids) 590 | if(len(selected_id)>0): 591 | point_selected = feature3d[selected_id] 592 | point_selected_2d = feature2d[selected_id] 593 | self.flat_feature = point_selected.copy() 594 | for feature in point_selected: 595 | self.flat_features.append(feature*scale) 596 | 597 | 598 | draw_feature(img,point_selected_2d,(255,255,0)) 599 | self.img = img 600 | 601 | 602 | def bool2id(flag): 603 | id_array = np.array(range(0,flag.shape[0])) 604 | ids = id_array[flag] 605 | return ids 606 | 607 | def draw_feature(img,feature,color=(255,255,0)): 608 | for i in range(feature.shape[0]): 609 | cv2.circle(img,(int(feature[i,0]),int(feature[i,1])),3,color,-1) 610 | 611 | 612 | def main(): 613 | # get initial motion pitch by motion matrix 614 | # triangle region norm and height 615 | # selected 616 | # calculate the road norm 617 | # filtering 618 | # scale recovery 619 | camera_height = 1.7 620 | scale_estimator = ScaleEstimator(camera_height) 621 | scale = scale_estimator.scale_calculation() 622 | 623 | if __name__ == '__main__': 624 | main() 625 | 626 | 627 | ''' 628 | #heights = (1/normals_len).reshape(-1) 629 | #heights = np.mean(triangles[:,:,1],1) 630 | #if np.sum(valid_pitch_id)==0: 631 | # return [] 632 | #precomput_h = np.median(heights[valid_pitch_id]) 633 | #height_sorted= np.sort(heights[valid_pitch_id]) 634 | #feature_height_sorted = np.sort(feature3d[:,1]) 635 | #print(heights) 636 | #precomput_h_lower = height_sorted[3*height_sorted.shape[0]//4] 637 | #precomput_h_lower_feature = feature_height_sorted[3*feature_height_sorted.shape[0]//4] 638 | #valid_height_id= heights> max(precomput_h_lower,precomput_h_lower_feature) 639 | 640 | 641 | #valid_id = valid_pitch_id & valid_height_id 642 | #print(triangle_ids.shape,valid_id.shape) 643 | 644 | #near_feature_ids = self.check_distance(feature3d) 645 | #feature2d = feature2d[near_feature_ids,:] 646 | #feature3d = feature3d[near_feature_ids,:] 647 | #draw_feature(img,feature2d,(0,255,0)) 648 | #plt.plot(feature3d[:,2],-feature3d[:,1],'.r') 649 | ''' 650 | -------------------------------------------------------------------------------- /src/thirdparty/MonocularVO/README.md: -------------------------------------------------------------------------------- 1 | **Keywords:** FAST Feature Detection, LK Feature Tracking, Five-Point Motion Estimation 2 | 3 | port from https://github.com/yueying/LearningVO 4 | 5 | ![map](map.png) 6 | 7 | ### Requirements 8 | * Python 2.7 9 | * Numpy 10 | * OpenCV 11 | 12 | ### Dataset 13 | [KITTI odometry data set (grayscale, 22 GB)](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) 14 | 15 | ### Usage 16 | Modify the path in test.py to your image sequences and ground truth trajectories, then run 17 | ``` 18 | python test.py 19 | ``` 20 | 21 | ### References 22 | 1. [一个简单的视觉里程计实现 | 冯兵的博客](http://fengbing.net/2015/07/26/%E4%B8%80%E4%B8%AA%E7%AE%80%E5%8D%95%E7%9A%84%E8%A7%86%E8%A7%89%E9%87%8C%E7%A8%8B%E8%AE%A1%E5%AE%9E%E7%8E%B01/ )
23 | 2. [Monocular Visual Odometry using OpenCV](http://avisingh599.github.io/vision/monocular-vo/) and its related project report [_Monocular Visual Odometry_](http://avisingh599.github.io/assets/ugp2-report.pdf) | Avi Singh 24 | 25 | Search "cv2.findEssentialMat", "cv2.recoverPose" etc. in github, you'll find more python projects on slam / visual odometry / 3d reconstruction 26 | -------------------------------------------------------------------------------- /src/thirdparty/MonocularVO/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/src/thirdparty/MonocularVO/map.png -------------------------------------------------------------------------------- /src/thirdparty/MonocularVO/test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import sys 4 | from matplotlib import pyplot as plt 5 | 6 | gt_path = sys.argv[1] 7 | image_path = sys.argv[2] 8 | 9 | from visual_odometry import PinholeCamera, VisualOdometry 10 | 11 | 12 | cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) 13 | vo = VisualOdometry(cam, gt_path) 14 | 15 | traj = np.zeros((600,600,3), dtype=np.uint8) 16 | 17 | for img_id in range(4541): 18 | print(img_id) 19 | img = cv2.imread(sys.argv[2]+str(img_id).zfill(6)+'.png', 0) 20 | #print(img) 21 | 22 | vo.update(img, img_id) 23 | img_vis = img.copy() 24 | if img_id>0: 25 | print(vo.feature3d.shape) 26 | feature2d = vo.feature3d[:,0:2] 27 | feature2d[:,0] = feature2d[:,0]*cam.fx/vo.feature3d[:,2]+cam.cx 28 | feature2d[:,1] = feature2d[:,1]*cam.fx/vo.feature3d[:,2]+cam.cy 29 | print(feature2d) 30 | for point2d in feature2d: 31 | cv2.circle(img_vis,(int(point2d[0]),int(point2d[1])),3,(0,255,0),-1) 32 | 33 | 34 | cur_t = vo.cur_t 35 | if(img_id > 2): 36 | x, y, z = cur_t[0], cur_t[1], cur_t[2] 37 | else: 38 | x, y, z = 0., 0., 0. 39 | draw_x, draw_y = int(x)+290, int(z)+90 40 | true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90 41 | 42 | cv2.circle(traj, (draw_x,draw_y), 1, (img_id*255/4540,255-img_id*255/4540,0), 1) 43 | cv2.circle(traj, (true_x,true_y), 1, (0,0,255), 2) 44 | cv2.rectangle(traj, (10, 20), (600, 60), (0,0,0), -1) 45 | text = "Coordinates: x=%2fm y=%2fm z=%2fm"%(x,y,z) 46 | cv2.putText(traj, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8) 47 | 48 | cv2.imshow('Road facing camera', img_vis) 49 | cv2.imshow('Trajectory', traj) 50 | cv2.waitKey(1) 51 | 52 | cv2.imwrite('map.png', traj) 53 | -------------------------------------------------------------------------------- /src/thirdparty/MonocularVO/visual_odometry.py: -------------------------------------------------------------------------------- 1 | # modified from https://github.com/uoip/monoVO-python 2 | 3 | import numpy as np 4 | import cv2 5 | import param 6 | import detector as dt 7 | STAGE_FIRST_FRAME = 0 8 | STAGE_SECOND_FRAME = 1 9 | STAGE_DEFAULT_FRAME = 2 10 | kMinNumFeature = param.minimum_feature_for_tracking 11 | 12 | lk_params = dict(winSize = (21, 21), 13 | #maxLevel = 3, 14 | criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)) 15 | 16 | def feature_check(kp1,kp2): 17 | dis = np.sum(np.abs(kp1-kp2),1) 18 | valid = dis>=1 19 | return kp1[valid,:],kp2[valid,:] 20 | 21 | def featureTracking(image_ref, image_cur, px_ref): 22 | kp2, st, err = cv2.calcOpticalFlowPyrLK(image_ref, image_cur, px_ref, None, **lk_params) #shape: [k,2] [k,1] [k,1] 23 | st = st.reshape(st.shape[0]) 24 | kp1 = px_ref[st == 1] 25 | kp2 = kp2[st == 1] 26 | kp1,kp2 = feature_check(kp1,kp2) 27 | return kp1, kp2 28 | 29 | 30 | class PinholeCamera: 31 | def __init__(self, width, height, fx, fy, cx, cy, 32 | k1=0.0, k2=0.0, p1=0.0, p2=0.0, k3=0.0): 33 | self.width = int(width) 34 | self.height = int(height) 35 | self.fx = fx 36 | self.fy = fy 37 | self.cx = cx 38 | self.cy = cy 39 | self.distortion = (abs(k1) > 0.0000001) 40 | self.d = [k1, k2, p1, p2, k3] 41 | 42 | 43 | class VisualOdometry: 44 | def __init__(self, cam, annotations=None): 45 | self.frame_stage = 0 46 | self.cam = cam 47 | self.new_frame = None 48 | self.last_frame = None 49 | self.cur_R = None 50 | self.cur_t = None 51 | self.px_ref = None 52 | self.px_cur = None 53 | self.px_cur_selected = None 54 | self.px_ref_selected = None 55 | self.focal = cam.fx 56 | self.pp = (cam.cx, cam.cy) 57 | self.camera_matrix = np.eye(3) 58 | self.camera_matrix[0,0] = self.camera_matrix[1,1] = cam.fx 59 | self.camera_matrix[0,2] = cam.cx 60 | self.camera_matrix[1,2] = cam.cy 61 | 62 | self.motion_R = None 63 | self.motion_t = None 64 | self.feature3d= None 65 | #print(self.camera_matrix) 66 | self.trueX, self.trueY, self.trueZ = 0, 0, 0 67 | #self.detector = cv2.FastFeatureDetector_create(threshold=param.fast_threshold, nonmaxSuppression=True) 68 | self.detector = dt.FeatureDetector()#cv2.FastFeatureDetector_create(threshold=param.fast_threshold, nonmaxSuppression=True) 69 | self.annotations = None 70 | if annotations!=None: 71 | with open(annotations) as f: 72 | self.annotations = f.readlines() 73 | 74 | def getAbsoluteScale(self, frame_id): #specialized for KITTI odometry dataset 75 | ss = self.annotations[frame_id-1].strip().split() 76 | x_prev = float(ss[3]) 77 | y_prev = float(ss[7]) 78 | z_prev = float(ss[11]) 79 | ss = self.annotations[frame_id].strip().split() 80 | x = float(ss[3]) 81 | y = float(ss[7]) 82 | z = float(ss[11]) 83 | self.trueX, self.trueY, self.trueZ = x, y, z 84 | return np.sqrt((x - x_prev)*(x - x_prev) + (y - y_prev)*(y - y_prev) + (z - z_prev)*(z - z_prev)) 85 | 86 | def processFirstFrame(self): 87 | self.px_ref = self.detector.detect(self.new_frame) 88 | #self.px_ref = np.array([x.pt for x in self.px_ref], dtype=np.float32) 89 | self.frame_stage = STAGE_SECOND_FRAME 90 | #return True 91 | 92 | def processSecondFrame(self): 93 | self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref) 94 | if self.px_cur.shape[0]<10: 95 | self.px_cur = self.detector.detect(self.new_frame) 96 | #self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32) 97 | self.px_ref = self.px_cur 98 | return False 99 | 100 | E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref,cameraMatrix = self.camera_matrix , method=cv2.RANSAC, prob=0.999, threshold=1.0) 101 | _, self.cur_R, self.cur_t, mask,points_3d = cv2.recoverPose(E, self.px_cur,\ 102 | self.px_ref,cameraMatrix=self.camera_matrix,distanceThresh=100) 103 | mask_bool = np.array(mask>0).reshape(-1) 104 | points_3d_selected = points_3d[:,mask_bool].T 105 | #print(points_3d_selected.shape) 106 | points_3d_selected[:,0] = points_3d_selected[:,0]/points_3d_selected[:,3] 107 | points_3d_selected[:,1] = points_3d_selected[:,1]/points_3d_selected[:,3] 108 | points_3d_selected[:,2] = points_3d_selected[:,2]/points_3d_selected[:,3] 109 | self.motion_R = self.cur_R 110 | self.motion_t = self.cur_t 111 | self.feature3d= points_3d_selected[:,0:3] 112 | self.px_cur_selected = self.px_cur[mask_bool,:] 113 | self.px_ref_selected = self.px_ref[mask_bool,:] 114 | #print(points_3d_selected) 115 | 116 | E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0) 117 | self.frame_stage = STAGE_DEFAULT_FRAME 118 | self.px_ref = self.px_cur 119 | return True 120 | def processFrame(self, frame_id): 121 | self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref) 122 | #E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0) 123 | # R,t from ref frame to cur frame 124 | if self.px_cur.shape[0]<10: 125 | self.px_cur = self.detector.detect(self.new_frame) 126 | #self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32) 127 | self.px_ref = self.px_cur 128 | return False 129 | E, mask_e = cv2.findEssentialMat(self.px_cur, self.px_ref,cameraMatrix = self.camera_matrix , method=cv2.RANSAC, 130 | prob=0.999, threshold=0.5) 131 | #_, R, t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp) 132 | _, R, t, mask,points_3d = cv2.recoverPose(E, self.px_cur,\ 133 | self.px_ref,cameraMatrix=self.camera_matrix,distanceThresh=100) 134 | mask_bool = np.array(mask>0).reshape(-1) 135 | mask_e_bool = np.array(mask_e>0).reshape(-1) 136 | mask_bool = mask_bool & mask_e_bool 137 | points_3d_selected = points_3d[:,mask_bool].T 138 | ## calculate reprojection error 139 | self.px_cur_selected = self.px_cur[mask_bool,:] 140 | self.px_ref_selected = self.px_ref[mask_bool,:] 141 | #print(points_3d_selected.shape) 142 | points_3d_selected[:,0] = points_3d_selected[:,0]/points_3d_selected[:,3] 143 | points_3d_selected[:,1] = points_3d_selected[:,1]/points_3d_selected[:,3] 144 | points_3d_selected[:,2] = points_3d_selected[:,2]/points_3d_selected[:,3] 145 | self.motion_R = R 146 | self.motion_t = t 147 | self.feature3d= points_3d_selected[:,0:3] 148 | if(self.px_ref.shape[0] < kMinNumFeature): 149 | self.px_cur = self.detector.detect(self.new_frame) 150 | #self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32) 151 | print('detection again',self.px_ref.shape[0],kMinNumFeature,self.px_cur.shape[0]) 152 | self.px_ref = self.px_cur 153 | return True 154 | def get_current_state(self,scale): 155 | self.cur_t = self.cur_t + scale*self.cur_R.dot(self.motion_t) 156 | self.cur_R = self.cur_R.dot(self.motion_R) 157 | return self.cur_R,self.cur_t 158 | def visualize(self,img): 159 | point=(int(img.shape[1]/2+self.cur_t[0,0]),int(img.shape[0]/2-self.cur_t[2,0])) 160 | cv2.circle(img,point,1,(0,255,0),-1) 161 | def update(self, img, frame_id): 162 | assert(img.ndim==2 and img.shape[0]==self.cam.height and img.shape[1]==self.cam.width), "Frame: provided image has not the same size as the camera model or image is not grayscale" 163 | self.new_frame = img 164 | if(self.frame_stage == STAGE_DEFAULT_FRAME): 165 | flag= self.processFrame(frame_id) 166 | elif(self.frame_stage == STAGE_SECOND_FRAME): 167 | flag= self.processSecondFrame() 168 | elif(self.frame_stage == STAGE_FIRST_FRAME): 169 | flag= self.processFirstFrame() 170 | self.last_frame = self.new_frame 171 | return flag 172 | -------------------------------------------------------------------------------- /src/thirdparty/Ransac/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | -------------------------------------------------------------------------------- /src/thirdparty/Ransac/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2013 Falcon Dai 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /src/thirdparty/Ransac/README.md: -------------------------------------------------------------------------------- 1 | py-ransac 2 | ========= 3 | 4 | python implemetation of RANSAC algorithm with a line fitting example and a plane fitting example. 5 | -------------------------------------------------------------------------------- /src/thirdparty/Ransac/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TimingSpace/MVOScaleRecovery/d147bea2dbc1a9c016b36222926d3397152ca9ae/src/thirdparty/Ransac/__init__.py -------------------------------------------------------------------------------- /src/thirdparty/Ransac/line_fitting.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from ransac import * 3 | 4 | def augment(xys): 5 | axy = np.ones((len(xys), 3)) 6 | axy[:, :2] = xys 7 | return axy 8 | 9 | def estimate(xys): 10 | axy = augment(xys[:2]) 11 | return np.linalg.svd(axy)[-1][-1, :] 12 | 13 | def is_inlier(coeffs, xy, threshold): 14 | return np.abs(coeffs.dot(augment([xy]).T)) < threshold 15 | 16 | if __name__ == '__main__': 17 | import matplotlib 18 | import matplotlib.pyplot as plt 19 | 20 | n = 100 21 | max_iterations = 100 22 | goal_inliers = n * 0.3 23 | 24 | # test data 25 | xys = np.random.random((n, 2)) * 10 26 | xys[:50, 1:] = xys[:50, :1] 27 | 28 | plt.scatter(xys.T[0], xys.T[1]) 29 | 30 | # RANSAC 31 | m, b = run_ransac(xys, estimate, lambda x, y: is_inlier(x, y, 0.01), goal_inliers, max_iterations, 20) 32 | a, b, c = m 33 | plt.plot([0, 10], [-c/b, -(c+10*a)/b], color=(0, 1, 0)) 34 | 35 | plt.show() 36 | -------------------------------------------------------------------------------- /src/thirdparty/Ransac/plane_fitting.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from ransac import * 3 | 4 | def augment(xyzs): 5 | axyz = np.ones((len(xyzs), 4)) 6 | axyz[:, :3] = xyzs 7 | return axyz 8 | 9 | def estimate(xyzs): 10 | axyz = augment(xyzs[:3]) 11 | return np.linalg.svd(axyz)[-1][-1, :] 12 | 13 | def is_inlier(coeffs, xyz, threshold): 14 | return np.abs(coeffs.dot(augment([xyz]).T)) < threshold 15 | 16 | if __name__ == '__main__': 17 | import matplotlib 18 | import matplotlib.pyplot as plt 19 | from mpl_toolkits import mplot3d 20 | fig = plt.figure() 21 | ax = mplot3d.Axes3D(fig) 22 | 23 | def plot_plane(a, b, c, d): 24 | xx, yy = np.mgrid[:10, :10] 25 | return xx, yy, (-d - a * xx - b * yy) / c 26 | 27 | n = 100 28 | max_iterations = 100 29 | goal_inliers = n * 0.3 30 | 31 | # test data 32 | xyzs = np.random.random((n, 3)) * 10 33 | xyzs[:50, 2:] = xyzs[:50, :1] 34 | 35 | ax.scatter3D(xyzs.T[0], xyzs.T[1], xyzs.T[2]) 36 | 37 | # RANSAC 38 | m, b = run_ransac(xyzs, estimate, lambda x, y: is_inlier(x, y, 0.01), 3, goal_inliers, max_iterations) 39 | a, b, c, d = m 40 | xx, yy, zz = plot_plane(a, b, c, d) 41 | ax.plot_surface(xx, yy, zz, color=(0, 1, 0, 0.5)) 42 | 43 | plt.show() 44 | -------------------------------------------------------------------------------- /src/thirdparty/Ransac/ransac.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | def run_ransac(data, estimate, is_inlier, sample_size, goal_inliers, max_iterations, stop_at_goal=True, random_seed=None): 4 | best_ic = 0 5 | best_model = None 6 | random.seed(random_seed) 7 | # random.sample cannot deal with "data" being a numpy array 8 | data = list(data) 9 | for i in range(max_iterations): 10 | s = random.sample(data, int(sample_size)) 11 | m = estimate(s) 12 | ic = 0 13 | for j in range(len(data)): 14 | if is_inlier(m, data[j]): 15 | ic += 1 16 | 17 | 18 | if ic > best_ic: 19 | best_ic = ic 20 | best_model = m 21 | if ic > goal_inliers and stop_at_goal: 22 | break 23 | return best_model, best_ic 24 | -------------------------------------------------------------------------------- /src/triangle_batch.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from scipy.spatial import Delaunay 4 | import cv2 5 | import math 6 | import sys 7 | 8 | frame_count = 1 9 | 10 | image_name_list_file = sys.argv[1] 11 | feature_pos_path = sys.argv[2] 12 | image_name_list = open(image_name_list_file) 13 | image_name = image_name_list.readline() 14 | while frame_count<4541: 15 | image_name = image_name_list.readline() 16 | image_name=image_name[:-1] 17 | img = cv2.imread(image_name) 18 | feature_pos_name = feature_pos_path+"/"+str(frame_count)+".txt" 19 | points3d = np.loadtxt(feature_pos_path+str(frame_count)+".txt") 20 | camera_focus=718.856 21 | camera_cx=607.1928 22 | camera_cy=182.2157 23 | points = points3d[:,0:2] 24 | tri = Delaunay(points) 25 | triangle_ids = tri.simplices 26 | b_matrix = np.matrix(np.ones((3,1),np.float)) 27 | data = [] 28 | figure = plt.figure() 29 | for triangle_id in triangle_ids: 30 | point_selected = points3d[triangle_id] 31 | a_array = np.array(point_selected) 32 | a_array[:,0] = a_array[:,2]*(a_array[:,0]-camera_cx)/camera_focus 33 | a_array[:,1] = a_array[:,2]*(a_array[:,1]-camera_cy)/camera_focus 34 | a_matrix = np.matrix(a_array) 35 | #print a_matrix 36 | a_matrix_inv = a_matrix.I 37 | norm = a_matrix_inv*b_matrix 38 | norm_norm_2 = norm.T*norm 39 | pitch = math.asin(norm[1,0]/math.sqrt(norm_norm_2[0,0])) 40 | height = np.mean(a_array[:,1]) 41 | pitch_deg = abs(pitch)*180/3.1415926 42 | #print norm[1,0]/math.sqrt(norm_norm_2[0,0]),pitch_deg,height 43 | pitch_height = [norm[1,0]/math.sqrt(norm_norm_2[0,0]),pitch_deg,height] 44 | data.append(pitch_height) 45 | if(pitch_deg>80 and pitch_deg<100): 46 | if(height>0): 47 | polygon = plt.Polygon(points[triangle_id],fill='g',color='g') 48 | else: 49 | polygon = plt.Polygon(points[triangle_id],fill='r',color='r') 50 | else: 51 | polygon = plt.Polygon(points[triangle_id],fill=None,color='b') 52 | plt.gca().add_patch(polygon) 53 | data = np.array(data) 54 | data = data[data[:,0]>0.98] 55 | data = data[data[:,2]>0] 56 | 57 | mean = np.mean(data[:,2]) 58 | std = np.std(data[:,2]) 59 | 60 | data = data[data[:,2]>mean - 3*std] 61 | data = data[data[:,2] result_log/log_program_$1_$2_$num 5 | echo 'evaluation vo' 6 | python script/evaluate_vo.py dataset/kitti_gt/$2.txt evaluate_result/kitti_$2_base_01_path.txt$1$2$num >result_log/log_vo_$1_$2_$num 7 | #echo 'evaluation scale' 8 | #python script/evaluate_scale.py dataset/kitti_gt/$2_speed.txt evaluate_result/kitti_$2_base_01_scales.txt$1$2$num>result_log/log_scale_$1_$2_$num 9 | 10 | python script/change_scale.py dataset/kitti_gt/$2.txt evaluate_result/kitti_$2_base_01_scales.txt$1$2$num 11 | echo 'evaluation gt+vo_scale' 12 | python script/evaluate_vo.py dataset/kitti_gt/$2.txt new_pose.txt$1$2$num >result_log/log_vo_s_$1_$2_$num 13 | done 14 | cat result_log/log_vo_$1_$2_[0123456789] > result_log/log_vo_$1_$2_all 15 | cat result_log/log_vo_s_$1_$2_[0123456789] > result_log/log_vo_s_$1_$2_all 16 | #cat result_log/log_scale_$1_$2_[0123456789] > result_log/log_scale_$1_$2_all 17 | -------------------------------------------------------------------------------- /test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | for num in 0 1 3 | do 4 | python src/main.py dataset/kitti_image_$2.txt $1$num > result/log_program_$1_$2_$num & 5 | done 6 | wait 7 | #!/bin/bash 8 | for num in 0 1 9 | do 10 | echo 'evaluation vo' $1 $2 $num 11 | python script/evaluate_vo.py dataset/kitti_gt/$2.txt result/kitti_image_$2_path.txt$1$num > result/log_vo_$1_$2_$num 12 | cat result/log_vo_$1_$2_$num 13 | done 14 | 15 | 16 | -------------------------------------------------------------------------------- /test_off_line.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #python src/main.py dataset/kitti_image_$2.txt .mvo > log_result/log_motion_.mvo_$2 3 | echo "motion estimation done" 4 | for num in 0 1 2 3 4 5 6 7 8 9 5 | do 6 | echo 'scale recovry' $1 $2 $num 7 | #python src/main_offline.py result/kitti_image_$2_result.npy.mvo.npy $1$num>log_result/log_rescale_$1_$2_$num & 8 | done 9 | wait 10 | echo "rescale done" 11 | for num in 0 1 2 3 4 5 6 7 8 9 12 | do 13 | echo 'evaluation vo' $1 $2 $num 14 | python script/evaluate_vo.py dataset/kitti_gt/$2.txt evaluate_result/kitti_image_$2_result_path.txt$1$num >log_result/log_vo_$1_$2_$num 15 | cat log_result/log_vo_$1_$2_$num 16 | done 17 | 18 | python script/change_scale.py evaluate_result/kitti_image_$2_result_path.txt$1$num dataset/kitti_gt/$2_speed.txt $1$num 19 | python script/evaluate_vo.py dataset/kitti_gt/$2.txt evaluate_result/kitti_image_$2_result_path_gt.txt$1$num>log_result/log_vo_gt_$1_$2_$num 20 | cat log_result/log_vo_$1_$2_? > log_result/log_vo_$1_$2_all 21 | cat log_result/log_vo_gt_$1_$2_$num 22 | python script/calculate_mean.py log_result/log_vo_$1_$2_all 23 | 24 | --------------------------------------------------------------------------------