From 73007fe0483c81f72b05220b8d0a45b8eb18a363 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jul 2020 19:24:38 -0400 Subject: [PATCH 1/6] test SmartFactor when body_P_sensor is passed in --- gtsam/slam/SmartFactorBase.h | 7 ++-- gtsam/slam/tests/testSmartFactorBase.cpp | 42 +++++++++++++++++++++--- 2 files changed, 41 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 717a9c1f2..3bedf599f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -206,11 +206,12 @@ protected: boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); if (body_P_sensor_ && Fs) { - const Pose3 sensor_P_body = body_P_sensor_->inverse(); + const Pose3 sensor_T_body = body_P_sensor_->inverse(); for (size_t i = 0; i < Fs->size(); i++) { - const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body; + const Pose3 world_T_body = cameras[i].pose() * sensor_T_body; Matrix J(6, 6); - const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + // Call compose to compute Jacobian + world_T_body.compose(*body_P_sensor_, J); Fs->at(i) = Fs->at(i) * J; } } diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index f69f4c113..bb04ad56d 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -37,11 +37,11 @@ class PinholeFactor: public SmartFactorBase > { public: typedef SmartFactorBase > Base; PinholeFactor() {} - PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { - } - virtual double error(const Values& values) const { - return 0.0; - } + PinholeFactor(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none, + size_t expectedNumberCameras = 10) + : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} + virtual double error(const Values& values) const { return 0.0; } virtual boost::shared_ptr linearize( const Values& values) const { return boost::shared_ptr(new JacobianFactor()); @@ -60,6 +60,38 @@ TEST(SmartFactorBase, Pinhole) { EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } +TEST(SmartFactorBase, PinholeWithSensor) { + Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0)); + PinholeFactor f = PinholeFactor(unit2, body_P_sensor); + EXPECT(assert_equal(f.body_P_sensor(), body_P_sensor)); + + PinholeFactor::Cameras cameras; + // Assume body at origin. + Pose3 world_T_body = Pose3(); + // Camera coordinates in world frame. + Pose3 wTc = world_T_body * body_P_sensor; + cameras.push_back(PinholeCamera(wTc)); + + // Simple point to project slightly off image center + Point3 p(0, 0, 10); + Point2 measurement = cameras[0].project(p); + f.add(measurement, 1); + + PinholeFactor::Cameras::FBlocks Fs; + Matrix E; + Vector error = f.unwhitenedError(cameras, p, Fs, E); + + Vector expectedError = Vector::Zero(2); + Matrix29 expectedFs; + expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0; + Matrix23 expectedE; + expectedE << 0.1, 0, 0.01, 0, 0.1, 0; + + EXPECT(assert_equal(error, expectedError)); + EXPECT(assert_equal(expectedFs, Fs[0])); // We only have the jacobian for the 1 camera + EXPECT(assert_equal(expectedE, E)); +} + /* ************************************************************************* */ #include From 0c199dd406fde5b27123b762b073a1e8ac98f20a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 00:46:21 -0400 Subject: [PATCH 2/6] revert variable change --- gtsam/slam/SmartFactorBase.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3bedf599f..c9b510605 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -206,12 +206,12 @@ protected: boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); if (body_P_sensor_ && Fs) { - const Pose3 sensor_T_body = body_P_sensor_->inverse(); + const Pose3 sensor_P_body = body_P_sensor_->inverse(); for (size_t i = 0; i < Fs->size(); i++) { - const Pose3 world_T_body = cameras[i].pose() * sensor_T_body; + const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; Matrix J(6, 6); // Call compose to compute Jacobian - world_T_body.compose(*body_P_sensor_, J); + world_P_body.compose(*body_P_sensor_, J); Fs->at(i) = Fs->at(i) * J; } } From 7dfc79971aa26a203aa9ef0bacb042a3955994b9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 11:52:06 -0400 Subject: [PATCH 3/6] reduced tolerance for checking jacobian --- gtsam/slam/tests/testSmartFactorBase.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index bb04ad56d..fd771f102 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -67,9 +67,9 @@ TEST(SmartFactorBase, PinholeWithSensor) { PinholeFactor::Cameras cameras; // Assume body at origin. - Pose3 world_T_body = Pose3(); + Pose3 world_P_body = Pose3(); // Camera coordinates in world frame. - Pose3 wTc = world_T_body * body_P_sensor; + Pose3 wTc = world_P_body * body_P_sensor; cameras.push_back(PinholeCamera(wTc)); // Simple point to project slightly off image center @@ -88,7 +88,9 @@ TEST(SmartFactorBase, PinholeWithSensor) { expectedE << 0.1, 0, 0.01, 0, 0.1, 0; EXPECT(assert_equal(error, expectedError)); - EXPECT(assert_equal(expectedFs, Fs[0])); // We only have the jacobian for the 1 camera + // We only have the jacobian for the 1 camera + // Use of a lower tolerance value due to compiler precision mismatch. + EXPECT(assert_equal(expectedFs, Fs[0], 1e-3)); EXPECT(assert_equal(expectedE, E)); } From f23587fafd9ceb388eeef6e4cec0c685dcf4be2d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 20:02:09 -0400 Subject: [PATCH 4/6] Add indentation --- gtsam/geometry/PinholePose.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 60088577c..79dbb9ce9 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -107,9 +107,9 @@ public: // If needed, apply chain rule if (Dpose) - *Dpose = Dpi_pn * *Dpose; + *Dpose = Dpi_pn * *Dpose; if (Dpoint) - *Dpoint = Dpi_pn * *Dpoint; + *Dpoint = Dpi_pn * *Dpoint; return pi; } From 60c88f67e9b5271d867efd16322080e6e02236ec Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 20:02:30 -0400 Subject: [PATCH 5/6] Handle extrinsics and intrinsics for jacobian --- gtsam/slam/SmartFactorBase.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index c9b510605..912de0bc1 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -207,11 +207,17 @@ protected: Vector ue = cameras.reprojectionError(point, measured_, Fs, E); if (body_P_sensor_ && Fs) { const Pose3 sensor_P_body = body_P_sensor_->inverse(); + size_t camera_dim = size_t(traits::dimension); + size_t pose_dim = size_t(traits::dimension); + for (size_t i = 0; i < Fs->size(); i++) { const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; - Matrix J(6, 6); - // Call compose to compute Jacobian - world_P_body.compose(*body_P_sensor_, J); + Matrix J = Matrix::Zero(camera_dim, camera_dim); + // Call compose to compute Jacobian for camera extrinsics + Matrix H(pose_dim, pose_dim); + world_P_body.compose(*body_P_sensor_, H); + // Assign extrinsics + J.block(0, 0, pose_dim, pose_dim) = H; Fs->at(i) = Fs->at(i) * J; } } From 7259f899d9a37b575853cc2d0ce415e6966d6c6e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Jul 2020 09:26:48 -0400 Subject: [PATCH 6/6] Use static matrix and constexpr --- gtsam/slam/SmartFactorBase.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 912de0bc1..1c80b8744 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -207,17 +207,18 @@ protected: Vector ue = cameras.reprojectionError(point, measured_, Fs, E); if (body_P_sensor_ && Fs) { const Pose3 sensor_P_body = body_P_sensor_->inverse(); - size_t camera_dim = size_t(traits::dimension); - size_t pose_dim = size_t(traits::dimension); + constexpr int camera_dim = traits::dimension; + constexpr int pose_dim = traits::dimension; for (size_t i = 0; i < Fs->size(); i++) { const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; - Matrix J = Matrix::Zero(camera_dim, camera_dim); + Eigen::Matrix J; + J.setZero(); + Eigen::Matrix H; // Call compose to compute Jacobian for camera extrinsics - Matrix H(pose_dim, pose_dim); world_P_body.compose(*body_P_sensor_, H); - // Assign extrinsics - J.block(0, 0, pose_dim, pose_dim) = H; + // Assign extrinsics part of the Jacobian + J.template block(0, 0) = H; Fs->at(i) = Fs->at(i) * J; } }