Merge pull request #562 from truher/main

make gtsam work in raspberry pi

.github/workflows/gtsam.yml

@@ -1,7 +1,7 @@
 # see https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python
 name: GTSAM tests
 
-on: [push]
+on: [push, workflow_dispatch]
 
 jobs:
   build:
@@ -38,7 +38,9 @@ jobs:
       - run: pip install robotpy-wpiutil
       - run: pip install robotpy-apriltag
       - run: pip install opencv-python
-      - run: pip install gtsam
+      #- run: pip install gtsam
+      # actually use the nightly, since the release version does not work.
+      - run: pip install --index-url https://test.pypi.org/simple --extra-index-url https://pypi.org/simple/ truher-gtsam-nightly 
 
 #      - run: python3 runtests.py
 

raspberry_pi/app/camera/interpreter_factory.py

@@ -1,14 +1,14 @@
 # pylint: disable=C0301,E0611,E1101,R0903
+import numpy as np
 
-
+from app.camera.camera_protocol import Camera
 from app.camera.interpreter_protocol import Interpreter
 from app.config.identity import Identity
-from app.camera.camera_protocol import Camera
 from app.dashboard.fake_display import FakeDisplay
 from app.dashboard.real_display import RealDisplay
-from app.network.network import Network
 from app.localization.note_detector import NoteDetector
 from app.localization.tag_detector import TagDetector
+from app.network.network import Network
 
 
 class InterpreterFactory:
@@ -28,7 +28,19 @@ def get(
                     int(scale * size.height),
                     "tag" + str(camera_num),
                 )
-                return NoteDetector(identity, cam, camera_num, display, network)
+
+                # GREEN TARGET VALUES
+                object_lower = np.array((40, 50, 100))
+                object_higher = np.array((70, 255, 255))
+                return NoteDetector(
+                    identity,
+                    cam,
+                    camera_num,
+                    display,
+                    network,
+                    object_lower,
+                    object_higher,
+                )
             case (
                 Identity.RIGHTAMP
                 | Identity.LEFTAMP

raspberry_pi/app/config/camera_config.py

@@ -17,13 +17,13 @@ def __init__(self, identity: Identity) -> None:
                     np.array([0, 0, 0.5]),
                 )
                 self.calib = gtsam.Cal3DS2(
-                    200.0, 200.0, 0.0, 400.0, 300.0, -0.2, 0.1, 0.0, 0.0
+                    200.0, 200.0, 0.0, 400.0, 300.0, -0.2, 0.1
                 )
             case _:
                 self.camera_offset = gtsam.Pose3(
                     gtsam.Rot3(),
                     np.array([0, 0, 1]),
                 )
                 self.calib = gtsam.Cal3DS2(
-                    200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1, 0.0, 0.0
+                    200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1
                 )

raspberry_pi/app/localization/note_detector.py

@@ -26,7 +26,13 @@ def __init__(
         camera_num: int,
         display: Display,
         network: Network,
+        object_lower: np.ndarray,
+        object_higher: np.ndarray
     ) -> None:
+        """
+        object_lower and object_higher are ([H, S, V]) bounds.
+        NOTE: hue values are 0-180, half the usual range.
+        """
         self.cam = cam
         self.display = display
         self.network = network
@@ -38,11 +44,8 @@ def __init__(
         self.width: int = size.width
         self.height: int = size.height
 
-        # opencv hue values are 0-180, half the usual number
-        lowerSat = 50
-        lowerValue = 100
-        self.object_lower = np.array((30, lowerSat, lowerValue))
-        self.object_higher = np.array((50, 255, 255))
+        self.object_lower = object_lower
+        self.object_higher = object_higher
 
         # TODO: move the identity part of this path to the Network object
         path = "noteVision/" + identity.value + "/" + str(camera_num)

raspberry_pi/app/network/structs.py

@@ -106,9 +106,9 @@ class Cal3DS2:
     # radial 2nd-order and 4th-order
     k1: float
     k2: float
-    #  tangential distortion
-    p1: float
-    p2: float
+    #  tangential distortion is not wrapped by gtsam
+    # p1: float
+    # p2: float
 
 
 @wpistruct.make_wpistruct 

raspberry_pi/app/pose_estimator/calibrate.py

@@ -42,8 +42,6 @@
     300.0,  # known precisely
     0.0,
     0.0,
-    0.0,
-    0.0,
 )
 
 # calibration prior sigma.
