KITTI How to Start Robotics Navigation for FREE
Join the Mainstream Robotics Community
Elliot Lee - Jan.5
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...