kitti_odometry module

class kitti_odometry.KittiEvalOdom

Bases: object

Evaluate odometry result Usage example:

vo_eval = KittiEvalOdom() vo_eval.eval(gt_pose_txt_dir, result_pose_txt_dir)

calc_sequence_errors(poses_gt, poses_result)

calculate sequence error :param poses_gt: {idx: 4x4 array}, ground truth poses :type poses_gt: dict :param poses_result: {idx: 4x4 array}, predicted poses :type poses_result: dict

Returns

[first_frame, rotation error, translation error, length, speed]

  • first_frame: frist frame index

  • rotation error: rotation error per length

  • translation error: translation error per length

  • length: evaluation trajectory length

  • speed: car speed (#FIXME: 10FPS is assumed)

Return type

err (list list)

compute_ATE(gt, pred)

Compute RMSE of ATE :param gt: ground-truth poses :type gt: 4x4 array dict :param pred: predicted poses :type pred: 4x4 array dict

compute_RPE(gt, pred)

Compute RPE :param gt: ground-truth poses :type gt: 4x4 array dict :param pred: predicted poses :type pred: 4x4 array dict

Returns

rpe_trans rpe_rot

compute_overall_err(seq_err)

Compute average translation & rotation errors :param seq_err: [[r_err, t_err],[r_err, t_err],…]

  • r_err (float): rotation error

  • t_err (float): translation error

Returns

average translation error ave_r_err (float): average rotation error

Return type

ave_t_err (float)

compute_segment_error(seq_errs)

This function calculates average errors for different segment. :param seq_errs: list of errs; [first_frame, rotation error, translation error, length, speed]

  • first_frame: frist frame index

  • rotation error: rotation error per length

  • translation error: translation error per length

  • length: evaluation trajectory length

  • speed: car speed (#FIXME: 10FPS is assumed)

Returns

{100:[avg_t_err, avg_r_err],…}

Return type

avg_segment_errs (dict)

eval(args)

Evaulate required/available sequences :param gt_dir: ground truth poses txt files path :type gt_dir: str :param result_dir: pose predictions txt files directory :type result_dir: str :param alignment: if not None, optimize poses by

  • scale: optimize scale factor for trajectory alignment and evaluation

  • scale_7dof: optimize 7dof for alignment and use scale for trajectory evaluation

  • 7dof: optimize 7dof for alignment and evaluation

  • 6dof: optimize 6dof for alignment and evaluation

last_frame_from_segment_length(dist, first_frame, length)

Find frame (index) that away from the first_frame with the required distance :param dist: distance of each pose w.r.t frame-0 :type dist: float list :param first_frame: start-frame index :type first_frame: int :param length: required distance :type length: float

Returns

end-frame index. if not found return -1

Return type

i (int) / -1

load_poses_from_txt(file_name)

Load poses from txt (KITTI format) Each line in the file should follow one of the following structures

  1. idx pose(3x4 matrix in terms of 12 numbers)

  2. pose(3x4 matrix in terms of 12 numbers)

Parameters

file_name (str) – txt file path

Returns

{idx: 4x4 array}

Return type

poses (dict)

plot_error(avg_segment_errs, file_name)

Plot per-length error :param avg_segment_errs: {100:[avg_t_err, avg_r_err],…} :type avg_segment_errs: dict :param file_name: the results file named. :type file_name: str

plot_trajectory(poses_gt, poses_result, file_name)

Plot trajectory for both GT and prediction :param poses_gt: {idx: 4x4 array}; ground truth poses :type poses_gt: dict :param poses_result: {idx: 4x4 array}; predicted poses :type poses_result: dict :param file_name: the results file named. :type file_name: str

rotation_error(pose_error)

Compute rotation error :param pose_error: relative pose error :type pose_error: 4x4 array

Returns

rotation error

Return type

rot_error (float)

save_sequence_errors(err, file_name)

Save sequence error :param err: error information :type err: list list :param file_name: txt file for writing errors :type file_name: str

scale_optimization(gt, pred)

Optimize scaling factor :param gt: ground-truth poses :type gt: 4x4 array dict :param pred: predicted poses :type pred: 4x4 array dict

Returns

predicted poses after optimization

Return type

new_pred (4x4 array dict)

trajectory_distances(poses)

Compute distance for each pose w.r.t frame-0 :param poses: {idx: 4x4 array} :type poses: dict

Returns

distance of each pose w.r.t frame-0

Return type

dist (float list)

translation_error(pose_error)

Compute translation error :param pose_error: relative pose error :type pose_error: 4x4 array

Returns

translation error

Return type

trans_error (float)

write_result(f, seq, errs)

Write result into a txt file :param f: :type f: IOWrapper :param seq: sequence number :type seq: int :param errs: [ave_t_err, ave_r_err, ate, rpe_trans, rpe_rot] :type errs: list

kitti_odometry.scale_lse_solver(X, Y)

Least-sqaure-error solver Compute optimal scaling factor so that s(X)-Y is minimum :param X: current data :type X: KxN array :param Y: reference data :type Y: KxN array

Returns

scaling factor

Return type

scale (float)

kitti_odometry.umeyama_alignment(x, y, with_scale=False)

Computes the least squares solution parameters of an Sim(m) matrix that minimizes the distance between a set of registered points. Umeyama, Shinji: Least-squares estimation of transformation parameters

between two point patterns. IEEE PAMI, 1991

Parameters

  • x – mxn matrix of points, m = dimension, n = nr. of data points

  • y – mxn matrix of points, m = dimension, n = nr. of data points

  • with_scale – set to True to align also the scale (default: 1.0 scale)

Returns

r, t, c - rotation matrix, translation vector and scale factor