@@ -174,11 +172,15 @@ def init(self) -> None:
 
     def update(self) -> None:
         """Run the solver"""
+        # print("FACTORS", self._new_factors)
+        # print("VALUES", self._new_values)
+        # print("TIMESTAMPS", self._new_timestamps)
+
         self._isam.update(self._new_factors, self._new_values, self._new_timestamps)
         self._result: gtsam.Values = self._isam.calculateEstimate()  # type: ignore
 
         # print("TIMESTAMPS")
-        # print(self.isam.timestamps())
+        # print(self._isam.timestamps())
         k = max(self._isam.timestamps().keys())
         ts = max(self._isam.timestamps().values())
         # print(self._result.atPose2(k))

raspberry_pi/app/pose_estimator/factors/apriltag_calibrate_batch.py

@@ -37,6 +37,9 @@ def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
         # this is z-forward y-down
         camera_pose = offset_pose.compose(CAM_COORD)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
+        # Camera.project() will throw CheiralityException sometimes
+        # so be sure to use GTSAM_THROW_CHEIRALITY_EXCEPTION=OFF
+        # (as nightly.yml does).
         return np.concatenate([camera.project(x) for x in landmarks])
 
     return h

raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py

@@ -40,6 +40,11 @@ def h(p0: gtsam.Pose2) -> np.ndarray:
         # this is z-forward y-down
         camera_pose = offset_pose.compose(CAM_COORD)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
+        # print("CAMERA", camera)
+        # print("LANDMARK", landmark)
+        # Camera.project() will throw CheiralityException sometimes
+        # so be sure to use GTSAM_THROW_CHEIRALITY_EXCEPTION=OFF
+        # (as nightly.yml does).
         return camera.project(landmark)
 
     return h
@@ -55,7 +60,10 @@ def h_H(
 ) -> np.ndarray:
     """Error function (in pixels), including Jacobians, H."""
     h = h_fn(landmark, offset, calib)
-    result = h(p0) - measured
+    estimate = h(p0)
+    # print("ESTIMATE", estimate)
+    # print("MEASURED", measured)
+    result = estimate - measured
     if H is not None:
         H[0] = numericalDerivative11(h, p0)
     return result

raspberry_pi/app/pose_estimator/factors/apriltag_smooth_batch.py

@@ -35,6 +35,9 @@ def h(p0: gtsam.Pose2) -> np.ndarray:
         # this is z-forward y-down
         camera_pose = offset_pose.compose(CAM_COORD)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
+        # Camera.project() will throw CheiralityException sometimes
+        # so be sure to use GTSAM_THROW_CHEIRALITY_EXCEPTION=OFF
+        # (as nightly.yml does).
         return np.concatenate([camera.project(x) for x in landmarks])
 
     return h

raspberry_pi/app/pose_estimator/nt_calibrate.py

@@ -73,6 +73,7 @@ def step(self) -> None:
         self._receive_gyro()
 
         self.est.keep_calib_hot(self.net.now())
+        print("UPDATE!")
         self.est.update()
 
         # print("NTEstimate.step() result ", self.est.result)
@@ -131,8 +132,8 @@ def step(self) -> None:
             ks0[4],
             ks0[5],
             ks0[6],
-            ks0[7],
-            ks0[8],
+            # ks0[7],
+            # ks0[8],
         )
 
         camera_calibration = CameraCalibration(cp0, ct0, kd0, kc0)
@@ -171,7 +172,7 @@ def _receive_blips(self) -> None:
             # so that the network schema and the estimate schema are more
             # similar
             for blip in blip_list:
-                print("TIME", time_slice, "BLIP", blip)
+                # print("TIME", time_slice, "BLIP", blip)
                 pixels = blip.measurement()
                 corners = self.field_map.get(blip.tag_id)
                 self.est.add_state(time_slice, self.state)
@@ -183,7 +184,7 @@ def _receive_odometry(self) -> None:
         for pos in odo:
             time_slice = util.discrete(pos[0])
             positions = pos[1]
-            print("TIME", time_slice, "POS", positions)
+            # print("TIME", time_slice, "POS", positions)
             self.est.add_state(time_slice, self.state)
             self.est.odometry(time_slice, positions, ODO_NOISE)
 
@@ -192,7 +193,7 @@ def _receive_gyro(self) -> None:
         for g in gyro:
             time_slice = util.discrete(g[0])
             yaw = g[1]
