lidar-camera-calibration.py

import numpy as np
import cv2
import open3d as o3d
from scipy.spatial.transform import Rotation
import argparse
import os
 
class LiDARCameraCalibrator:
    def __init__(self, point_cloud_path, image_path, camera_matrix, dist_coeffs):
        """
        Initialize the calibrator with point cloud and image data
       
        Args:
            point_cloud_path (str): Path to LiDAR point cloud file (.pcd, .ply, .bin, etc.)
            image_path (str): Path to camera image
            camera_matrix (np.array): 3x3 camera intrinsic matrix
            dist_coeffs (np.array): Camera distortion coefficients
        """
        # Load point cloud
        self.points_3d = self.load_point_cloud(point_cloud_path)
       
        # Load image
        self.image = cv2.imread(image_path)
        self.original_image = self.image.copy()
        self.height, self.width = self.image.shape[:2]
       
        # Camera parameters
        self.camera_matrix = camera_matrix
        self.dist_coeffs = dist_coeffs
 
        # self.fixed_translation = np.array([0.676, 0.0, 1.486])
        # self.fixed_transform = np.eye(4)
        # self.fixed_transform[:3, 3] = self.fixed_translation
       
        # Initial transformation parameters (adjustable via trackbars)
        # tx=1.932+0.676, ty=0.25+0, tz=1.4+1.486, rx=0, ry=-2.0, rz=1.5
        self.tx = -1.16  # Translation X
        self.ty = -0.23  # Translation Y  
        self.tz = 0.42  # Translation Z
        self.rx = 0.0  # Rotation X (degrees)
        self.ry = 2.0  # Rotation Y (degrees)
        self.rz = -0.55  # Rotation Z (degrees)
       
        # Trackbar ranges (scaled by 1000 for precision)
        self.t_range = 50000  # ±50m in mm
        self.r_range = 18000  # ±180 degrees in 0.01 degree units
       
        # Setup OpenCV window and trackbars
        self.setup_gui()
   
    def load_point_cloud(self, file_path):
        """
        Load point cloud from various formats including .bin files
       
        Args:
            file_path (str): Path to point cloud file
           
        Returns:
            np.array: Nx3 array of 3D points
        """
        file_ext = os.path.splitext(file_path)[1].lower()
       
        if file_ext == '.bin':
            # Load binary point cloud (common format for KITTI, nuScenes, etc.)
            # Assumes format: [x, y, z, intensity] or [x, y, z, intensity, ring, time, etc.]
            points = np.fromfile(file_path, dtype=np.float32)
           
            # Determine the number of fields per point
            # Common formats: 4 (x,y,z,intensity), 5 (x,y,z,intensity,ring), 6+ (additional fields)
            if len(points) % 4 == 0:
                points = points.reshape(-1, 4)
                points_3d = points[:, :3]  # Take only x, y, z
                self.intensities = points[:, 3]  # Store intensity for potential use
                # print(self.intensities.max())
                # print(self.intensities.min())
                print(f"Loaded {len(points_3d)} points from .bin file (4 fields per point)")
            elif len(points) % 6 == 0:
                points = points.reshape(-1, 6)
                points_3d = points[:, :3]  # Take only x, y, z
                self.intensities = points[:, 3]  # Store intensity
                print(f"Loaded {len(points_3d)} points from .bin file (6 fields per point)")
            else:
                # Try to infer the structure or default to 3D points only
                # Assume the most common case where points are stored as [x,y,z,...]
                n_fields = 4  # Default assumption
                if len(points) % 3 == 0 and len(points) % 4 != 0:
                    n_fields = 3
               
                points = points.reshape(-1, n_fields)
                points_3d = points[:, :3]
                if n_fields > 3:
                    self.intensities = points[:, 3]
                else:
                    self.intensities = None
                print(f"Loaded {len(points_3d)} points from .bin file ({n_fields} fields per point)")
           
            return points_3d
           
        else:
            # Use Open3D for standard formats (.pcd, .ply, etc.)
            try:
                pcd = o3d.io.read_point_cloud(file_path)
                points_3d = np.asarray(pcd.points)
                self.intensities = None  # Open3D formats don't always have intensity
                print(f"Loaded {len(points_3d)} points using Open3D")
                return points_3d
            except Exception as e:
                raise ValueError(f"Could not load point cloud {file_path}: {str(e)}")
       
       
    def setup_gui(self):
        """Setup OpenCV window and trackbars for real-time adjustment"""
        cv2.namedWindow('LiDAR-Camera Calibration', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('LiDAR-Camera Calibration', 1200, 800)
       
        # Create trackbars for translation (in mm for precision)
        cv2.createTrackbar('TX (mm)', 'LiDAR-Camera Calibration',
                          int(self.tx * 1000) + self.t_range, 2 * self.t_range, self.update_transform)
        cv2.createTrackbar('TY (mm)', 'LiDAR-Camera Calibration',
                          int(self.ty * 1000) + self.t_range, 2 * self.t_range, self.update_transform)
        cv2.createTrackbar('TZ (mm)', 'LiDAR-Camera Calibration',
                          int(self.tz * 1000) + self.t_range, 2 * self.t_range, self.update_transform)
       
        # Create trackbars for rotation (in 0.01 degree units)
        cv2.createTrackbar('RX (0.01°)', 'LiDAR-Camera Calibration',
                          int(self.rx * 100) + self.r_range, 2 * self.r_range, self.update_transform)
        cv2.createTrackbar('RY (0.01°)', 'LiDAR-Camera Calibration',
                          int(self.ry * 100) + self.r_range, 2 * self.r_range, self.update_transform)
        cv2.createTrackbar('RZ (0.01°)', 'LiDAR-Camera Calibration',
                          int(self.rz * 100) + self.r_range, 2 * self.r_range, self.update_transform)
       
        # Additional controls
        cv2.createTrackbar('Point Size', 'LiDAR-Camera Calibration', 2, 10, self.update_transform)
        cv2.createTrackbar('Max Distance (m)', 'LiDAR-Camera Calibration', 100, 200, self.update_transform)
        cv2.createTrackbar('Min Distance (m)', 'LiDAR-Camera Calibration', 0, 50, self.update_transform)
        cv2.createTrackbar('Intensity Filter', 'LiDAR-Camera Calibration', 0, 100, self.update_transform)
   
    def set_initial_values(self, tx=0, ty=0, tz=0, rx=0, ry=0, rz=0):
        """Set initial transformation values"""
        self.tx, self.ty, self.tz = tx, ty, tz
        self.rx, self.ry, self.rz = rx, ry, rz
       
        # Update trackbars
        cv2.setTrackbarPos('TX (mm)', 'LiDAR-Camera Calibration', int(tx * 1000) + self.t_range)
        cv2.setTrackbarPos('TY (mm)', 'LiDAR-Camera Calibration', int(ty * 1000) + self.t_range)
        cv2.setTrackbarPos('TZ (mm)', 'LiDAR-Camera Calibration', int(tz * 1000) + self.t_range)
        cv2.setTrackbarPos('RX (0.01°)', 'LiDAR-Camera Calibration', int(rx * 100) + self.r_range)
        cv2.setTrackbarPos('RY (0.01°)', 'LiDAR-Camera Calibration', int(ry * 100) + self.r_range)
        cv2.setTrackbarPos('RZ (0.01°)', 'LiDAR-Camera Calibration', int(rz * 100) + self.r_range)
   
    def update_transform(self, val):
        """Callback function for trackbar updates"""
        # Get current trackbar values
        self.tx = (cv2.getTrackbarPos('TX (mm)', 'LiDAR-Camera Calibration') - self.t_range) / 1000.0
        self.ty = (cv2.getTrackbarPos('TY (mm)', 'LiDAR-Camera Calibration') - self.t_range) / 1000.0
        self.tz = (cv2.getTrackbarPos('TZ (mm)', 'LiDAR-Camera Calibration') - self.t_range) / 1000.0
        self.rx = (cv2.getTrackbarPos('RX (0.01°)', 'LiDAR-Camera Calibration') - self.r_range) / 100.0
        self.ry = (cv2.getTrackbarPos('RY (0.01°)', 'LiDAR-Camera Calibration') - self.r_range) / 100.0
        self.rz = (cv2.getTrackbarPos('RZ (0.01°)', 'LiDAR-Camera Calibration') - self.r_range) / 100.0
       
        # Update visualization
        self.visualize_projection()
   
    def get_transformation_matrix(self):
        """Calculate 4x4 transformation matrix from current parameters"""
        # Create rotation matrix from Euler angles (XYZ order)
        rotation = Rotation.from_euler('xyz', [self.rx, self.ry, self.rz], degrees=True)
        # R = rotation.as_matrix()
 
        coord_transform = np.array([
            [0, -1, 0],
            [0, 0, -1],
            [1, 0, 0]
        ])
 
        R = rotation.as_matrix() #@ coord_transform
       
        # Create translation vector
        t = np.array([self.tx, self.ty, self.tz])
       
        # Build 4x4 transformation matrix
        variable_T = np.eye(4)
        variable_T[:3, :3] = R
        variable_T[:3, 3] = t
 
        # T = variable_T @ self.fixed_transform
        T = variable_T
 
        # Convert combined transform to camera coordinate frame
        C4 = np.eye(4)
        C4[:3, :3] = coord_transform   # maps robot -> camera
        T_camera = C4 @ T
       
        return T_camera
   
    def project_points(self):
        """Project 3D LiDAR points to image coordinates"""
        if len(self.points_3d) == 0:
            return np.array([]), np.array([])
       
        # Apply transformation
        T = self.get_transformation_matrix()
        points_homo = np.column_stack([self.points_3d, np.ones(len(self.points_3d))])
        transformed_points = (T @ points_homo.T).T[:, :3]
       
        # Filter points that are in front of camera
        valid_mask = transformed_points[:, 2] > 0
        valid_points = transformed_points[valid_mask]
        valid_inten = self.intensities[valid_mask]
       
        if len(valid_points) == 0:
            return np.array([]), np.array([])
       
        # Filter by distance
        max_dist = cv2.getTrackbarPos('Max Distance (m)', 'LiDAR-Camera Calibration')
        min_dist = cv2.getTrackbarPos('Min Distance (m)', 'LiDAR-Camera Calibration')
        distances = np.linalg.norm(valid_points, axis=1)
        dist_mask = (distances >= min_dist) & (distances <= max_dist)
        valid_points = valid_points[dist_mask]
        valid_inten = valid_inten[dist_mask]
        valid_distances = distances[dist_mask]
       
        # Filter by intensity if available
        if hasattr(self, 'intensities') and self.intensities is not None:
            intensity_threshold = cv2.getTrackbarPos('Intensity Filter', 'LiDAR-Camera Calibration') / 100.0
            # Apply original valid_mask and dist_mask to intensities
            original_valid_mask = np.arange(len(self.points_3d))[valid_mask[:len(valid_mask)]][:len(valid_points)]
            if len(original_valid_mask) > 0 and len(original_valid_mask) <= len(self.intensities):
                valid_intensities = self.intensities[original_valid_mask]
                # Normalize intensities to 0-1 range
                if len(valid_intensities) > 0:
                    intensity_norm = (valid_intensities - np.min(valid_intensities)) / (np.max(valid_intensities) - np.min(valid_intensities) + 1e-8)
                    intensity_mask = intensity_norm >= intensity_threshold
                    valid_points = valid_points[intensity_mask]
                    valid_inten = valid_inten[intensity_mask]
                    valid_distances = valid_distances[intensity_mask]
       
        if len(valid_points) == 0:
            return np.array([]), np.array([])
       
        # Project to image coordinates
        rvec = np.zeros(3)  # No additional rotation
        tvec = np.zeros(3)  # No additional translation
       
        image_points, _ = cv2.projectPoints(
            valid_points, rvec, tvec, self.camera_matrix, self.dist_coeffs
        )
       
        image_points = image_points.reshape(-1, 2)
       
        # Filter points within image bounds
        h, w = self.height, self.width
        valid_img_mask = ((image_points[:, 0] >= 0) & (image_points[:, 0] < w) &
                         (image_points[:, 1] >= 0) & (image_points[:, 1] < h))
       
        return image_points[valid_img_mask], valid_distances[valid_img_mask], valid_inten[valid_img_mask]
   
    def visualize_projection(self):
        """Update the visualization with current transformation"""
        # Reset image
        self.image = self.original_image.copy()
       
        # Project points
        projected_points, distances, intensities = self.project_points()
       
        if len(projected_points) == 0:
            cv2.putText(self.image, "No valid points to display", (50, 50),
                       cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
        else:
            # Color points by distance (closer = red, farther = blue)
            max_dist = cv2.getTrackbarPos('Max Distance (m)', 'LiDAR-Camera Calibration')
            normalized_dist = distances.clip(0, 100) / min(max_dist,distances.max())
            normalized_intensity = intensities/255
           
            point_size = cv2.getTrackbarPos('Point Size', 'LiDAR-Camera Calibration')
            if point_size == 0:
                point_size = 1
               
            # for i, (point, dist) in enumerate(zip(projected_points, normalized_dist)):
            for i, (point, dist) in enumerate(zip(projected_points, normalized_intensity)):
                # Color from red (close) to blue (far)
                color = (255 * (1 - dist), 0, 255 * dist)
                cv2.circle(self.image, (int(point[0]), int(point[1])),
                          point_size, color, -1)
       
        # Add parameter information
        info_text = [
            f"TX: {self.tx:.3f}m  TY: {self.ty:.3f}m  TZ: {self.tz:.3f}m",
            f"RX: {self.rx:.2f}°  RY: {self.ry:.2f}°  RZ: {self.rz:.2f}°",
            f"Projected points: {len(projected_points)}",
            "Press 'q' to quit, 's' to save parameters, 'r' to reset"
        ]
       
        for i, text in enumerate(info_text):
            cv2.putText(self.image, text, (10, 30 + i * 25),
                       cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(self.image, text, (10, 30 + i * 25),
                       cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
       
        cv2.imshow('LiDAR-Camera Calibration', self.image)
   
    def save_parameters(self, filename="calibration_params.txt"):
        """Save current transformation parameters to file"""
        T = self.get_transformation_matrix()
       
        with open(filename, 'w') as f:
            f.write("LiDAR-Camera Calibration Parameters\n")
            f.write("=" * 40 + "\n\n")
            f.write(f"Translation (meters):\n")
            f.write(f"  TX: {self.tx:.6f}\n")
            f.write(f"  TY: {self.ty:.6f}\n")
            f.write(f"  TZ: {self.tz:.6f}\n\n")
            f.write(f"Rotation (degrees):\n")
            f.write(f"  RX: {self.rx:.6f}\n")
            f.write(f"  RY: {self.ry:.6f}\n")
            f.write(f"  RZ: {self.rz:.6f}\n\n")
            f.write("4x4 Transformation Matrix:\n")
            f.write(str(T) + "\n")
       
        print(f"Parameters saved to {filename}")
   
    def run(self):
        """Start the interactive calibration tool"""
        print("LiDAR-Camera Calibration Tool")
        print("Use trackbars to adjust transformation parameters")
        print("Controls:")
        print("  'q' - Quit")
        print("  's' - Save current parameters")
        print("  'r' - Reset to initial values")
       
        # Initial visualization
        self.visualize_projection()
       
        while True:
            key = cv2.waitKey(30) & 0xFF
           
            if key == ord('q'):
                break
            elif key == ord('s'):
                self.save_parameters()
            elif key == ord('r'):
                self.set_initial_values()
                self.visualize_projection()
       
        cv2.destroyAllWindows()
 
def create_camera_matrix(focal_length_mm, principal_point_x_pixels, principal_point_y_pixels,
                        pixels_per_mm_x, pixels_per_mm_y):
    """
    Create camera matrix from physical camera parameters.
   
    Args:
        focal_length_mm: Focal length in millimeters
        principal_point_x_pixels: Principal point X coordinate in pixels
        principal_point_y_pixels: Principal point Y coordinate in pixels
        pixels_per_mm_x: Pixels per millimeter in X direction
        pixels_per_mm_y: Pixels per millimeter in Y direction
       
    Returns:
        camera_matrix: 3x3 camera intrinsic matrix
    """
    # Debug: Check inputs
    # print(f"Input values:")
    # print(f"  focal_length_mm: {focal_length_mm}")
    # print(f"  principal_point_x_pixels: {principal_point_x_pixels}")
    # print(f"  principal_point_y_pixels: {principal_point_y_pixels}")
    # print(f"  pixels_per_mm_x: {pixels_per_mm_x}")
    # print(f"  pixels_per_mm_y: {pixels_per_mm_y}")
   
    # Check for None or invalid values
    if any(x is None for x in [focal_length_mm, principal_point_x_pixels, principal_point_y_pixels,
                               pixels_per_mm_x, pixels_per_mm_y]):
        print("ERROR: One or more input parameters is None!")
        return None
   
    if pixels_per_mm_x == 0 or pixels_per_mm_y == 0:
        print("ERROR: pixels_per_mm cannot be zero!")
        return None
   
    try:
        # Convert focal length from mm to pixels
        fx = focal_length_mm * pixels_per_mm_x
        fy = focal_length_mm * pixels_per_mm_y
       
        # Principal point is already in pixels
        cx = principal_point_x_pixels
        cy = principal_point_y_pixels
       
        # print(f"DEBUG: Camera matrix calculation:")
        # print(f"  fx = {focal_length_mm} * {pixels_per_mm_x} = {fx}")
        # print(f"  fy = {focal_length_mm} * {pixels_per_mm_y} = {fy}")
        # print(f"  cx = {cx}")
        # print(f"  cy = {cy}")
       
        camera_matrix = np.array([
            [fx,  0,  cx],
            [0,  fy,  cy],
            [0,   0,   1]
        ], dtype=np.float64)
       
        # print(f"DEBUG: Final camera matrix:\n{camera_matrix}")
       
        return camera_matrix
       
    except Exception as e:
        print(f"ERROR in create_camera_matrix: {e}")
        return None
 
def create_sample_data():
    """Create sample point cloud and camera parameters for testing"""
    # Generate sample point cloud (random points in a cube)
    np.random.seed(42)
    n_points = 1000
    points = np.random.uniform(-10, 10, (n_points, 3))
    points[:, 2] += 20  # Move points in front of camera
   
    # Add intensity values
    intensities = np.random.uniform(0, 255, n_points)
   
    # Create .bin file (KITTI format: x, y, z, intensity)
    bin_data = np.column_stack([points, intensities]).astype(np.float32)
    bin_data.tofile("sample_pointcloud.bin")
   
    # Also create .pcd for compatibility
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    o3d.io.write_point_cloud("sample_pointcloud.pcd", pcd)
   
    # Create sample camera matrix (typical values)
 
    focal_length_mm = 12.0            # Your focal length in mm
    principal_point_x_pixels = 960    # Principal point X in pixels
    principal_point_y_pixels = 600    # Principal point Y in pixels  
    pixels_per_mm_x = 170.648           # Your pixels per mm in X
    pixels_per_mm_y = 170.648           # Your pixels per mm in Y
   
    # Create camera matrix
    camera_matrix = create_camera_matrix(
        focal_length_mm, principal_point_x_pixels, principal_point_y_pixels,
        pixels_per_mm_x, pixels_per_mm_y
    )
    # camera_matrix = np.array([
    #     [800, 0, 320],
    #     [0, 800, 240],
    #     [0, 0, 1]
    # ], dtype=np.float32)
   
    # Sample distortion coefficients
    # dist_coeffs = np.array([0.1, -0.2, 0, 0, 0.1], dtype=np.float32)
    dist_coeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
   
    # Create a simple test image
    test_image = np.zeros((1200, 1920, 3), dtype=np.uint8)
    cv2.putText(test_image, "Sample Camera Image", (150, 240),
                cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
    cv2.imwrite("sample_image.jpg", test_image)
   
    return camera_matrix, dist_coeffs
 
 
def main():
    parser = argparse.ArgumentParser(description='LiDAR-Camera Calibration Tool')
    parser.add_argument('--pcd', type=str, help='Path to point cloud file (.pcd, .ply, .bin, etc.)')
    parser.add_argument('--image', type=str, help='Path to camera image')
    parser.add_argument('--demo', action='store_true', help='Run with sample data')
   
    args = parser.parse_args()
   
    if args.demo or (not args.pcd or not args.image):
        print("Creating sample data for demonstration...")
        camera_matrix, dist_coeffs = create_sample_data()
        pcd_path = "sample_pointcloud.bin"  # Use .bin file for demo
        img_path = "sample_image.jpg"
    else:
        pcd_path = args.pcd
        img_path = args.image
       
        # You'll need to provide your camera calibration parameters here
        focal_length_mm = 12.0            # Your focal length in mm
        principal_point_x_pixels = 960    # Principal point X in pixels
        principal_point_y_pixels = 600    # Principal point Y in pixels  
        pixels_per_mm_x = 170.648           # Your pixels per mm in X
        pixels_per_mm_y = 170.648           # Your pixels per mm in Y
       
        # Create camera matrix
        camera_matrix = create_camera_matrix(
            focal_length_mm, principal_point_x_pixels, principal_point_y_pixels,
            pixels_per_mm_x, pixels_per_mm_y
        )
        # camera_matrix = np.array([
        #     [800, 0, 320],
        #     [0, 800, 240],
        #     [0, 0, 1]
        # ], dtype=np.float32)
       
        # Sample distortion coefficients
        # dist_coeffs = np.array([0.1, -0.2, 0, 0, 0.1], dtype=np.float32)
        dist_coeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
   
    # Check if files exist
    if not os.path.exists(pcd_path):
        print(f"Error: Point cloud file {pcd_path} not found")
        return
   
    if not os.path.exists(img_path):
        print(f"Error: Image file {img_path} not found")
        return
   
    # Initialize calibrator
    calibrator = LiDARCameraCalibrator(pcd_path, img_path, camera_matrix, dist_coeffs)
   
    # Set initial transformation values (modify these as needed)
    # calibrator.set_initial_values(tx=1.932, ty=0, tz=1.4, rx=0, ry=-2.0, rz=1.5)
    # calibrator.set_initial_values(tx=1.932, ty=0.25, tz=1.4, rx=0.0, ry=-2.0, rz=1.5)
    calibrator.set_initial_values(tx=-1.256, ty=-0.1, tz=0.5, rx=0.0, ry=2.2, rz=-0.55)
    # calibrator.set_initial_values(tx=-1.16, ty=-0.23, tz=0.42, rx=0.0, ry=2.0, rz=-0.55)
    # calibrator.set_initial_values(tx=1.932, ty=0.25, tz=1.486, rx=0, ry=-2.0, rz=1.5)
    # calibrator.set_initial_values(tx=0.23, ty=-0.42, tz=-1.16, rx=-2.5, ry=0.46, rz=-0.7)
    # calibrator.set_initial_values(tx=0.192, ty=-0.641, tz=-1.474, rx=-2.51, ry=0.46, rz=-0.69)
   
    # Run the calibration tool
    calibrator.run()
 
 
if __name__ == "__main__":
    main(