update python to match z-fwd camera

raspberry_pi/app/config/camera_config.py

@@ -7,23 +7,20 @@
 
 from app.config.identity import Identity
 
+
 # TODO: more configs for testing
 class CameraConfig:
     def __init__(self, identity: Identity) -> None:
         match identity:
             case Identity.UNKNOWN:
                 self.camera_offset = gtsam.Pose3(
-                    gtsam.Rot3(),
+                    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
                     np.array([0, 0, 0.5]),
                 )
-                self.calib = gtsam.Cal3DS2(
-                    200.0, 200.0, 0.0, 400.0, 300.0, -0.2, 0.1
-                )
+                self.calib = gtsam.Cal3DS2(200.0, 200.0, 0.0, 400.0, 300.0, -0.2, 0.1)
             case _:
                 self.camera_offset = gtsam.Pose3(
-                    gtsam.Rot3(),
+                    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
                     np.array([0, 0, 1]),
                 )
-                self.calib = gtsam.Cal3DS2(
-                    200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1
-                )
+                self.calib = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)

raspberry_pi/app/pose_estimator/calibrate.py

@@ -68,8 +68,9 @@
 # in simulation, the "z" component is fixed within 0.25 sec (!)
 # but the "x" and "y" are *never* fixed, i think because a fixed offset
 # just doesn't matter much in the sim geometry.
