Add main calibration loop, UI and output files (#40)

This patch adds the main calibration logic, the CLI behavior and the logic to
generate the calibration output files.

Author: frneer

.gitignore

@@ -58,3 +58,5 @@ Build
 build
 install
 log
+
+images/

README.md

@@ -1,53 +1,103 @@
-![Kalibr](https://raw.githubusercontent.com/wiki/ethz-asl/kalibr/images/kalibr_small.png)
+# Kalibr2
 
-[![ROS1 Ubuntu 20.04](https://github.com/ethz-asl/kalibr/actions/workflows/docker_2004_build.yaml/badge.svg)](https://github.com/ethz-asl/kalibr/actions/workflows/docker_2004_build.yaml)
-[![ROS1 Ubuntu 18.04](https://github.com/ethz-asl/kalibr/actions/workflows/docker_1804_build.yaml/badge.svg)](https://github.com/ethz-asl/kalibr/actions/workflows/docker_1804_build.yaml)
-[![ROS1 Ubuntu 16.04](https://github.com/ethz-asl/kalibr/actions/workflows/docker_1604_build.yaml/badge.svg)](https://github.com/ethz-asl/kalibr/actions/workflows/docker_1604_build.yaml)
+A modern multi-camera calibration toolbox built on ROS 2.
 
 ## Introduction
-Kalibr is a toolbox that solves the following calibration problems:
-
-1. **Multi-Camera Calibration**: Intrinsic and extrinsic calibration of a camera-systems with non-globally shared overlapping fields of view with support for a wide range of [camera models](https://github.com/ethz-asl/kalibr/wiki/supported-models).
-1. **Visual-Inertial Calibration (CAM-IMU)**: Spatial and temporal calibration of an IMU w.r.t a camera-system along with IMU intrinsic parameters
-1. **Multi-Inertial Calibration (IMU-IMU)**: Spatial and temporal calibration of an IMU w.r.t a base inertial sensor along with IMU intrinsic parameters (requires 1-aiding camera sensor).
-1. **Rolling Shutter Camera Calibration**: Full intrinsic calibration (projection, distortion and shutter parameters) of rolling shutter cameras.
-
-To install follow the [install wiki page](https://github.com/ethz-asl/kalibr/wiki/installation) instructions for which you can either use Docker or install from source in a ROS workspace.
-Please find more information on the [wiki pages](https://github.com/ethz-asl/kalibr/wiki) of this repository.
-For questions or comments, please open an issue on Github.
-
-
-## News / Events
-
-* **Nov 24, 2022** - Some new visualization of trajectory and IMU rate for the generated report along with fixed support for exporting poses to file (see PR [#578](https://github.com/ethz-asl/kalibr/pull/578),[#581](https://github.com/ethz-asl/kalibr/pull/581),[#582](https://github.com/ethz-asl/kalibr/pull/582))
-* **May 3, 2022** - Support for Ubuntu 20.04 along with Docker scripts have been merged into master via PR [#515](https://github.com/ethz-asl/kalibr/pull/515). A large portion was upgrading to Python 3. A special thanks to all the contributors that made this possible. Additionally, contributed fixes for the different validation and visualization scripts have been merged.
-* **Febuary 3, 2020** - Initial Ubuntu 18.04 support has been merged via PR [#241](https://github.com/ethz-asl/kalibr/pull/241). Additionally, support for inputting an initial guess for focal length can be provided from the cmd-line on failure to initialize them.
-* **August 15, 2018** - Double sphere camera models have been contributed to the repository via PR [#210](https://github.com/ethz-asl/kalibr/pull/210). If you are interested you can refer to the [paper](https://arxiv.org/abs/1807.08957) for a nice overview of the models in the repository.
-* **August 25, 2016** - Rolling shutter camera calibration support was added as a feature via PR [#65](https://github.com/ethz-asl/kalibr/pull/65). The [paper](https://www.cv-foundation.org/openaccess/content_cvpr_2013/papers/Oth_Rolling_Shutter_Camera_2013_CVPR_paper.pdf) provides details for those interested.
-* **May 18, 2016** - Support for multiple IMU-to-IMU spacial and IMU intrinsic calibration was released.
-* **June 18, 2014** - Initial public release of the repository.
-
-
-## Authors
-* Paul Furgale
-* Hannes Sommer
-* Jérôme Maye
-* Jörn Rehder
-* Thomas Schneider ([email](thomas.schneider@voliro.com))
-* Luc Oth
 
+Kalibr2 is a camera calibration toolbox that provides:
+
+- **Multi-Camera Calibration**: Intrinsic and extrinsic calibration of multi-camera systems with support for various camera models (pinhole, omni-directional, EUCM, double sphere)
+- **ROS 2 Integration**: Native support for ROS 2
+- **Modern C++**: Built with C++17 and modern CMake practices
+
+This is a modernized version based on the original [Kalibr](https://github.com/ethz-asl/kalibr) calibration toolbox developed at ETH Zurich.
+
+## Build it using docker
+
+```bash
+cd docker/
+docker compose build kalibr2_ros
+docker compose run kalibr2_ros
+```
+
+## Building on Ubuntu 24.04
+
+### Prerequisites
+- **ROS 2 Jazzy Desktop** - Install following the [official ROS 2 installation instructions](https://docs.ros.org/en/jazzy/Installation.html)
+
+### Install System Dependencies
+```bash
+# Install all required packages
+sudo apt update && sudo apt install -y $(cat /tmp/packages.txt)
+```
+
+### Build
+```bash
+# Source ROS 2 environment
+source /opt/ros/jazzy/setup.bash
+
+# Build with colcon
+colcon build
+
+# Source the workspace
+source install/setup.bash
+```
+
+## Usage
+
+### Camera Calibration
+```bash
+# Calibrate cameras from a ROS 2 bag file
+ros2 run kalibr2_ros kalibr_calibrate_cameras \
+  --config path/to/calibration_config.yaml \
+  --output-dir path/to/output/directory
+```
+
+### Calibration file example
+
+```yaml
+board:
+  target_type: 'aprilgrid' # grid type
+  tagCols: 6               # number of apriltags in the x direction
+  tagRows: 5               # number of apriltags in the y direction
+  tagSize: 0.088           # size of apriltag, edge to edge [m]
+  tagSpacing: 0.2954       # ratio of space between tags to tagSize
+
+cameras:
+  camera_1_name:
+    model: 'pinhole-radtan'
+    focal_length_fallback: 881.0
+    source:
+      rosbag_path: '/path/to/your.mcap'
+      topic: '/camera_1_topic/image'
+  camera_2_topic:
+    model: 'pinhole-radtan'
+    focal_length_fallback: 881.0
+    source:
+      rosbag_path: '/path/to/your.mcap'
+      topic: '/camera_2_topic/image'
+```
+
+### Output files
+Calibration results are exported as:
+- `calibration_camera_name.yaml` - Camera intrinsics in ROS CameraInfo format
+- `transform_camera_0_to_camera_1.yaml` - Extrinsic transform (for 2-camera systems)
+- `camera_chain_transforms.yaml` - All extrinsic transforms in TF2 format (for multi-camera systems)
 
 ## References
-The calibration approaches used in Kalibr are based on the following papers. Please cite the appropriate papers when using this toolbox or parts of it in an academic publication.
 
-1. <a name="joern1"></a>Joern Rehder, Janosch Nikolic, Thomas Schneider, Timo Hinzmann, Roland Siegwart (2016). Extending kalibr: Calibrating the extrinsics of multiple IMUs and of individual axes. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 4304-4311, Stockholm, Sweden.
-1. <a name="paul1"></a>Paul Furgale, Joern Rehder, Roland Siegwart (2013). Unified Temporal and Spatial Calibration for Multi-Sensor Systems. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan.
-1. <a name="paul2"></a>Paul Furgale, T D Barfoot, G Sibley (2012). Continuous-Time Batch Estimation Using Temporal Basis Functions. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 2088–2095, St. Paul, MN.
-1. <a name="jmaye"></a> J. Maye, P. Furgale, R. Siegwart (2013). Self-supervised Calibration for Robotic Systems, In Proc. of the IEEE Intelligent Vehicles Symposium (IVS)
-1. <a name="othlu"></a>L. Oth, P. Furgale, L. Kneip, R. Siegwart (2013). Rolling Shutter Camera Calibration, In Proc. of the IEEE Computer Vision and Pattern Recognition (CVPR)
+The calibration approaches in Kalibr2 are based on the original Kalibr toolbox. Please cite the appropriate papers when using this toolbox in academic work:
+
+1. Paul Furgale, Joern Rehder, Roland Siegwart (2013). Unified Temporal and Spatial Calibration for Multi-Sensor Systems. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan.
+2. Paul Furgale, T D Barfoot, G Sibley (2012). Continuous-Time Batch Estimation Using Temporal Basis Functions. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 2088–2095, St. Paul, MN.
 
-## Acknowledgments
-This work is supported in part by the European Union's Seventh Framework Programme (FP7/2007-2013) under grants #269916 (V-Charge), and #610603 (EUROPA2).
+## Original Kalibr Authors
+- Paul Furgale
+- Hannes Sommer
+- Jérôme Maye
+- Jörn Rehder
+- Thomas Schneider
+- Luc Oth
 
 ## License (BSD)
 Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland<br>

aslam_cv/aslam_cameras/src/GridDetector.cpp

@@ -8,6 +8,8 @@
 #include <sm/logging.hpp>
 #include <aslam/cameras/GridDetector.hpp>
 
+#include <chrono>
+
 namespace aslam {
 namespace cameras {
 
@@ -90,22 +92,23 @@ bool GridDetector::findTarget(const cv::Mat & image,
 }
 
 bool GridDetector::findTargetNoTransformation(const cv::Mat & image, const aslam::Time & stamp,
-    GridCalibrationTargetObservation & outObservation) const {
+  GridCalibrationTargetObservation & outObservation) const {
   bool success = false;
 
   // Extract the calibration target corner points
   Eigen::MatrixXd cornerPoints;
   std::vector<bool> validCorners;
+
   success = _target->computeObservation(image, cornerPoints, validCorners);
 
   // Set the image, target, and timestamp regardless of success.
   outObservation.setTarget(_target);
   outObservation.setImage(image);
   outObservation.setTime(stamp);
 
-  // Set the observed corners in the observation
-  for (int i = 0; i < cornerPoints.rows(); i++) {
-    if (validCorners[i])
+  // time the loop that updates observed corners
+  for (int i = 0; i < cornerPoints.rows(); ++i) {
+    if (i < static_cast<int>(validCorners.size()) && validCorners[i])
       outObservation.updateImagePoint(i, cornerPoints.row(i).transpose());
   }
 
@@ -133,7 +136,7 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
   //calculate reprojection errors
   auto compute_stats = [&](double &mean, double &std, Eigen::MatrixXd &reprojection_errors_norm,
                            std::vector<cv::Point2f> &corners_reproj, std::vector<cv::Point2f> &corners_detected) {
-    
+
     corners_reproj.clear();
     corners_detected.clear();
     outObservation.getCornerReprojection(_geometry, corners_reproj);
@@ -211,17 +214,17 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
       Eigen::MatrixXd reprojection_errors_norm;
       std::vector<cv::Point2f> corners_reproj, corners_detected;
       compute_stats(mean, std, reprojection_errors_norm, corners_reproj, corners_detected);
-      
+
       // show the on the rendered image
       auto format_str = [](double data) {
         std::ostringstream ss;
         ss << std::setprecision(3) << data;
         return ss.str();
       };
-      cv::putText(imageCopy1, "reproj err mean: " + format_str(mean), 
+      cv::putText(imageCopy1, "reproj err mean: " + format_str(mean),
                   cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8,
                   CV_RGB(0,255,0), 3, 8, false);
-      cv::putText(imageCopy1, "reproj err std: " + format_str(std), 
+      cv::putText(imageCopy1, "reproj err std: " + format_str(std),
                   cv::Point(50, 100), cv::FONT_HERSHEY_SIMPLEX, 0.8,
                   CV_RGB(0,255,0), 3, 8, false);