Merge pull request #240 from johnwlambert/sim2-clasmethod-and-calib-h…

…elpers

Updated: Add factory functions for Sim(2) clasmethod and calibration helpers in loader

argoverse/data_loading/simple_track_dataloader.py

@@ -2,10 +2,11 @@
 
 import glob
 from pathlib import Path
-from typing import Any, List, Mapping, Optional
+from typing import Any, Dict, List, Mapping, Optional
 
 from argoverse.data_loading.pose_loader import get_city_SE3_egovehicle_at_sensor_t, read_city_name
 from argoverse.data_loading.synchronization_database import SynchronizationDB
+from argoverse.utils.calibration import CameraConfig, get_calibration_config
 from argoverse.utils.json_utils import read_json_file
 from argoverse.utils.se3 import SE3
 
@@ -26,7 +27,7 @@ def __init__(self, data_dir: str, labels_dir: str) -> None:
         self.sdb = SynchronizationDB(data_dir)
 
     def get_city_name(self, log_id: str) -> str:
-        """
+        """Return the name of the city where the log of interest was captured.
         Args:
             log_id: str
 
@@ -38,7 +39,7 @@ def get_city_name(self, log_id: str) -> str:
         assert isinstance(city_name, str)
         return city_name
 
-    def get_log_calibration_data(self, log_id: str) -> Mapping[str, Any]:
+    def get_log_calibration_data(self, log_id: str) -> Dict[str, Any]:
         """
         Args:
             log_id: str
@@ -51,6 +52,12 @@ def get_log_calibration_data(self, log_id: str) -> Mapping[str, Any]:
         assert isinstance(log_calib_data, dict)
         return log_calib_data
 
+    def get_log_camera_config(self, log_id: str, camera_name: str) -> CameraConfig:
+        """Return an object containing camera extrinsics, intrinsics, and image dimensions."""
+        log_calib_data = self.get_log_calibration_data(log_id)
+        camera_config = get_calibration_config(log_calib_data, camera_name)
+        return camera_config
+
     def get_city_to_egovehicle_se3(self, log_id: str, timestamp: int) -> Optional[SE3]:
         """Deprecated version of get_city_SE3_egovehicle() below, as does not follow standard naming convention
         Args:

argoverse/utils/sim2.py

@@ -1,16 +1,21 @@
 """
 Utility for 2d rigid body transformations with scaling.
 
-Refs:
+References:
     http://ethaneade.com/lie_groups.pdf
     https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Similarity3.h
 """
 
+import json
+import os
 from typing import Union
 
 import numpy as np
 
 from argoverse.utils.helpers import assert_np_array_shape
+from argoverse.utils.json_utils import save_json_dict
+
+_PathLike = Union[str, "os.PathLike[str]"]
 
 
 class Sim2:
@@ -141,3 +146,38 @@ def transform_from(self, point_cloud: np.ndarray) -> np.ndarray:
     def transform_point_cloud(self, point_cloud: np.ndarray) -> np.ndarray:
         """Alias for `transform_from()`, for synchrony w/ API provided by SE(2) and SE(3) classes."""
         return self.transform_from(point_cloud)
+
+    def save_as_json(self, save_fpath: _PathLike) -> None:
+        """Save the Sim(2) object to a JSON representation on disk.
+
+        Args:
+            save_fpath: path to where json file should be saved
+        """
+        dict_for_serialization = {
+            "R": self.rotation.flatten().tolist(),
+            "t": self.translation.flatten().tolist(),
+            "s": self.scale,
+        }
+        save_json_dict(save_fpath, dict_for_serialization)
+
+    @classmethod
+    def from_json(cls, json_fpath: _PathLike) -> "Sim2":
+        """Generate class inst. from a JSON file containing Sim(2) parameters as flattened matrices (row-major)."""
+        with open(json_fpath, "r") as f:
+            json_data = json.load(f)
+
+        R = np.array(json_data["R"]).reshape(2, 2)
+        t = np.array(json_data["t"]).reshape(2)
+        s = float(json_data["s"])
+        return cls(R, t, s)
+
+    @classmethod
+    def from_matrix(cls, T: np.ndarray) -> "Sim2":
+        """Generate class instance from a 3x3 Numpy matrix."""
+        if np.isclose(T[2, 2], 0.0):
+            raise ZeroDivisionError("Sim(2) scale calculation would lead to division by zero.")
+
+        R = T[:2, :2]
+        t = T[:2, 2]
+        s = 1 / T[2, 2]
+        return cls(R, t, s)

tests/test_data/a_Sim2_b.json

@@ -0,0 +1 @@
+{"R": [1, 0, 0, 1], "t": [3930, 3240], "s": 1.6666666666666667}

tests/test_data/a_Sim2_b___invalid.json

@@ -0,0 +1 @@
+{"R": [1, 0, 0, 1], "t": [3930, 3240], "s": 0.0}

tests/test_sim2.py

@@ -1,11 +1,15 @@
 import copy
+from pathlib import Path
 
 import numpy as np
 import pytest
 
+from argoverse.utils.json_utils import read_json_file
 from argoverse.utils.se2 import SE2
 from argoverse.utils.sim2 import Sim2
 
+TEST_DATA_ROOT = Path(__file__).resolve().parent / "test_data"
+
 
 def test_constructor() -> None:
     """Sim(2) to perform p_b = bSa * p_a"""
@@ -142,6 +146,29 @@ def test_matrix() -> None:
     assert np.allclose(bSa_expected, bSa.matrix)
 
 