+# remember the camera is now z-fwd
 OFFSET_PRIOR_MEAN = gtsam.Pose3(
-    gtsam.Rot3(),
+    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
     np.array(
         [
             0.0,
@@ -82,17 +83,9 @@
 # offset prior sigma.
 # we can measure the offset within a few inches and tens of degrees
 # in gtsam, a pose3 logmap is [R_x,R_y,R_z,T_x,T_y,T_z]
+# remember camera is z-fwd
 OFFSET_PRIOR_NOISE = noiseModel.Diagonal.Sigmas(
-    np.array(
-        [
-            0.1,  # roll
-            0.2,  # pitch
-            0.2,  # yaw
-            0.2,  # x
-            0.2,  # y
-            0.2,  # z
-        ]
-    )
+    np.array([0.2, 0.2, 0.1, 0.2, 0.2, 0.2])
 )
 
 # TODO: real noise estimates.
@@ -121,6 +114,7 @@
 # performance advantage to batching in C++)
 USE_BATCH = False
 
+
 class Calibrate:
     def __init__(self, lag_s: float) -> None:
         """lag_s: size of lag window in seconds"""
@@ -181,7 +175,6 @@ def update(self) -> None:
         # 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
 
@@ -274,7 +267,9 @@ def apriltag_for_calibration_batch(
         measured: concatenated px measurements
         TODO: flatten landmarks"""
         if USE_BATCH:
-            noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
+            noise = noiseModel.Diagonal.Sigmas(
+                np.concatenate([[1, 1] for _ in landmarks])
+            )
             self._new_factors.push_back(
                 apriltag_calibrate_batch.factor(
                     landmarks, measured, noise, X(t0_us), C(0), K(0)
@@ -285,10 +280,8 @@ def apriltag_for_calibration_batch(
                 px = measured[i * 2 : (i + 1) * 2]
                 noise = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
                 self._new_factors.push_back(
-                    apriltag_calibrate.factor(
-                        landmark, px, noise, X(t0_us), C(0), K(0)
-                    )
-                )   
+                    apriltag_calibrate.factor(landmark, px, noise, X(t0_us), C(0), K(0))
+                )
 
     def keep_calib_hot(self, t0_us: int) -> None:
         """Even if we don't see any targets, remember the

raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py

@@ -23,11 +23,6 @@
     numericalDerivative33,
 )
 
-# camera "zero" is facing +z; this turns it to face +x
-CAM_COORD = gtsam.Pose3(
-    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
-    gtsam.Point3(0, 0, 0),  # type:ignore
-)
 
 
 def h_fn(
@@ -39,12 +34,7 @@ def h_fn(
 
     def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
         """Estimated pixel location of the target."""
-        # this is x-forward z-up
-        offset_pose = gtsam.Pose3(p0).compose(offset)
-        # this is z-forward y-down
-        camera_pose = offset_pose.compose(CAM_COORD)
-        print("camera_pose", camera_pose)
-        print("landmark", landmark)
+        camera_pose = gtsam.Pose3(p0).compose(offset) 
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
         return camera.project(landmark)
 
@@ -107,8 +97,8 @@ def factorNative(
     offset_key: int,
     calib_key: int,
 ) -> gtsam.NoiseModelFactor:
-    return gtsam.PlanarSFMFactor(
-        landmark, measured, model, p0_key, offset_key, calib_key
+    return gtsam.PlanarProjectionFactor3(
+        p0_key, offset_key, calib_key, landmark, measured, model
     )
 
 

raspberry_pi/app/pose_estimator/factors/apriltag_calibrate_batch.py

@@ -16,26 +16,17 @@
     numericalDerivative33,
 )
 
-# camera "zero" is facing +z; this turns it to face +x
-CAM_COORD = gtsam.Pose3(
-    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
-    gtsam.Point3(0, 0, 0),  # type:ignore
-)
-
 
 def h_fn(
     landmarks: list[np.ndarray],
 ) -> Callable[[gtsam.Pose2, gtsam.Pose3, gtsam.Cal3DS2], np.ndarray]:
     """Returns a pixel estimation function for an array of constant landmarks,
-    with variable pose, offset, and calibration. 
+    with variable pose, offset, and calibration.
     The landmark is the field position of a tag corner."""
 
     def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
         """Estimated pixel location of the target."""
-        # this is x-forward z-up
-        offset_pose = gtsam.Pose3(p0).compose(offset)
-        # this is z-forward y-down
-        camera_pose = offset_pose.compose(CAM_COORD)
+        camera_pose = gtsam.Pose3(p0).compose(offset)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
         # Camera.project() will throw CheiralityException sometimes
         # so be sure to use GTSAM_THROW_CHEIRALITY_EXCEPTION=OFF

raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py

@@ -17,12 +17,6 @@
 
 from app.pose_estimator.numerical_derivative import numericalDerivative11
 
-# camera "zero" is facing +z; this turns it to face +x
-CAM_COORD = gtsam.Pose3(
-    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
-    gtsam.Point3(0, 0, 0),  # type:ignore
-)
-
 
 def h_fn(
     landmark: np.ndarray,
@@ -35,13 +29,8 @@ def h_fn(
 
     def h(p0: gtsam.Pose2) -> np.ndarray:
         """Estimated pixel location of the target."""
-        # this is x-forward z-up
-        offset_pose = gtsam.Pose3(p0).compose(offset)
-        # this is z-forward y-down
-        camera_pose = offset_pose.compose(CAM_COORD)
+        camera_pose = gtsam.Pose3(p0).compose(offset)
         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).
@@ -103,8 +92,8 @@ def factorNative(
     model: SharedNoiseModel,
     p0_key: int,
 ) -> gtsam.NoiseModelFactor:
-    return gtsam.PlanarProjectionFactor(
-        landmark, measured, offset, calib, model, p0_key
+    return gtsam.PlanarProjectionFactor1(
+        p0_key, landmark, measured, offset, calib, model
     )
 
 

raspberry_pi/app/pose_estimator/factors/apriltag_smooth_batch.py

@@ -12,11 +12,6 @@
 
 from app.pose_estimator.numerical_derivative import numericalDerivative11
 
-# camera "zero" is facing +z; this turns it to face +x
-CAM_COORD = gtsam.Pose3(
-    gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
-    gtsam.Point3(0, 0, 0),  # type:ignore
-)
 
 
 def h_fn(
@@ -30,10 +25,7 @@ def h_fn(
 
     def h(p0: gtsam.Pose2) -> np.ndarray:
         """estimated pixel location of the target"""
-        # this is x-forward z-up
-        offset_pose = gtsam.Pose3(p0).compose(offset)
-        # this is z-forward y-down
-        camera_pose = offset_pose.compose(CAM_COORD)
+        camera_pose = gtsam.Pose3(p0).compose(offset)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
         # Camera.project() will throw CheiralityException sometimes
         # so be sure to use GTSAM_THROW_CHEIRALITY_EXCEPTION=OFF

raspberry_pi/tests/pose_estimator/calibrate_apriltag_test.py

@@ -37,6 +37,7 @@ def test_apriltag_0(self) -> None:
         self.assertEqual(3, est.result_size())
 
         p0: gtsam.Pose2 = est.mean_pose2(X(0))
+        print(p0)
         self.assertAlmostEqual(0, p0.x(), 3)
         self.assertAlmostEqual(0, p0.y(), 3)
         self.assertAlmostEqual(0, p0.theta(), 3)
@@ -48,14 +49,16 @@ def test_apriltag_0(self) -> None:
         self.assertAlmostEqual(0, c0.x(), 3)
         self.assertAlmostEqual(0, c0.y(), 3)
         self.assertAlmostEqual(0.6, c0.z(), 3)
-        self.assertAlmostEqual(0, c0.rotation().roll(), 3)
-        self.assertAlmostEqual(0, c0.rotation().pitch(), 3)
-        self.assertAlmostEqual(0, c0.rotation().yaw(), 3)
+        # these rotations match the offset
+        # which is z-fwd remember
+        rotVec = c0.rotation().xyz()
+        print("rotVec", rotVec)
+        self.assertTrue(np.allclose(np.array([-1.571, 0, -1.571]), rotVec, atol=0.001))
         sc0 = est.sigma(C(0))
-        print(sc0)
+        print("sc0", sc0)
         self.assertTrue(
             np.allclose(
-                np.array([0.094, 0.163, 0.191, 0.199, 0.194, 0.164]), sc0, atol=0.001
+                np.array([0.163, 0.191, 0.094, 0.194, 0.164, 0.199]), sc0, atol=0.001
             )
         )
 

raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py

@@ -34,7 +34,10 @@ def test_apriltag_1(self) -> None:
             landmarks,
             measured,
             0,
-            gtsam.Pose3(),
+            gtsam.Pose3(
+                gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+                gtsam.Point3(0, 0, 0),
+            ),
             gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1),
         )
         est.update()

raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py

@@ -20,7 +20,10 @@ class AprilTagCalibrateBatchTest(unittest.TestCase):
     def test_h_center_1(self) -> None:
         landmarks = [np.array([1, 0, 0]), np.array([1, 0, 0])]
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3()
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 0),  # type: ignore
+        )
         estimate_px: np.ndarray = apriltag_calibrate_batch.h_fn(landmarks)(
             p0, offset, KCAL
         )
@@ -43,7 +46,10 @@ def test_h_side_0(self) -> None:
         # second point is above the camera bore
         landmarks = [np.array([1, 0, 0]), np.array([1, 0, 1])]
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3()
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 0),  # type: ignore
+        )
         estimate_px: np.ndarray = apriltag_calibrate_batch.h_fn(landmarks)(
             p0, offset, KCAL
         )
@@ -66,7 +72,10 @@ def test_h_upper_left(self) -> None:
         # second point is above and to the left
         landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3()
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 0),  # type: ignore
+        )
         estimate_px: np.ndarray = apriltag_calibrate_batch.h_fn(landmarks)(
             p0, offset, KCAL
         )
@@ -92,7 +101,10 @@ def test_H_upper_left(self) -> None:
         measured = np.array([200, 200, 0, 0])
         landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3()
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 0),  # type: ignore
+        )
         H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))]
         err_px: np.ndarray = apriltag_calibrate_batch.h_H(
             landmarks, measured, p0, offset, KCAL, H
@@ -128,14 +140,15 @@ def test_H_upper_left(self) -> None:
                 atol=0.001,
             )
         )
