├── .gitignore ├── LICENSE ├── README.md ├── assets ├── cover.jpg └── demo.gif ├── odometry └── orbslam2 │ └── 00.txt ├── requirements.txt └── src ├── __init__.py ├── filter.py ├── run_kitti.py └── visualizer.py /.gitignore: -------------------------------------------------------------------------------- 1 | results 2 | __pycache__ -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Jiaxin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, 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, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ground_normal_filter 2 | Official implementation of: Towards Accurate Ground Plane Normal Estimation from Ego-Motion 3 | [Paper](https://www.mdpi.com/1424-8220/22/23/9375) | [Arxiv](https://arxiv.org/abs/2212.04224) 4 | | [Youtube](https://youtu.be/1wCIwQxCGWw) | [Bilibili](https://www.bilibili.com/video/BV1sW4y1M79W/) 5 | 6 |

7 | 8 |

9 | 10 | ## TL;DR 11 | Estimate ground plane normal vector from **ONLY** odometry information 12 |

13 | 14 |

15 | 16 | ## Quick Start 17 | ### 📦 Prepare data 18 | * Download the [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php). 19 | * Prepare odometry results 20 | * One example has already been provided in `odometry/orbslam2/00.txt`. 21 | * You can download full results from [here](https://github.com/Huangying-Zhan/DF-VO#part-4-result-evaluation) 22 | 23 | ### 🏗️️ Setup 24 | ```bash 25 | python3 -m pip install -r requirements.txt 26 | sudo apt install ffmpeg # for video visualization 27 | ``` 28 | 29 | ### 🏃 Run the example 30 | ```bash 31 | python3 src/run_kitti.py \ 32 | --sequence 00 \ 33 | --kitti_root /path/to/kitti/odometry/sequences \ 34 | --pose_root odometry/orbslam2 \ 35 | --output_root results 36 | ffmpeg -y -framerate 10 -pattern_type glob -i 'results/vis/*.jpg' results/video.mp4 37 | ``` 38 | 39 | 40 | ## Citation 41 | If you find our work useful, please consider citing our paper: 42 | ``` 43 | @article{zhang2022towards, 44 | title={Towards Accurate Ground Plane Normal Estimation from Ego-Motion}, 45 | author={Zhang, Jiaxin and Sui, Wei and Zhang, Qian and Chen, Tao and Yang, Cong}, 46 | journal={Sensors}, 47 | volume={22}, 48 | number={23}, 49 | pages={9375}, 50 | year={2022}, 51 | publisher={Multidisciplinary Digital Publishing Institute} 52 | } 53 | ``` 54 | -------------------------------------------------------------------------------- /assets/cover.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/manymuch/ground_normal_filter/091599fdd0daa085bead6aa222b0b464baab5e38/assets/cover.jpg -------------------------------------------------------------------------------- /assets/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/manymuch/ground_normal_filter/091599fdd0daa085bead6aa222b0b464baab5e38/assets/demo.gif -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | matplotlib 2 | opencv-python 3 | numpy 4 | tqdm 5 | scipy 6 | inekf -------------------------------------------------------------------------------- /src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/manymuch/ground_normal_filter/091599fdd0daa085bead6aa222b0b464baab5e38/src/__init__.py -------------------------------------------------------------------------------- /src/filter.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import inekf 3 | 4 | 5 | class So3Process(inekf.ProcessModel[inekf.SO3, "Vec3"]): 6 | def __init__(self, Q): 7 | super().__init__() 8 | self.Q = Q 9 | 10 | def f(self, u, dt, state): 11 | return state 12 | 13 | def makePhi(self, u, dt, state, error): 14 | return np.eye(3, dtype=float) 15 | 16 | 17 | class GroundNormalFilterIEKF: 18 | def __init__(self): 19 | cov_init = np.eye(3, dtype=np.float32) 20 | process_cov = np.eye(3, dtype=np.float32) * 1e-2 21 | b = np.asarray([0., 0., 1.]) 22 | measure_cov = np.eye(3) 23 | measure = inekf.MeasureModel[inekf.SO3](b, measure_cov, inekf.ERROR.LEFT) 24 | process = So3Process(process_cov) 25 | x0 = inekf.SO3(0, 0, 0, cov_init) 26 | iekf = inekf.InEKF(process, x0, inekf.ERROR.LEFT) 27 | iekf.addMeasureModel("measure", measure) 28 | self.iekf = iekf 29 | self.u = inekf.SO3(0., 0., 0.).log 30 | self.cumulative_rotation = np.eye(3, dtype=np.float32) 31 | 32 | def update(self, relative_se3): 33 | relative_so3 = relative_se3[:3, :3] 34 | self.cumulative_rotation = self.cumulative_rotation @ relative_so3 35 | state = self.iekf.predict(self.u) 36 | predict_rotation = state.mat.copy() 37 | measure_vector = self.cumulative_rotation[:, 2] 38 | self.iekf.update("measure", measure_vector) 39 | compensation_se3 = np.eye(4, dtype=np.float32) 40 | compensation_so3 = predict_rotation.T @ self.cumulative_rotation 41 | compensation_se3[:3, :3] = compensation_so3 42 | return compensation_se3 43 | -------------------------------------------------------------------------------- /src/run_kitti.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | from os.path import join, isdir 4 | from os import listdir, mkdir 5 | from tqdm import tqdm 6 | from filter import GroundNormalFilterIEKF 7 | from visualizer import Visualization 8 | import argparse 9 | 10 | 11 | def invT(transform): 12 | """inverse a transform matrix without using np.linalg.inv 13 | 14 | Args: 15 | transform (ndarray): input transform matrix with shape=(4,4) 16 | 17 | Returns: 18 | ndarray: output transform matrix with shape=(4,4) 19 | """ 20 | R_Transposed = transform[:3, :3].T 21 | result = np.eye(4) 22 | result[:3, :3] = R_Transposed 23 | result[:3, 3] = -R_Transposed @ transform[:3, 3] 24 | return result 25 | 26 | 27 | def read_kitti_calib(calib_path): 28 | """Read kitti calibration file and get camera intrinsic matrix 29 | 30 | Args: 31 | calib_path (string): path to calibration file (xxx/calib.txt) 32 | 33 | Returns: 34 | ndarray: camera intrinsic matrix with shape=(3,3) 35 | """ 36 | with open(calib_path, "r") as f: 37 | lines = f.readlines() 38 | p2 = lines[2].split()[1:] 39 | p2 = np.array(p2, dtype=np.float32).reshape(3, 4) 40 | return p2[:, :3] 41 | 42 | 43 | def read_kitti_pose(pose_path): 44 | """Read kitti pose file and get relative transform 45 | 46 | Args: 47 | pose_path (string): path to pose file 48 | 49 | Returns: 50 | ndarray: relative transforms with shape=(N,4,4) 51 | """ 52 | input_pose = np.loadtxt(pose_path) 53 | assert (input_pose.shape[1] == 13) 54 | image_ids = input_pose[:, 0].astype(np.int32) 55 | input_pose = input_pose[:, 1:] 56 | length = input_pose.shape[0] 57 | input_pose = input_pose.reshape(-1, 3, 4) 58 | bottom = np.zeros((length, 1, 4)) 59 | bottom[:, :, -1] = 1 60 | absolute_transform = np.concatenate((input_pose, bottom), axis=1) 61 | relative_transforms = [] 62 | for idx in range(length - 1): 63 | relative_transform = invT( 64 | absolute_transform[idx + 1]) @ absolute_transform[idx] 65 | relative_transforms.append(relative_transform) 66 | return image_ids, relative_transforms 67 | 68 | 69 | if __name__ == "__main__": 70 | parser = argparse.ArgumentParser() 71 | parser.add_argument("--sequence", type=str, default="00") 72 | parser.add_argument("--kitti_root", type=str, default="KITTI_odom/sequences") 73 | parser.add_argument("--pose_root", type=str, default="odometry/orbslam2") 74 | parser.add_argument("--output_root", type=str, default="results") 75 | args = parser.parse_args() 76 | 77 | # create output dir 78 | if not isdir(args.output_root): 79 | mkdir(args.output_root) 80 | vis_dir = join(args.output_root, "vis") 81 | if not isdir(vis_dir): 82 | mkdir(vis_dir) 83 | 84 | # read poses 85 | sequence = args.sequence 86 | pose_file = join(args.pose_root, f"{sequence}.txt") 87 | image_ids, relative_transforms = read_kitti_pose(pose_file) 88 | 89 | # prepare image list 90 | image_dir = join(args.kitti_root, sequence, "image_2") 91 | image_list = listdir(image_dir) 92 | image_list.sort() 93 | 94 | # remove last image 95 | image_ids = image_ids[:-1] 96 | # image_list start with 000000.png while image_ids start with 1 97 | image_list = [image_list[i+1] for i in image_ids] 98 | 99 | # read calibration 100 | calib_path = join(args.kitti_root, sequence, "calib.txt") 101 | camera_K = read_kitti_calib(calib_path) 102 | 103 | # run 104 | gnf = GroundNormalFilterIEKF() 105 | vis = Visualization(K=camera_K, d=None, input_wh=(1241, 376)) 106 | for idx, image_file in enumerate(tqdm(image_list)): 107 | image_path = join(image_dir, image_file) 108 | relative_so3 = relative_transforms[idx][:3, :3] 109 | compensation_se3 = gnf.update(relative_so3) 110 | compensation_so3 = compensation_se3[:3, :3] 111 | image = cv2.imread(image_path) 112 | combined_image = vis.get_frame(image, compensation_so3) 113 | output_path = join(vis_dir, f"{idx:06d}.jpg") 114 | cv2.imwrite(output_path, combined_image) 115 | -------------------------------------------------------------------------------- /src/visualizer.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from scipy.spatial.transform import Rotation as R 4 | import cv2 5 | import io 6 | 7 | 8 | def get_img_from_fig(fig, dpi=100): 9 | buf = io.BytesIO() 10 | fig.savefig(buf, format="png", dpi=dpi) 11 | buf.seek(0) 12 | img_arr = np.frombuffer(buf.getvalue(), dtype=np.uint8) 13 | buf.close() 14 | img = cv2.imdecode(img_arr, 1) 15 | return img 16 | 17 | 18 | class Visualization(object): 19 | def __init__(self, K, d, input_wh): 20 | self.bev_img_h = 480 21 | self.bev_img_w = 480 22 | 23 | ipm_height_max_meter = 52 24 | ipm_height_min_meter = 10 25 | ipm_width_meter = 8 26 | self.camera_height = 2 27 | self.ipm_region = np.float32([[-ipm_width_meter, ipm_height_max_meter], 28 | [ipm_width_meter, ipm_height_max_meter], 29 | [-ipm_width_meter, 30 | (ipm_height_max_meter+ipm_height_min_meter)/2], 31 | [ipm_width_meter, (ipm_height_max_meter+ipm_height_min_meter)/2]]) 32 | self.H_target_pts = np.float32([[0, 0], 33 | [self.bev_img_w-1, 0], 34 | [0, self.bev_img_h/2], 35 | [self.bev_img_w-1, self.bev_img_h/2]]) 36 | 37 | self.front2vcs = np.asarray([[0, -1, 0, 0], 38 | [1, 0, 0, -20], 39 | [0, 0, 1, 0.7], 40 | [0, 0, 0, 1]], dtype=np.float32) 41 | 42 | self.K = K 43 | self.d = d 44 | self.input_wh = input_wh 45 | self.output_wh = (480, 256) 46 | # resize K by input and output shape 47 | self.output_K = self.K.copy() 48 | self.output_K[0, :] *= self.output_wh[0] / self.input_wh[0] 49 | self.output_K[1, :] *= self.output_wh[1] / self.input_wh[1] 50 | 51 | # storage for plots 52 | self.plot_history_length = 50 53 | self.compensation_rotations = [] 54 | 55 | def get_vanishing_line(self, dynamic_R=np.eye(3, dtype=np.float32)): 56 | R_mat = dynamic_R 57 | R_mat = R_mat.astype(np.float32) 58 | t = np.zeros((3, 1), dtype=np.float32) 59 | Rt = np.concatenate((R_mat, t), axis=1) 60 | P = self.output_K @ Rt 61 | p0 = (P[0, 2]/P[2, 2], P[1, 2]/P[2, 2]) 62 | p1 = (0, p0[1]) 63 | p2 = (self.output_wh[0], p0[1]) 64 | return np.asarray(p1).astype(int), np.asarray(p2).astype(int) 65 | 66 | def paint_vanishing_line(self, img, dynamic_R): 67 | paint_img = img.copy() 68 | p1, p2 = self.get_vanishing_line() 69 | cv2.line(paint_img, p1, p2, (0, 0, 255), thickness=1) 70 | p1, p2 = self.get_vanishing_line(dynamic_R) 71 | cv2.line(paint_img, p1, p2, (0, 255, 0), thickness=1) 72 | return paint_img 73 | 74 | def get_bev(self, img, dynamic_R=np.eye(3, dtype=np.float32)): 75 | ground_pts_2d = self.ipm_region.copy() 76 | y_axis = (self.camera_height * np.ones((4, 1), dtype=np.float32)).copy() 77 | ground_pts_vcs_3d = np.concatenate( 78 | (ground_pts_2d[:, :1], y_axis, ground_pts_2d[:, 1:]), axis=1).T 79 | img_plane = self.K @ dynamic_R @ ground_pts_vcs_3d 80 | img_plane /= img_plane[2, :] 81 | H_source_pts = (img_plane[:2, :].T).astype(np.float32) 82 | H = cv2.getPerspectiveTransform(H_source_pts, self.H_target_pts) 83 | ipm_img = cv2.warpPerspective(img, H, (self.bev_img_w, self.bev_img_h)) 84 | return ipm_img 85 | 86 | def get_plot(self): 87 | idx = len(self.compensation_rotations) 88 | start_idx = max(0, idx - self.plot_history_length) 89 | timestamps = np.arange(start_idx, idx, 1) 90 | pitch_list = [] 91 | for j in range(start_idx, idx): 92 | rotation = self.compensation_rotations[j] 93 | pitch = R.from_matrix(rotation).as_euler('zxy', degrees=True)[1] 94 | pitch_list.append(pitch) 95 | fig, ax1 = plt.subplots(figsize=(4.8, 2.56), dpi=100) 96 | axes = plt.gca() 97 | axes.set_ylim([-2, 2]) 98 | ax1.set_xlabel('frame') 99 | ax1.set_ylabel('pitch (degree)') 100 | ax1.plot((timestamps, timestamps), ([0] * len(timestamps), pitch_list), c='black') 101 | ax1.scatter(timestamps, pitch_list, s=10, c='red') 102 | plt.tight_layout() 103 | cv_plot = get_img_from_fig(fig) 104 | plt.close(fig) 105 | return cv_plot 106 | 107 | def get_frame(self, image, dynamic_R): 108 | undistort_image = cv2.undistort(image, self.K, self.d) 109 | resized_image = cv2.resize(undistort_image, self.output_wh) 110 | self.compensation_rotations.append(dynamic_R) 111 | vanilla_bev = self.get_bev(undistort_image) 112 | calib_bev = self.get_bev(undistort_image, dynamic_R) 113 | vanishing_line = self.paint_vanishing_line(resized_image, dynamic_R) 114 | plot = self.get_plot() 115 | top = np.concatenate((vanilla_bev, calib_bev), axis=1) 116 | bottom = np.concatenate((plot, vanishing_line), axis=1) 117 | combined = np.concatenate((top, bottom), axis=0) 118 | return combined 119 | --------------------------------------------------------------------------------