-            print("TIME", time_slice, "YAW", yaw)
+            # print("TIME", time_slice, "YAW", yaw)
             # if this is the only factor attached to this variable
             # then it will be underconstrained (i.e. no constraint on x or y)
             self.est.add_state(time_slice, self.state)

raspberry_pi/app/pose_estimator/nt_estimate.py

@@ -120,7 +120,7 @@ def _receive_blips(self) -> None:
             # so that the network schema and the estimate schema are more
             # similar
             for blip in blip_list:
-                print("TIME", time_slice, "BLIP", blip)
+                # print("TIME", time_slice, "BLIP", blip)
                 pixels = blip.measurement()
                 corners = self.field_map.get(blip.tag_id)
                 self.est.add_state(time_slice, self.state)

raspberry_pi/app/pose_estimator/parking_lot/parking_lot.py

@@ -89,7 +89,7 @@ def update(self) -> None:
         # print(self.isam.timestamps())
         k = max(self._isam.timestamps().keys())
         ts = max(self._isam.timestamps().values())
-        print(self._result.atPose2(k))
+        # print(self._result.atPose2(k))
         # print(ts)
 
         # reset the accumulators

raspberry_pi/app/pose_estimator/util.py

@@ -62,8 +62,9 @@ def pose2d_to_pose2(pose2d: Pose2d) -> gtsam.Pose2:
 
 
 def to_cal(gc: gtsam.Cal3DS2) -> Cal3DS2:
-    v = gc.vector()
-    return Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8])
+    return Cal3DS2(gc.fx(), gc.fy(), gc.skew(), gc.px(), gc.py(), gc.k1(), gc.k2())
+    # v = gc.vector()
+    # return Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8])
 
 
 def make_smoother(lag_s: float) -> gtsam.BatchFixedLagSmoother:

raspberry_pi/tests/localization/note_detector_test.py

@@ -1,6 +1,7 @@
 import unittest
 
 import ntcore
+import numpy as np
 from wpimath.geometry import Rotation3d
 
 from app.camera.fake_camera import FakeCamera
@@ -25,9 +26,18 @@ def test_one_note_found(self) -> None:
         # HSV range in the note detector
         # the blob is in the lower right quadrant, so the result
         # should be pitch-down yaw-right.
-        camera = FakeCamera("blob.jpg")
+        # ORANGE TARGET
+        # camera = FakeCamera("blob.jpg")
+        # GREEN PRACTICE TARGET
+        camera = FakeCamera("green_blob.jpg")
         display = FakeDisplay()
-        note_detector = NoteDetector(identity, camera, 0, display, network)
+
+        # GREEN TARGET VALUES
+        object_lower = np.array((40, 50, 100))
+        object_higher = np.array((70, 255, 255))
+        note_detector = NoteDetector(
+            identity, camera, 0, display, network, object_lower, object_higher
+        )
         request = camera.capture_request()
         note_detector.analyze(request)
 
@@ -37,26 +47,27 @@ def test_one_note_found(self) -> None:
         self.assertEqual(2, len(display.locs))
         self.assertEqual(1, display.frame_count)
 
-        self.assertEqual(482, display.circles[0][0])
+        self.assertAlmostEqual(482, display.circles[0][0], -1)
         self.assertEqual(468, display.circles[0][1])
 
         rots = sub.get()
         self.assertEqual(1, len(rots))
         rot = rots[0]
+        # NOTE: 0.01 rad resolution is all that can be expected.
         # ~zero
-        self.assertAlmostEqual(-0.0284, rot.x, 3)
+        self.assertAlmostEqual(-0.03, rot.x, 2)
         # pitch down
-        self.assertAlmostEqual(0.332, rot.y, 3)
+        self.assertAlmostEqual(0.33, rot.y, 2)
         # yaw right
-        self.assertAlmostEqual(-0.169, rot.z, 3)
+        self.assertAlmostEqual(-0.17, rot.z, 2)
         rot2d = rot.toRotation2d()
         # right yaw is about 10 deg
         self.assertAlmostEqual(-9.69, rot2d.degrees(), 2)
         q = rot.getQuaternion()
