Merge pull request #41 from open-dynamic-robot-initiative/fkloss/sens…

…or_info

Some minor changes regarding sensor info

demos/demo_tricamera.py

@@ -9,7 +9,6 @@
 import pickle
 
 import cv2
-import numpy as np
 
 import trifinger_cameras
 from trifinger_cameras import utils
@@ -48,17 +47,8 @@ def main() -> None:  # noqa: D103
     camera_frontend = trifinger_cameras.tricamera.Frontend(camera_data)
     observations_timestamps_list = []
 
-    np.set_printoptions(precision=3, suppress=True)
-    print("=== Camera Info: ======================")
-    for i, info in enumerate(camera_frontend.get_sensor_info().camera):
-        print(f"--- Camera {i}: ----------------------")
-        print(f"fps: {info.frame_rate_fps}")
-        print(f"width x height: {info.image_width}x{info.image_height}")
-        print(info.camera_matrix)
-        print(info.distortion_coefficients)
-        print(info.tf_world_to_camera)
-        print("---------------------------------------")
-    print("=======================================")
+    print("=== Camera Info: ===")
+    utils.print_tricamera_sensor_info(camera_frontend.get_sensor_info())
 
     while True:
         observation = camera_frontend.get_latest_observation()

doc/configuration.rst

@@ -54,12 +54,16 @@ How to use the custom configuration
 ===================================
 
 To use the file, simply write the path to the environment variable
-``TRIFINGER_CAMERA_CONFIG``.
+``TRIFINGER_CAMERA_CONFIG``:
+
+.. code-block:: bash
+
+   export TRIFINGER_CAMERA_CONFIG="/path/to/config.toml"
 
 Alternatively, if you instantiate the :cpp:class:`~trifinger_cameras::PylonDriver` or
-:cpp:class:`~trifinger_cameras::TriCameraDriver` classes in your own code, you can create a
-:cpp:class:`~trifinger_cameras::Settings` instance (specify the path to the TOML file in the constructor)
-and pass this to the constructor of the driver class.
+:cpp:class:`~trifinger_cameras::TriCameraDriver` classes in your own code, you can
+create a :cpp:class:`~trifinger_cameras::Settings` instance (specify the path to the
+TOML file in the constructor) and pass this to the constructor of the driver class.
 
 
 .. _pylon_settings_file:

include/trifinger_cameras/pybullet_tricamera_driver.hpp

@@ -33,16 +33,15 @@ class PyBulletTriCameraDriver
      * coefficients).
      *
      * **Important:**  The calibration coefficients are only set if the driver
-     * is initialized with a calibration file (see constructor).  Otherwise,
-     * they will be empty.
+     * is initialized with rendering enabled.  Otherwise, they will be all zero.
      */
-    virtual TriCameraInfo get_sensor_info() override;
+    TriCameraInfo get_sensor_info() override;
 
     /**
      * @brief Get the latest observation from the three cameras
      * @return TricameraObservation
      */
-    virtual trifinger_cameras::TriCameraObservation get_observation() override;
+    trifinger_cameras::TriCameraObservation get_observation() override;
 
 private:
     //! @brief Python object to access cameras in pyBullet.

include/trifinger_cameras/tricamera_driver.hpp

@@ -65,16 +65,16 @@ class TriCameraDriver
      * coefficients).
      *
      * **Important:**  The calibration coefficients are only set if the driver
-     * is initialized with a calibration file (see constructor).  Otherwise,
-     * they will be empty.
+     * is initialized with calibration files (see constructor).  Otherwise,
+     * they will be all zero.
      */
-    virtual TriCameraInfo get_sensor_info() override;
+    TriCameraInfo get_sensor_info() override;
 
     /**
      * @brief Get the latest observation from the three cameras
      * @return TricameraObservation
      */
-    TriCameraObservation get_observation();
+    TriCameraObservation get_observation() override;
 
 private:
     std::chrono::time_point<std::chrono::system_clock> last_update_time_;

