chore: Rewrite in Python3

.github/workflows/humble.yml

@@ -55,7 +55,7 @@ jobs:
             python3-wstool \
             ros-$ROS_DISTRO-cv-bridge \
             ros-$ROS_DISTRO-image-transport
-          sudo apt-get install -y ros-velodyne-pcl
+          sudo apt-get install -y ros-$ROS_DISTRO-velodyne-pcl
 
           sudo apt upgrade
           sudo rosdep init

.gitignore

@@ -0,0 +1,31 @@
+# Environments
+.venv
+
+# Data
+/data/
+
+# IDE
+.idea
+
+# Distribution / packaging
+**/__pycache__
+*.egg-info/
+build
+
+# Storage Files
+*.npy
+*.h5
+
+# Unit test / coverage reports
+.coverage*
+coverage*
+# But keep .coveragerc and badges
+!.coveragerc
+!.badges/*
+
+# Profiling
+profile*.html
+
+accelerate_config.yaml
+wandb
+modal_scripts

lidar_camera_calibration_py/README.md

@@ -0,0 +1,3 @@
+# Python3 version of LiDAR-Camera Calibration
+# This package will contain the Python implementation and tests.
+

lidar_camera_calibration_py/package.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lidar_camera_calibration_py</name>
+  <version>0.1.0</version>
+  <description>Python3 version of LiDAR-Camera Calibration using latest ROS conventions.</description>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <exec_depend>rospy</exec_depend>
+  <test_depend>pytest</test_depend>
+</package>

lidar_camera_calibration_py/pyproject.toml

@@ -0,0 +1,23 @@
+[build-system]
+requires = ["setuptools>=61.0", "wheel"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "lidar_camera_calibration_py"
+version = "0.1.0"
+description = "Python3 version of LiDAR-Camera Calibration using latest ROS conventions."
+readme = "README.md"
+requires-python = ">=3.8"
+dependencies = [
+    "pytest",
+    "opencv-python",
+    "numpy",
+    "scipy",
+]
+
+[project.scripts]
+lidar_camera_calibration_py_node = "lidar_camera_calibration_py.main:main"
+
+[tool.setuptools.packages.find]
+where = ["src"]
+# Note: ROS and rospy must be installed via apt/ros installation, not pip.

lidar_camera_calibration_py/ruff.toml

@@ -0,0 +1,45 @@
+line-length = 88
+
+[lint]
+select = [
+    "E",# pycodestyle
+    "F",# Pyflakes
+    "UP",# pyupgrade
+    "B",# flake8-bugbear
+    "SIM",# flake8-simplify
+    "I",# isort
+    "NPY", # numpy
+    "PD", # pandas
+    "RUF",
+    # TODO: Fix this and enable ANN
+    # "ANN", # flake8-annotations: checks for missing type hints
+]
+
+ignore = [
+    # Avoid using the generic variable name `df` for DataFrames.
+    "PD901",
+    # Use `key in dict` instead of `key in dict.keys()`
+    "SIM118",
+]
+
+
+[lint.pydocstyle]
+# Use Google-style docstrings.
+convention = "google"
+
+[lint.isort]
+force-single-line = true
+
+[format]
+# Like Black, use double quotes for strings.
+quote-style = "double"
+docstring-code-format = true
+
+# Like Black, indent with spaces, rather than tabs.
+indent-style = "space"
+
+# Like Black, respect magic trailing commas.
+skip-magic-trailing-comma = false
+
+# Like Black, automatically detect the appropriate line ending.
+line-ending = "auto"

lidar_camera_calibration_py/src/calibration_math.py

@@ -0,0 +1,49 @@
+from __future__ import annotations
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from typing import Tuple, Any
+
+def estimate_transform(camera_points: list[Any], lidar_points: list[Any]) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    Estimate the rigid transformation (rotation and translation) that aligns lidar_points to camera_points.
+
+    Args:
+        camera_points (list[Any]): List of 3D points in the camera frame.
+        lidar_points (list[Any]): List of 3D points in the LiDAR frame (corresponding to camera_points).
+
+    Returns:
+        tuple[np.ndarray, np.ndarray]:
+            - R_est (np.ndarray): Estimated 3x3 rotation matrix.
+            - t_est (np.ndarray): Estimated 3x1 translation vector.
+    """
+    assert len(camera_points) == len(lidar_points)
+    camera_points = np.asarray(camera_points)
+    lidar_points = np.asarray(lidar_points)
+    centroid_camera = np.mean(camera_points, axis=0)
+    centroid_lidar = np.mean(lidar_points, axis=0)
+    camera_centered = camera_points - centroid_camera
+    lidar_centered = lidar_points - centroid_lidar
+    H = lidar_centered.T @ camera_centered
+    U, S, Vt = np.linalg.svd(H)
+    R_est = Vt.T @ U.T
+    if np.linalg.det(R_est) < 0:
+        Vt[-1, :] *= -1
+        R_est = Vt.T @ U.T
+    t_est = centroid_camera - R_est @ centroid_lidar
+    return R_est, t_est
+
+def average_quaternions(quaternions: list[np.ndarray]) -> np.ndarray:
+    """
+    Average a list of quaternions (Nx4 numpy array).
+
+    Args:
+        quaternions (list[np.ndarray]): List of quaternions to average (each as a 4-element array).
+
+    Returns:
+        np.ndarray: The average quaternion as a 4-element array.
+    """
+    Q = np.array(quaternions)
+    M = Q.T @ Q
+    eigvals, eigvecs = np.linalg.eigh(M)
+    avg_quat = eigvecs[:, np.argmax(eigvals)]
+    return avg_quat

lidar_camera_calibration_py/src/calibration_node.py

@@ -0,0 +1,97 @@
+from __future__ import annotations
+import rospy
+from sensor_msgs.msg import Image, PointCloud2, CameraInfo
+from cv_bridge import CvBridge
+import cv2
+from lidar_camera_calibration_py.src.config_utils import parse_config_file, parse_marker_coordinates
+from lidar_camera_calibration_py.src.calibration_math import estimate_transform
+import os
+from typing import Any, Tuple, Optional
+
+class LidarCameraCalibrationNode:
+    def __init__(self) -> None:
+        """
+        Initialize the LiDAR-Camera Calibration Node, load configuration, and subscribe to topics.
+        """
+        self.bridge = CvBridge()
+        # Load configuration files
+        config_path = os.path.join(os.path.dirname(__file__), '../../conf/config_file.txt')
+        marker_path = os.path.join(os.path.dirname(__file__), '../../conf/marker_coordinates.txt')
+        self.config = parse_config_file(config_path)
+        self.marker_boards = parse_marker_coordinates(marker_path)
+        self.image_sub = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
+        self.lidar_sub = rospy.Subscriber('/velodyne_points', PointCloud2, self.lidar_callback)
+        self.camera_info_sub = rospy.Subscriber('/camera/camera_info', CameraInfo, self.camera_info_callback)
+        rospy.loginfo('LidarCameraCalibrationNode initialized.')
+        # TODO: Add calibration logic, marker/corner detection, transformation estimation
+
+    def image_callback(self, msg: Image) -> None:
+        """
+        Callback for camera image topic. Detects ArUco markers in the image.
+
+        Args:
+            msg (Image): ROS Image message.
+        """
+        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        # Example: Detect ArUco markers (replace with your board/marker detection logic)
+        aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
+        aruco_params = cv2.aruco.DetectorParameters_create()
+        corners, ids, _ = cv2.aruco.detectMarkers(cv_image, aruco_dict, parameters=aruco_params)
+        if ids is not None:
+            rospy.loginfo(f'Detected ArUco markers: {ids.flatten()}')
+            cv_image = cv2.aruco.drawDetectedMarkers(cv_image, corners, ids)
+        else:
+            rospy.loginfo('No ArUco markers detected.')
+        # TODO: Use corners for calibration, visualize or store as needed
+
+    def lidar_callback(self, msg: PointCloud2) -> None:
+        """
+        Callback for LiDAR point cloud topic. Filters and processes point cloud data.
+
+        Args:
+            msg (PointCloud2): ROS PointCloud2 message.
+        """
+        # Convert PointCloud2 to numpy array
+        import sensor_msgs.point_cloud2 as pc2
+        points = list(pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True))
+        filtered_points = [p for p in points if self.config['x_min'] <= p[0] <= self.config['x_max'] and
+                                             self.config['y_min'] <= p[1] <= self.config['y_max'] and
+                                             self.config['z_min'] <= p[2] <= self.config['z_max'] and
+                                             p[3] >= self.config['cloud_intensity_threshold']]
+        rospy.loginfo(f'Received lidar point cloud. Filtered points: {len(filtered_points)}')
+        # TODO: Use filtered_points for calibration
+
+    def camera_info_callback(self, msg: CameraInfo) -> None:
+        """
+        Callback for camera info topic. Stores camera intrinsic parameters.
+
+        Args:
+            msg (CameraInfo): ROS CameraInfo message.
+        """
+        # TODO: Store camera intrinsic parameters
+        rospy.loginfo('Received camera info.')
+
+    def perform_calibration(self, camera_points: list[Any], lidar_points: list[Any]) -> Tuple[Optional[Any], Optional[Any]]:
+        """
+        Estimate the transformation between camera and LiDAR frames using corresponding points.
+
+        Args:
+            camera_points (list[Any]): List of 3D points in the camera frame.
+            lidar_points (list[Any]): List of 3D points in the LiDAR frame.
+
+        Returns:
+            tuple[Optional[Any], Optional[Any]]: Estimated rotation matrix and translation vector, or (None, None) if insufficient points.
+        """
+        if len(camera_points) != len(lidar_points) or len(camera_points) == 0:
+            rospy.logwarn('Insufficient or mismatched points for calibration.')
+            return None, None
+        R_est, t_est = estimate_transform(camera_points, lidar_points)
+        rospy.loginfo(f'Estimated rotation:\n{R_est}')
+        rospy.loginfo(f'Estimated translation: {t_est}')
+        # TODO: Save or publish results as needed
+        return R_est, t_est
+
+if __name__ == '__main__':
+    rospy.init_node('lidar_camera_calibration_py_node')
+    node = LidarCameraCalibrationNode()
+    rospy.spin()

lidar_camera_calibration_py/src/config_utils.py

@@ -0,0 +1,55 @@
+from __future__ import annotations
+from typing import Any, Dict
+
+def parse_config_file(config_path: str) -> dict[str, Any]:
+    """Parse the main configuration file for calibration parameters.
+
+    Args:
+        config_path (str): Path to the configuration file.
+
+    Returns:
+        dict[str, Any]: Dictionary of configuration parameters.
+    """
+    params: Dict[str, Any] = {}
+    with open(config_path, 'r') as f:
+        lines = [line.strip() for line in f if line.strip() and not line.startswith('#')]
+    # Parse lines according to README.md spec
+    params['image_width'], params['image_height'] = map(float, lines[0].split())
+    params['x_min'], params['x_max'] = map(float, lines[1].split())
+    params['y_min'], params['y_max'] = map(float, lines[2].split())
+    params['z_min'], params['z_max'] = map(float, lines[3].split())
+    params['cloud_intensity_threshold'] = float(lines[4])
+    params['number_of_markers'] = int(lines[5])
+    params['use_camera_info_topic'] = int(lines[6])
+    params['fx'], _, params['cx'], _ = map(float, lines[7].split())
+    _, params['fy'], params['cy'], _ = map(float, lines[8].split())
+    _, _, _, _ = map(float, lines[9].split()) # 3x3 matrix, only first row used
+    params['MAX_ITERS'] = int(lines[10])
+    params['initial_rot_x'], params['initial_rot_y'], params['initial_rot_z'] = map(float, lines[11].split())
+    params['lidar_type'] = int(lines[12])
+    return params
+
+def parse_marker_coordinates(marker_path: str) -> list[dict[str, float]]:
+    """Parse the marker coordinates file for board definitions.
+
+    Args:
+        marker_path (str): Path to the marker coordinates file.
+
+    Returns:
+        list[dict[str, float]]: List of board definitions with dimensions and properties.
+    """
+    boards: List[Dict[str, float]] = []
+    with open(marker_path, 'r') as f:
+        lines = [line.strip() for line in f if line.strip()]
+    N = int(lines[0])
+    for i in range(N):
+        idx = 1 + i*5
+        board = {
+            'length': float(lines[idx]),
+            'breadth': float(lines[idx+1]),
+            'border_width_length': float(lines[idx+2]),
+            'border_width_breadth': float(lines[idx+3]),
+            'aruco_edge_length': float(lines[idx+4]),
+        }
+        boards.append(board)
+    return boards

lidar_camera_calibration_py/src/main.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+
+"""
+Main entry point for the LiDAR-Camera calibration ROS node (Python version).
+
+This script initializes the ROS node and starts the calibration process.
+"""
+
+import rospy
+
+
+def main() -> None:
+    """
+    Main function to initialize the ROS node and start the calibration node.
+    """
+    rospy.init_node('lidar_camera_calibration_py_node')
+    rospy.loginfo('Python3 LiDAR-Camera Calibration node started.')
+    # TODO: Port calibration logic here
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()
+