From e4538d5b3e4ecddfabc8c8d81dc09553772f7147 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 17 Dec 2024 22:39:13 -0800 Subject: [PATCH] address some comments --- gtsam/geometry/Pose3.cpp | 11 ++ gtsam/geometry/Pose3.h | 2 + gtsam/slam/PlanarProjectionFactor.h | 164 +++++++++--------- gtsam/slam/slam.i | 36 ++-- .../slam/tests/testPlanarProjectionFactor.cpp | 24 ++- 5 files changed, 130 insertions(+), 107 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bb1483432f..5d77329476 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -42,6 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } +Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { + if (H) *H << (gtsam::Matrix(6, 3) << // + 0., 0., 0., // + 0., 0., 0.,// + 0., 0., 1.,// + 1., 0., 0.,// + 0., 1., 0.,// + 0., 0., 0.).finished(); + return Pose3(p); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c803..baac21ccb1 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -78,6 +78,8 @@ class GTSAM_EXPORT Pose3: public LieGroup { OptionalJacobian<6, 3> HR = {}, OptionalJacobian<6, 3> Ht = {}); + static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {}); + /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 285b649001..21f24da93c 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -38,69 +38,67 @@ namespace gtsam { PlanarProjectionFactorBase() {} /** - * @param measured corresponding point in the camera frame - * @param poseKey index of the robot pose in the z=0 plane + * @param measured pixels in the camera frame */ PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {} /** - * @param landmark point3 - * @param pose - * @param offset oriented parallel to pose2 zero i.e. +x - * @param calib - * @param H1 landmark jacobian - * @param H2 pose jacobian - * @param H3 offset jacobian - * @param H4 calib jacobian + * Predict the projection of the landmark in camera pixels. + * + * @param landmark point3 of the target + * @param wTb "world to body": planar pose2 of vehicle body frame in world frame + * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x + * @param calib camera calibration with distortion + * @param Hlandmark jacobian + * @param HwTb jacobian + * @param HbTc jacobian + * @param Hcalib jacobian */ - Point2 h( + Point2 predict( const Point3& landmark, - const Pose2& pose, - const Pose3& offset, + const Pose2& wTb, + const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType H1, // 2x3 (x, y, z) - OptionalMatrixType H2, // 2x3 (x, y, theta) - OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta) - OptionalMatrixType H4 // 2x9 + OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z) + OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta) + OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta) + OptionalMatrixType Hcalib = nullptr // 2x9 ) const { +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION try { - // this is x-forward z-up +#endif + gtsam::Matrix Hp; // 6x3 gtsam::Matrix H0; // 6x6 - Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is x-forward z-up + Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr); // this is z-forward y-down gtsam::Matrix H00; // 6x6 - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + Pose3 camera_pose = wTc.compose(CAM_COORD, H00); PinholeCamera camera = PinholeCamera(camera_pose, calib); - if (H2 || H3) { + if (HwTb || HbTc) { // Dpose is for pose3, 2x6 (R,t) gtsam::Matrix Dpose; - Point2 result = camera.project(landmark, Dpose, H1, H4); + Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); gtsam::Matrix DposeOffset = Dpose * H00; // 2x6 - if (H3) - *H3 = DposeOffset; // with Eigen this is a deep copy (!) - if (H2) { - gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; - *H2 = Matrix::Zero(2, 3); - (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx - (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx - (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy - (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy - (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw - (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw - } + if (HbTc) + *HbTc = DposeOffset; // with Eigen this is a deep copy (!) + if (HwTb) + *HwTb = DposeOffset * H0 * Hp; return result; } else { return camera.project(landmark, {}, {}, {}); } +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION } catch (CheiralityException& e) { std::cout << "****** CHIRALITY EXCEPTION ******\n"; - if (H1) *H1 = Matrix::Zero(2, 3); - if (H2) *H2 = Matrix::Zero(2, 3); - if (H3) *H3 = Matrix::Zero(2, 6); - if (H4) *H4 = Matrix::Zero(2, 9); + if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3); + if (HwTb) *HwTb = Matrix::Zero(2, 3); + if (HbTc) *HbTc = Matrix::Zero(2, 6); + if (Hcalib) *Hcalib = Matrix::Zero(2, 9); // return a large error return Matrix::Constant(2, 1, 2.0 * calib.fx()); } +#endif } Point2 measured_; // pixel measurement @@ -140,41 +138,42 @@ namespace gtsam { /** + * @brief constructor for known landmark, offset, and calibration + * @param poseKey index of the robot pose2 in the z=0 plane * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame - * @param offset constant camera offset from pose + * @param bTc "body to camera": constant camera offset from pose * @param calib constant camera calibration * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose2 in the z=0 plane */ PlanarProjectionFactor1( + Key poseKey, const Point3& landmark, const Point2& measured, - const Pose3& offset, + const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model, - Key poseKey) + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey), landmark_(landmark), - offset_(offset), + bTc_(bTc), calib_(calib) { assert(2 == model->dim()); } /** - * @param pose estimated pose2 - * @param H1 pose jacobian + * @param wTb "world to body": estimated pose2 + * @param HwTb jacobian */ Vector evaluateError( - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone) const override { - return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_; + const Pose2& wTb, + OptionalMatrixType HwTb = OptionalNone) const override { + return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; } private: Point3 landmark_; // landmark - Pose3 offset_; // camera offset to robot pose + Pose3 bTc_; // "body to camera": camera offset to robot pose Cal3DS2 calib_; // camera calibration }; @@ -188,9 +187,10 @@ namespace gtsam { * Camera offset and calibration are constant. * This is similar to GeneralSFMFactor, used for SLAM. */ - class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { public: - typedef NoiseModelFactorN Base; + typedef NoiseModelFactorN Base; using Base::evaluateError; PlanarProjectionFactor2() {} @@ -204,43 +204,44 @@ namespace gtsam { } /** + * @brief constructor for variable landmark, known offset and calibration + * @param poseKey index of the robot pose2 in the z=0 plane + * @param landmarkKey index of the landmark point3 * @param measured corresponding point in the camera frame - * @param offset constant camera offset from pose + * @param bTc "body to camera": constant camera offset from pose * @param calib constant camera calibration * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose2 in the z=0 plane - * @param landmarkKey index of the landmark point3 */ PlanarProjectionFactor2( + Key poseKey, + Key landmarkKey, const Point2& measured, - const Pose3& offset, + const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model, - Key landmarkKey, - Key poseKey) + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, landmarkKey, poseKey), - offset_(offset), + bTc_(bTc), calib_(calib) { assert(2 == model->dim()); } /** + * @param wTb "world to body": estimated pose2 * @param landmark estimated landmark - * @param pose estimated pose2 - * @param H1 landmark jacobian - * @param H2 pose jacobian + * @param HwTb jacobian + * @param Hlandmark jacobian */ Vector evaluateError( + const Pose2& wTb, const Point3& landmark, - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const override { - return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_; + OptionalMatrixType HwTb = OptionalNone, + OptionalMatrixType Hlandmark = OptionalNone) const override { + return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; } private: - Pose3 offset_; // camera offset to robot pose + Pose3 bTc_; // "body to camera": camera offset to robot pose Cal3DS2 calib_; // camera calibration }; @@ -270,6 +271,7 @@ namespace gtsam { } /** + * @brief constructor for variable pose, offset, and calibration, known landmark. * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame * @param model stddev of the measurements, ~one pixel? @@ -277,12 +279,12 @@ namespace gtsam { * @param offsetKey index of camera offset from pose * @param calibKey index of camera calibration */ PlanarProjectionFactor3( - const Point3& landmark, - const Point2& measured, - const SharedNoiseModel& model, Key poseKey, Key offsetKey, - Key calibKey) + Key calibKey, + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey, offsetKey, calibKey), landmark_(landmark) { @@ -290,21 +292,21 @@ namespace gtsam { } /** - * @param pose estimated pose2 - * @param offset pose3 offset from pose2 +x + * @param wTb "world to body": estimated pose2 + * @param bTc "body to camera": pose3 offset from pose2 +x * @param calib calibration - * @param H1 pose jacobian + * @param HwTb pose jacobian * @param H2 offset jacobian * @param H3 calibration jacobian */ Vector evaluateError( - const Pose2& pose, - const Pose3& offset, + const Pose2& wTb, + const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone) const override { - return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_; + OptionalMatrixType HwTb = OptionalNone, + OptionalMatrixType HbTc = OptionalNone, + OptionalMatrixType Hcalib = OptionalNone) const override { + return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; } private: diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e141cb50c7..1761a6cc35 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -27,32 +27,32 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { PlanarProjectionFactor1( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::Pose3& offset, - const gtsam::Cal3DS2& calib, - const gtsam::noiseModel::Base* model, - size_t poseKey); + size_t poseKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); void serialize() const; }; virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor { PlanarProjectionFactor2( - const gtsam::Point2& measured, - const gtsam::Pose3& offset, - const gtsam::Cal3DS2& calib, - const gtsam::noiseModel::Base* model, - size_t landmarkKey, - size_t poseKey); + size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); void serialize() const; }; virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { PlanarProjectionFactor3( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, - size_t poseKey, - size_t offsetKey, - size_t calibKey); + size_t poseKey, + size_t offsetKey, + size_t calibKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model); void serialize() const; }; diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 03a0b7bed0..67ebd5c256 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -39,7 +39,8 @@ TEST(PlanarProjectionFactor1, error1) { Pose2 pose(0, 0, 0); values.insert(X(0), pose); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -65,7 +66,8 @@ TEST(PlanarProjectionFactor1, error2) { Pose3 offset; Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Values values; Pose2 pose(0, 0, 0); @@ -96,7 +98,8 @@ TEST(PlanarProjectionFactor1, error3) { // distortion Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Values values; Pose2 pose(0, 0, 0); @@ -131,7 +134,8 @@ TEST(PlanarProjectionFactor1, jacobian) { Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Pose2 pose(s(g), s(g), s(g)); @@ -164,7 +168,8 @@ TEST(PlanarProjectionFactor3, error1) { values.insert(C(0), offset); values.insert(K(0), calib); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -213,7 +218,8 @@ TEST(PlanarProjectionFactor3, error2) { Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Values values; Pose2 pose(0, 0, 0); @@ -267,7 +273,8 @@ TEST(PlanarProjectionFactor3, error3) { Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Values values; Pose2 pose(0, 0, 0); @@ -328,7 +335,8 @@ TEST(PlanarProjectionFactor3, jacobian) { Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Pose2 pose(s(g), s(g), s(g));