Ubicoders - Robotics and AI, Coding, and Software Development!

KITTI How to Start Robotics Navigation for FREE

Join the Mainstream Robotics Community

Picture of the author
Elliot Lee - Jan.5

read_gt.py

python

Line #:

import numpy as np import matplotlib.pyplot as plt # File path for KITTI ground truth pose and timestamp data pose_path = "E:/KITTI/odom/dataset/poses" ts_path = "E:/KITTI/odom/dataset/sequences" gt_file = f"{pose_path}/00.txt" ts_file = f"{ts_path}/00/times.txt" # Load the ground truth and timestamp data gt_data = np.loadtxt(gt_file, delimiter=" ") # N x 12 matrix ts_data = np.loadtxt(ts_file) print(f"Ground truth data shape: {gt_data.shape}") print(f"Timestamp data shape: {ts_data.shape}") # Extract translation components (x, y, z) from ground truth data gt_data = np.reshape(gt_data, (gt_data.shape[0], 3, 4)) x = gt_data[:, 0, 3] y = gt_data[:, 1, 3] z = gt_data[:, 2, 3] # Plot bird's eye view (x vs y) plt.figure(figsize=(10, 6)) plt.plot(x, z, label="Ground Truth Path", color="blue") plt.title("Ground Truth Bird's Eye View (x vs y)") plt.xlabel("x (meters)") plt.ylabel("y (meters)") plt.legend() plt.axis("equal") plt.grid() # Plot x, y, z vs time plt.figure(figsize=(10, 6)) plt.plot(ts_data, x, label="x", color="red") plt.plot(ts_data, y, label="y", color="green") plt.plot(ts_data, z, label="z", color="blue") plt.title("Ground Truth x, y, z vs Time") plt.xlabel("Time (seconds)") plt.ylabel("Position (meters)") plt.legend() plt.grid() plt.show()

read_calib.py

python

Line #:

import numpy as np # suppress scientific notation np.set_printoptions(suppress=True) # File path for KITTI calibration data path = "E:/KITTI/odom/dataset/sequences/00" calib_file = path + "/calib.txt" # Load the calibration data calib = {} with open(calib_file, 'r') as f: for line in f: key, value = line.split(':', 1) # Convert the remaining values into a numpy array value = np.array([float(x) for x in value.split()]) # Store the 3x4 camera matrix calib[key] = value.reshape(3, 4) # Print the calibration data for key, matrix in calib.items(): print(f"{key}:") print(matrix) # Access specific matrices P0 = calib['P0'] P1 = calib['P1'] P2 = calib['P2'] P3 = calib['P3'] # Print the camera matrices print("\nCamera matrices:") print(f"P0:\n{P0}") print(f"P1:\n{P1}") print(f"P2:\n{P2}") print(f"P3:\n{P3}")

read_image.py

python

Line #:

import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import cv2 # File path for KITTI color images path = "E:/KITTI/odom/dataset/sequences/00" left_image = path + "/image_2/000123.png" right_image = path + "/image_3/000123.png" # Load the image left = cv2.imread(left_image) right = cv2.imread(right_image) cv2.imshow('Left Image', left) cv2.imshow('Right Image', right) cv2.waitKey(0)

read_lidar.py

python

Line #:

import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # File path for KITTI LiDAR point cloud data path = "E:/KITTI/odom/dataset/sequences/00/velodyne" file = path + "/000123.bin" # Load the binary file and reshape to points points = np.fromfile(file, dtype=np.float32).reshape(-1, 4) # Print the shape and first 10 points print(f"Shape of point cloud: {points.shape}") print("First 10 points:") print(points[0:10]) #============================================================================== # Reduce the number of points to plot (e.g., 10%) sample_fraction =1 indices = np.random.choice(points.shape[0], int(points.shape[0] * sample_fraction), replace=False) points_sampled = points[indices] # Extract x, y, z, and intensity x = points_sampled[:, 0] y = points_sampled[:, 1] z = points_sampled[:, 2] intensity = points_sampled[:, 3] # Filter points to focus on a region (e.g., front of the vehicle, 0 <= x <= 50 meters) mask = (x >= 0) & (x <= 50) # Adjust x range as needed x, y, z, intensity = x[mask], y[mask], z[mask], intensity[mask] # Calculate distance from the LiDAR sensor distance = np.sqrt(x**2 + y**2 + z**2) # Normalize intensity and distance for visualization intensity_norm = (intensity - intensity.min()) / (intensity.max() - intensity.min()) distance_norm = (distance - distance.min()) / (distance.max() - distance.min()) # Create a 3D scatter plot fig = plt.figure(figsize=(10, 8)) ax = fig.add_subplot(111, projection='3d') # Scatter plot: color by intensity, size by distance scatter = ax.scatter( x, y, z, c=intensity_norm, # Color map from intensity s=distance_norm * 10 + 1, # Scale point size by distance cmap='viridis', # Colormap alpha=0.8 ) # Add color bar to show intensity scale cbar = plt.colorbar(scatter, ax=ax, shrink=0.5) cbar.set_label('Intensity (normalized)') # Set axis limits to equalize scale max_range = max(x.max() - x.min(), y.max() - y.min(), z.max() - z.min()) / 2.0 mid_x = (x.max() + x.min()) / 2.0 mid_y = (y.max() + y.min()) / 2.0 mid_z = (z.max() + z.min()) / 2.0 ax.set_xlim(mid_x - max_range, mid_x + max_range) ax.set_ylim(mid_y - max_range, mid_y + max_range) ax.set_zlim(mid_z - max_range, mid_z + max_range) # Labels and title ax.set_xlabel('X (meters)') ax.set_ylabel('Y (meters)') ax.set_zlabel('Z (meters)') ax.set_title('LiDAR Point Cloud Visualization') plt.show()
Loading...