python/trifinger_cameras/utils.py

@@ -1,13 +1,20 @@
 """Utility functions."""
 
+from __future__ import annotations
+
 import typing
 
 import cv2
 import numpy as np
+import tabulate
+
+
+if typing.TYPE_CHECKING:
+    from trifinger_cameras.py_tricamera_types import TriCameraInfo
 
 
 def rodrigues_to_matrix(rvec):
-    """Convert Rodrigues vector to homogeneous transformation matrix
+    """Convert Rodrigues vector to homogeneous transformation matrix.
 
     Args:
         rvec (array-like):  Rotation in Rodrigues format as returned by OpenCV.
@@ -79,3 +86,32 @@ def check_image_sharpness(
     edge_mean = np.mean(edge_image)
 
     return edge_mean, edge_image
+
+
+def print_tricamera_sensor_info(tricamera_info: TriCameraInfo) -> None:
+    """Pretty-print the sensor info struct of the TriCamera driver."""
+    camera_names = ["camera60", "camera180", "camera300"]
+
+    camera_info = [
+        (
+            info.frame_rate_fps,
+            f"{info.image_width}x{info.image_height}",
+            info.camera_matrix,
+            info.distortion_coefficients,
+            info.tf_world_to_camera,
+        )
+        for info in tricamera_info.camera
+    ]
+    # add headers
+    camera_info = [
+        ("fps", "resolution", "camera_matrix", "distortion", "tf_world_to_camera"),
+        *camera_info,
+    ]
+    # transpose the list for printing (so it's one column per camera)
+    camera_info = list(map(tuple, zip(*camera_info)))
+
+    with np.printoptions(precision=3, suppress=True):
+        print(
+            tabulate.tabulate(camera_info, headers=camera_names, tablefmt="pipe"),
+            "\n",
+        )

src/pybullet_tricamera_driver.cpp

@@ -86,12 +86,15 @@ PyBulletTriCameraDriver::PyBulletTriCameraDriver(
             sensor_info_.camera[i].image_width = width;
             sensor_info_.camera[i].image_height = height;
 
-            // if (py::hasattr(camera, "_original_camera_matrix"))
             if (py::isinstance(camera, mod_camera.attr("CalibratedCamera")))
             {
                 sensor_info_.camera[i].camera_matrix =
                     camera.attr("_original_camera_matrix")
                         .cast<Eigen::Matrix3d>();
+
+                sensor_info_.camera[i].distortion_coefficients =
+                    camera.attr("_distortion_coefficients")
+                        .cast<Eigen::Matrix<double, 1, 5>>();
             }
             else
             {

srcpy/py_camera_types.cpp

@@ -43,6 +43,7 @@ PYBIND11_MODULE(py_camera_types, m)
         .def(pybind11::init<const std::filesystem::path&, bool>(),
              pybind11::arg("camera_calibration_file"),
              pybind11::arg("downsample_images") = true)
+        .def("get_sensor_info", &PylonDriver::get_sensor_info)
         .def("get_observation", &PylonDriver::get_observation);
 #endif
 

srcpy/py_tricamera_types.cpp

@@ -44,6 +44,7 @@ PYBIND11_MODULE(py_tricamera_types, m)
              pybind11::arg("camera_calibration_file_3"),
              pybind11::arg("downsample_images") = true)
         .def_readonly("rate", &TriCameraDriver::rate)
+        .def("get_sensor_info", &TriCameraDriver::get_sensor_info)
         .def("get_observation", &TriCameraDriver::get_observation);
 #endif
 
@@ -69,5 +70,6 @@ PYBIND11_MODULE(py_tricamera_types, m)
                             bool>(),
              pybind11::arg("robot_data"),
              pybind11::arg("render_images") = true)
+        .def("get_sensor_info", &PyBulletTriCameraDriver::get_sensor_info)
         .def("get_observation", &PyBulletTriCameraDriver::get_observation);
 }