From 5f667b44255b0e6181b620833ba8097f596cd0b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 17:24:13 -0500 Subject: [PATCH] Rename factor --- gtsam/sfm/TransferFactor.h | 18 ++++++++++++++++++ gtsam/sfm/sfm.i | 7 +++++-- gtsam/sfm/tests/testTransferFactor.cpp | 2 +- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index b73a71aa4f..0338762be2 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -253,6 +253,24 @@ class EssentialTransferFactorK TransferEdges(edge1, edge2), triplets_(triplets) {} + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @note Calibrations are assumed all same, using given key `keyK`. + * + * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) + * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) + * @param keyK Calibration key for all views. + * @param triplets A vector of triplets containing (pa, pb, pc) + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2, keyK, keyK, keyK), + TransferEdges(edge1, edge2), + triplets_(triplets) {} + /// Transfer points pa and pb to view c and evaluate error. Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa, const Matrix3& Ecb, const K& Kb, const Point2& pb, diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index a15b73ea19..24f24f1bb4 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets, - const gtsam::noiseModel::Base* model = nullptr); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index ced3d2ce7c..b34bdf8327 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) { } //************************************************************************* -// Test for EssentialTransferFactor +// Test for EssentialTransferFactorK TEST(EssentialTransferFactor, Jacobians) { using namespace fixture;