From 4deda04a3a22c26cf5bd76b2c87bac6538a26ae6 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 23 Dec 2024 19:52:26 -0800 Subject: [PATCH] update python to match z-fwd camera --- raspberry_pi/app/config/camera_config.py | 13 ++-- raspberry_pi/app/pose_estimator/calibrate.py | 27 +++----- .../factors/apriltag_calibrate.py | 16 +---- .../factors/apriltag_calibrate_batch.py | 13 +--- .../pose_estimator/factors/apriltag_smooth.py | 17 +---- .../factors/apriltag_smooth_batch.py | 10 +-- .../pose_estimator/calibrate_apriltag_test.py | 13 ++-- .../estimate_apriltag_batch_test.py | 5 +- .../factors/apriltag_calibrate_batch_test.py | 52 +++++++++++---- .../factors/apriltag_calibrate_test.py | 65 ++++++++++++------- .../factors/apriltag_smooth_batch_test.py | 39 ++++++++--- .../factors/apriltag_smooth_test.py | 38 ++++++++--- .../parking_lot/estimate_apriltag_test.py | 13 ++-- .../simulation/circle_simulator.py | 13 +--- .../simulation/circle_simulator_test.py | 5 +- 15 files changed, 190 insertions(+), 149 deletions(-) diff --git a/raspberry_pi/app/config/camera_config.py b/raspberry_pi/app/config/camera_config.py index 87e865e38..bf76e2feb 100644 --- a/raspberry_pi/app/config/camera_config.py +++ b/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) diff --git a/raspberry_pi/app/pose_estimator/calibrate.py b/raspberry_pi/app/pose_estimator/calibrate.py index 6e34107ad..af0e5f532 100644 --- a/raspberry_pi/app/pose_estimator/calibrate.py +++ b/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 diff --git a/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py b/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py index 6a103362d..0f8894c62 100644 --- a/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py +++ b/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 ) diff --git a/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate_batch.py b/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate_batch.py index 4ee094d16..34d280319 100644 --- a/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate_batch.py +++ b/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 diff --git a/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py b/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py index 4ec9bae2d..d0cd697db 100644 --- a/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py +++ b/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 ) diff --git a/raspberry_pi/app/pose_estimator/factors/apriltag_smooth_batch.py b/raspberry_pi/app/pose_estimator/factors/apriltag_smooth_batch.py index aefdc6fe6..8e1ce7131 100644 --- a/raspberry_pi/app/pose_estimator/factors/apriltag_smooth_batch.py +++ b/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 diff --git a/raspberry_pi/tests/pose_estimator/calibrate_apriltag_test.py b/raspberry_pi/tests/pose_estimator/calibrate_apriltag_test.py index d674f231c..d993c790e 100644 --- a/raspberry_pi/tests/pose_estimator/calibrate_apriltag_test.py +++ b/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 ) ) diff --git a/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py b/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py index cbde24fb6..dc9e3ecc4 100644 --- a/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py +++ b/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() diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py index e1f8c3e3e..cd6ed65bc 100644 --- a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py +++ b/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, diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py index 51b43ae1f..46876f723 100644 --- a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py +++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py @@ -19,11 +19,12 @@ class AprilTagCalibrateTest(unittest.TestCase): def test_h_center_1(self) -> None: landmark = np.array([1, 0, 0]) p0 = gtsam.Pose2() - offset = gtsam.Pose3() - KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) - estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)( - p0, offset, KCAL + offset = gtsam.Pose3( + gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), + gtsam.Point3(0, 0, 0), # type: ignore ) + KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) + estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)(p0, offset, KCAL) # landmark on the camera bore, so it's at (cx, cy) self.assertTrue( np.allclose( @@ -41,11 +42,12 @@ def test_h_side_0(self) -> None: # higher than the camera landmark = np.array([1, 0, 1]) p0 = gtsam.Pose2() - offset = gtsam.Pose3() - KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) - estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)( - p0, offset, KCAL + offset = gtsam.Pose3( + gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), + gtsam.Point3(0, 0, 0), # type: ignore ) + KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) + estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)(p0, offset, KCAL) # landmark above the camera bore, so the 'y' value is less self.assertTrue( np.allclose( @@ -63,11 +65,12 @@ def test_h_upper_left(self) -> None: # higher than the camera landmark = np.array([1, 1, 1]) p0 = gtsam.Pose2() - offset = gtsam.Pose3() - KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) - estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)( - p0, offset, KCAL + offset = gtsam.Pose3( + gtsam.Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), + gtsam.Point3(0, 0, 0), # type: ignore ) + KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) + estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)(p0, offset, KCAL) # above and to the left, so both x and y are less. # (coincidentally right on the edge) print("estimate: ", estimate_px) @@ -88,7 +91,10 @@ def test_H_upper_left(self) -> None: measured = np.array([0, 0]) landmark = 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 + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))] err_px: np.ndarray = apriltag_calibrate.h_H( @@ -121,12 +127,13 @@ def test_H_upper_left(self) -> None: atol=0.001, ) ) + # remember camera is z-fwd now self.assertTrue( np.allclose( np.array( [ - [-200, -440, 640, -360, 280, 80], - [200, -640, 440, -360, 80, 280], + [440, -640, -200, -280, -80, -360], + [640, -440, 200, -80, -280, -360], ] ), H[1], @@ -151,7 +158,10 @@ def test_H_upper_left2(self) -> None: measured = np.array([0, 0]) landmark = 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 + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, 0, 0) H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))] err_px: np.ndarray = apriltag_calibrate.h_H( @@ -188,8 +198,8 @@ def test_H_upper_left2(self) -> None: np.allclose( np.array( [ - [-200, -200, 400, -200, 200, 0], - [200, -400, 200, -200, 0, 200], + [200, -400, -200, -200, 0, -200], + [400, -200, 200, 0, -200, -200], ] ), H[1], @@ -215,7 +225,12 @@ def test_H_upper_left3(self) -> None: landmark = np.array([1, 1, 1]) p0 = gtsam.Pose2() # Remember Ypr is intrinsic - offset = gtsam.Pose3(gtsam.Rot3().Ypr(1, -1, 0), np.array([0, 0, 0])) + offset = gtsam.Pose3(gtsam.Rot3().Ypr(1, -1, 0), 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 print("OFFSET", offset) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, 0, 0) H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))] @@ -233,7 +248,7 @@ def test_H_upper_left3(self) -> None: ] ), err_px, - atol=1 + atol=1, ) ) print("H[0]:\n", H[0]) @@ -251,12 +266,13 @@ def test_H_upper_left3(self) -> None: atol=1, ) ) + # camera is z-fwd self.assertTrue( np.allclose( np.array( [ - [78, -14, 207, 24, 126, 0], - [-38, -230, 15, 49, 0, 126], + [14, -207, 78, -126, 0, 24], + [231, -14, -38, 0, -126, 49], ] ), H[1], @@ -281,7 +297,10 @@ def test_factor(self) -> None: measured = np.array([0, 0]) landmark = np.array([1, 1, 1]) p0 = gtsam.Pose2() - offset = gtsam.Pose3() + # camera is now z-forward y-down + offset = gtsam.Pose3( + gtsam.Rot3(0, 0, 1, -1, 0, 0, 0, -1, 0), np.array([0, 0, 0]) + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) f: gtsam.NoiseModelFactor = apriltag_calibrate.factor( landmark, diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py index 705f86d61..bc6857d2b 100644 --- a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py +++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py @@ -19,7 +19,10 @@ class AprilTagSmoothBatchTest(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_smooth_batch.h_fn(landmarks, offset, KCAL)( p0 ) @@ -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_smooth_batch.h_fn(landmarks, offset, KCAL)( p0 ) @@ -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_smooth_batch.h_fn(landmarks, offset, KCAL)( p0 ) @@ -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))] err_px: np.ndarray = apriltag_smooth_batch.h_H( landmarks, measured, p0, offset, KCAL, H @@ -134,7 +146,10 @@ def test_factor(self) -> None: np.array([1, 1, 1]), # upper left corner ] measured = np.array([200, 200, 0, 0]) - 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_smooth_batch.factor( landmarks, measured, @@ -177,7 +192,10 @@ def test_factor2(self) -> None: 200, # center ] ) - 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_smooth_batch.factor( landmarks, measured, @@ -220,7 +238,10 @@ def test_factor3(self) -> None: 49, ] ) - 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_smooth_batch.factor( landmarks, measured, @@ -263,7 +284,9 @@ def test_factor4(self) -> None: 200, # center ] ) - 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_smooth_batch.factor( landmarks, measured, diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py index 0fb003484..72d1d6a61 100644 --- a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py +++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py @@ -10,11 +10,15 @@ import app.pose_estimator.factors.apriltag_smooth as apriltag_smooth + class AprilTagSmoothTest(unittest.TestCase): def test_h_center_1(self) -> None: landmark = 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 + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) estimate_px: np.ndarray = apriltag_smooth.h_fn(landmark, offset, KCAL)(p0) # landmark on the camera bore, so it's at (cx, cy) @@ -34,7 +38,10 @@ def test_h_side_0(self) -> None: # higher than the camera landmark = 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 + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) estimate_px: np.ndarray = apriltag_smooth.h_fn(landmark, offset, KCAL)(p0) # landmark above the camera bore, so the 'y' value is less @@ -54,7 +61,10 @@ def test_h_upper_left(self) -> None: # higher than the camera landmark = 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 + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) estimate_px: np.ndarray = apriltag_smooth.h_fn(landmark, offset, KCAL)(p0) # above and to the left, so both x and y are less. @@ -77,7 +87,10 @@ def test_H_center(self) -> None: # as above but with jacobians landmark = np.array([1, 0, 0]) measured = np.array([200, 200]) - 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 + ) p0 = gtsam.Pose2() # target on bore, distortion has no impact. KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) @@ -121,7 +134,10 @@ def test_H_upper_left(self) -> None: # as above but with jacobians landmark = np.array([1, 1, 1]) measured = np.array([0, 0]) - 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 + ) p0 = gtsam.Pose2() # distortion KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) @@ -167,7 +183,10 @@ def test_H_upper_left2(self) -> None: # as above but with jacobians landmark = np.array([1, 1, 1]) measured = np.array([0, 0]) - 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 + ) p0 = gtsam.Pose2() # no distortion KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, 0, 0) @@ -194,7 +213,7 @@ def test_H_upper_left2(self) -> None: # du/dtheta = + (as above) # dv/dx = - (dolly fwd => px goes up) # dv/dy = 0 (just truck) - # dv/dtheta = + + # dv/dtheta = + self.assertTrue( np.allclose( np.array( @@ -213,7 +232,10 @@ def test_factor(self) -> None: measured = np.array([0, 0]) landmark = np.array([1, 1, 1]) p0 = gtsam.Pose2() - offset = gtsam.Pose3() + # camera is now z-forward y-down + offset = gtsam.Pose3( + gtsam.Rot3(0, 0, 1, -1, 0, 0, 0, -1, 0), np.array([0, 0, 0]) + ) KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) noise = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1])) f: gtsam.NoiseModelFactor = apriltag_smooth.factor( diff --git a/raspberry_pi/tests/pose_estimator/parking_lot/estimate_apriltag_test.py b/raspberry_pi/tests/pose_estimator/parking_lot/estimate_apriltag_test.py index e847c1b72..d83cd8480 100644 --- a/raspberry_pi/tests/pose_estimator/parking_lot/estimate_apriltag_test.py +++ b/raspberry_pi/tests/pose_estimator/parking_lot/estimate_apriltag_test.py @@ -29,13 +29,12 @@ def test_apriltag_1(self) -> None: # upper left, this is from apriltag_calibrate_test. landmark = np.array([1, 1, 1]) measured = np.array([0, 0]) - est.apriltag_for_smoothing( - landmark, - measured, - 0, - gtsam.Pose3(), - gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1), + calib = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) + # camera is now z-forward y-down + offset = gtsam.Pose3( + gtsam.Rot3(0, 0, 1, -1, 0, 0, 0, -1, 0), np.array([0, 0, 0]) ) + est.apriltag_for_smoothing(landmark, measured, 0, offset, calib) est.update() self.assertEqual(1, est.result_size()) p0: gtsam.Pose2 = est.mean_pose2(X(0)) @@ -67,6 +66,6 @@ def test_apriltag_1(self) -> None: [0.09, 0.09, 0.09], ), s0, - atol=0.01 + atol=0.01, ) ) diff --git a/raspberry_pi/tests/pose_estimator/simulation/circle_simulator.py b/raspberry_pi/tests/pose_estimator/simulation/circle_simulator.py index 5bf90bc0c..7e157f1e1 100644 --- a/raspberry_pi/tests/pose_estimator/simulation/circle_simulator.py +++ b/raspberry_pi/tests/pose_estimator/simulation/circle_simulator.py @@ -48,11 +48,6 @@ PAN_PERIOD_S = PATH_PERIOD_S / 3 # maximum pan angle, radians PAN_SCALE_RAD = 1.0 -# camera "zero" is facing +z; this turns it to face +x -CAM_COORD = Pose3( - Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), - Point3(0, 0, 0), # type: ignore -) class CircleSimulator: @@ -192,12 +187,6 @@ def _px( # type: ignore ) -> np.ndarray: """Project the landmark point into the camera frame and return (x, y) in pixels. Robot_pose and camera_offset are x-forward, z-up.""" - # ctor a pose3 with x,y,yaw, x-forward, z-up - offset_pose = Pose3(robot_pose).compose(camera_offset) # type: ignore - # print("offset pose ", offset_pose) - camera_pose = offset_pose.compose(CAM_COORD) # type: ignore - # camera constructor expects z-forward y-down - # print("camera pose ", camera_pose) - # print("landmark ", landmark) + camera_pose = Pose3(robot_pose).compose(camera_offset) # type: ignore camera = PinholeCameraCal3DS2(camera_pose, calib) return camera.project(landmark) # type: ignore diff --git a/raspberry_pi/tests/pose_estimator/simulation/circle_simulator_test.py b/raspberry_pi/tests/pose_estimator/simulation/circle_simulator_test.py index a3e7adebd..3c272f6d7 100644 --- a/raspberry_pi/tests/pose_estimator/simulation/circle_simulator_test.py +++ b/raspberry_pi/tests/pose_estimator/simulation/circle_simulator_test.py @@ -58,7 +58,10 @@ def test_camera(self) -> None: # this is the lower right corner landmark = Point3(4, -(0.1651 / 2.0), 1 - (0.1651 / 2)) robot_pose = Pose2(2, 0, 0) - camera_offset = Pose3(Rot3(), np.array([0, 0, 1])) + camera_offset = Pose3( + Rot3(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]])), + Point3(0, 0, 1), # type: ignore + ) calib = Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1) px: np.ndarray = sim._px(landmark, robot_pose, camera_offset, calib) # pixel should be in the lower right quadrant