+        # remember the camera is now z-fwd y-down
         self.assertTrue(
             np.allclose(
                 np.array(
                     [
-                        [0, 0, 200, 0, 200, 0],
-                        [0, -200, 0, 0, 0, 200],
-                        [-200, -440, 640, -360, 280, 80],
-                        [200, -640, 440, -360, 80, 280],
+                        [0, -200, 0, -200, 0, 0],
+                        [200, 0, 0, 0, -200, 0],
+                        [440, -640, -200, -280, -80, -360],
+                        [640, -440, 200, -80, -280, -360],
                     ]
                 ),
                 H[1],
@@ -171,7 +184,10 @@ def test_factor(self) -> None:
             ]
         )
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3()
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 0),  # type: ignore
+        )
         f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
             landmarks,
             measured,
@@ -213,7 +229,10 @@ def test_factor2(self) -> None:
             ]
         )
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 1]))
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 1),  # type: ignore
+        )
         f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
             landmarks,
             measured,
@@ -255,7 +274,12 @@ def test_factor3(self) -> None:
             ]
         )
         p0 = gtsam.Pose2()
-        offset = gtsam.Pose3(gtsam.Rot3.Pitch(-0.1), np.array([0, 0, 0]))
+        offset = gtsam.Pose3(gtsam.Rot3.Pitch(-0.1), np.array([0, 0, 0])).compose(
+            gtsam.Pose3(
+                gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+                gtsam.Point3(0, 0, 0),
+            )
+        )  # type: ignore
         f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
             landmarks,
             measured,
@@ -297,7 +321,11 @@ def test_factor4(self) -> None:
             ]
         )
         p0 = gtsam.Pose2(0.05, 0, 0)
-        offset = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 1]))
+
+        offset = gtsam.Pose3(
+            gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])),
+            gtsam.Point3(0, 0, 1),
+        )
         f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
             landmarks,
             measured,