-        self.assertAlmostEqual(0, q.X(), 3)
-        self.assertAlmostEqual(0.166, q.Y(), 3)
-        self.assertAlmostEqual(-0.081, q.Z(), 3)
-        self.assertAlmostEqual(0.983, q.W(), 3)
+        self.assertAlmostEqual(0, q.X(), 2)
+        self.assertAlmostEqual(0.17, q.Y(), 2)
+        self.assertAlmostEqual(-0.08, q.Z(), 2)
+        self.assertAlmostEqual(0.98, q.W(), 2)
 
     def test_zero_notes_found(self) -> None:
         inst = ntcore.NetworkTableInstance.getDefault()
@@ -69,7 +80,13 @@ def test_zero_notes_found(self) -> None:
         # nothing in this image
         camera = FakeCamera("white_square.jpg")
         display = FakeDisplay()
-        note_detector = NoteDetector(identity, camera, 0, display, network)
+
+        # GREEN TARGET VALUES
+        object_lower = np.array((40, 50, 100))
+        object_higher = np.array((70, 255, 255))
+        note_detector = NoteDetector(
+            identity, camera, 0, display, network, object_lower, object_higher
+        )
         request = camera.capture_request()
         note_detector.analyze(request)
 

raspberry_pi/tests/pose_estimator/calibrate_apriltag_test.py

@@ -65,10 +65,11 @@ def test_apriltag_0(self) -> None:
         self.assertAlmostEqual(180, k0.fy(), 3)
         self.assertAlmostEqual(400, k0.px(), 3)
         self.assertAlmostEqual(300, k0.py(), 3)
-        self.assertAlmostEqual(0, k0.k()[0], 3)
-        self.assertAlmostEqual(0, k0.k()[1], 3)
-        self.assertAlmostEqual(0, k0.k()[2], 3)
-        self.assertAlmostEqual(0, k0.k()[3], 3)
+        self.assertAlmostEqual(0, k0.k1(), 3)
+        self.assertAlmostEqual(0, k0.k2(), 3)
+        # these are not wrapped
+        # self.assertAlmostEqual(0, k0.p1(), 3)
+        # self.assertAlmostEqual(0, k0.p2(), 3)
 
         # not meaningful i think
         sk0 = est.sigma(K(0))

raspberry_pi/tests/pose_estimator/calibrate_simulate_test.py

@@ -235,7 +235,7 @@ def test_camera_batched(self) -> None:
                 f"{s0[0]:{f}}, {s0[1]:{f}}, {s0[2]:{f}}, "
                 f"{err_x:{f}}, {err_y:{f}}, {err_theta:{f}}, "
                 f"{k0.fx():{f}}, {k0.fy():{f}}, {k0.px():{f}}, "
-                f"{k0.py():{f}}, {k0.k()[0]:{f}}, {k0.k()[1]:{f}}, "
+                f"{k0.py():{f}}, {k0.k1():{f}}, {k0.k2():{f}}, "
                 f"{c0.x():{f}}, {c0.y():{f}}, {c0.z():{f}}, "
                 f"{c0.rotation().roll():{f}}, {c0.rotation().pitch():{f}}, {c0.rotation().yaw():{f}}"
             )
@@ -329,7 +329,7 @@ def test_batched_camera_and_odometry_and_gyro(self) -> None:
                 f"{s0[0]:{f}}, {s0[1]:{f}}, {s0[2]:{f}}, "
                 f"{err_x:{f}}, {err_y:{f}}, {err_theta:{f}}, "
                 f"{k0.fx():{f}}, {k0.fy():{f}}, {k0.px():{f}}, "
-                f"{k0.py():{f}}, {k0.k()[0]:{f}}, {k0.k()[1]:{f}}, "
+                f"{k0.py():{f}}, {k0.k1():{f}}, {k0.k2():{f}}, "
                 f"{c0.x():{f}}, {c0.y():{f}}, {c0.z():{f}}, "
                 f"{c0.rotation().roll():{f}}, {c0.rotation().pitch():{f}}, {c0.rotation().yaw():{f}}"
             )

raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py

@@ -35,7 +35,7 @@ def test_apriltag_1(self) -> None:
             measured,
             0,
             gtsam.Pose3(),
-            gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1, 0.0, 0.0),
+            gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1),
         )
         est.update()
         self.assertEqual(1, est.result_size())

raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py

@@ -12,7 +12,7 @@
 
 # example calibration, focal length 200, pixel center 200, a little bit
 # of distortion.
-KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1, 0.0, 0.0)
+KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
 NOISE2 = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))