+def test_from_matrix() -> None:
+    """Ensure that classmethod can construct an object instance from a 3x3 numpy matrix."""
+
+    bRa = np.array([[0, -1], [1, 0]])
+    bta = np.array([1, 2])
+    bsa = 3.0
+    bSa = Sim2(R=bRa, t=bta, s=bsa)
+
+    bSa_ = Sim2.from_matrix(bSa.matrix)
+
+    # ensure we can reconstruct new instance from matrix
+    assert bSa == bSa_
+
+    # ensure generated class object has correct attributes
+    assert np.allclose(bSa_.rotation, bRa)
+    assert np.allclose(bSa_.translation, bta)
+    assert np.isclose(bSa_.scale, bsa)
+
+    # ensure generated class object has correct 3x3 matrix attribute
+    bSa_expected = np.array([[0, -1, 1], [1, 0, 2], [0, 0, 1 / 3]])
+    assert np.allclose(bSa_expected, bSa_.matrix)
+
+
 def test_matrix_homogenous_transform() -> None:
     """Ensure 3x3 matrix transforms homogenous points as expected."""
     expected_img_pts = np.array([[6, 4], [4, 6], [0, 0], [1, 7]])
@@ -262,3 +289,46 @@ def test_transform_from_wrong_dims() -> None:
 
     with pytest.raises(ValueError) as e_info:
         val = bSa.transform_from(np.array([1.0, 3.0]))
+
+
+def test_from_json() -> None:
+    """Ensure that classmethod can construct an object instance from a json file."""
+    json_fpath = TEST_DATA_ROOT / "a_Sim2_b.json"
+    aSb = Sim2.from_json(json_fpath)
+
+    expected_rotation = np.array([[1.0, 0.0], [0.0, 1.0]])
+    expected_translation = np.array([3930.0, 3240.0])
+    expected_scale = 1.6666666666666667
+    assert np.allclose(aSb.rotation, expected_rotation)
+    assert np.allclose(aSb.translation, expected_translation)
+    assert np.isclose(aSb.scale, expected_scale)
+
+
+def test_from_json_invalid_scale() -> None:
+    """Ensure that classmethod raises an error with invalid JSON input."""
+    json_fpath = TEST_DATA_ROOT / "a_Sim2_b___invalid.json"
+
+    with pytest.raises(ZeroDivisionError) as e_info:
+        aSb = Sim2.from_json(json_fpath)
+
+
+def test_save_as_json() -> None:
+    """Ensure that JSON serialization of a class instance works correctly."""
+    bSc = Sim2(R=np.array([[0, 1], [1, 0]]), t=np.array([-5, 5]), s=0.1)
+    save_fpath = TEST_DATA_ROOT / "b_Sim2_c.json"
+    bSc.save_as_json(save_fpath=save_fpath)
+
+    bSc_dict = read_json_file(save_fpath)
+    assert bSc_dict["R"] == [0, 1, 1, 0]
+    assert bSc_dict["t"] == [-5, 5]
+    assert bSc_dict["s"] == 0.1
+
+
+def test_round_trip() -> None:
+    """Test round trip of serialization, then de-serialization."""
+    bSc = Sim2(R=np.array([[0, 1], [1, 0]]), t=np.array([-5, 5]), s=0.1)
+    save_fpath = TEST_DATA_ROOT / "b_Sim2_c.json"
+    bSc.save_as_json(save_fpath=save_fpath)
+
+    bSc_ = Sim2.from_json(save_fpath)
+    assert bSc_ == bSc

tests/test_simple_track_dataloader.py

@@ -3,9 +3,12 @@
 import os
 import pathlib
 
+import numpy as np
 import pytest
+from scipy.spatial.transform import Rotation
 
 from argoverse.data_loading.simple_track_dataloader import SimpleArgoverseTrackingDataLoader
+from argoverse.utils.calibration import CameraConfig
 
 _TEST_DATA = pathlib.Path(__file__).parent / "test_data" / "tracking"
 _LOG_ID = "1"
@@ -27,6 +30,30 @@ def test_get_log_calibration_data(
     assert data_loader.get_log_calibration_data(_LOG_ID)
 
 
+def test_get_log_camera_config(data_loader: SimpleArgoverseTrackingDataLoader):
+    """Ensure attributes of CameraConfig object are generated correctly."""
+    camera_name = "ring_front_center"
+    cam_config = data_loader.get_log_camera_config(_LOG_ID, camera_name)
+    assert isinstance(cam_config, CameraConfig)
+
+    assert cam_config.img_height == 1200
+    assert cam_config.img_width == 1920
+
+    # check intrinsics, should be 3x4 since we use 4x4 extrinsics
+    expected_K = np.array([[1392.11, 0, 980.18, 0], [0, 1392.11, 604.35, 0], [0, 0, 1, 0]])
+    assert np.allclose(expected_K, cam_config.intrinsic, atol=0.01)
+    assert cam_config.distortion_coeffs == [-0.1720396447593493, 0.11689572230654095, -0.02511932396889168]
+
+    # check extrinsics
+    qw, qx, qy, qz = 0.49605542988442836, -0.49896196582115804, 0.5027901707576079, -0.5021633313331392
+    R = Rotation.from_quat([qx, qy, qz, qw]).as_matrix()
+    t = [1.6519358245144808, -0.0005354981581146487, 1.3613890006792675]
+    egoTc = np.eye(4)
+    egoTc[:3, :3] = R
+    egoTc[:3, 3] = t
+    assert np.allclose(cam_config.extrinsic, np.linalg.inv(egoTc), atol=1e-2)
+
+
 def test_get_city_SE3_egovehicle(
     data_loader: SimpleArgoverseTrackingDataLoader,
 ) -> None: