From 28eae9af3919aafaa6b2afd1cb5d88bbb324a633 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 8 Sep 2024 14:55:10 -0400 Subject: [PATCH 1/7] add the bata translation factor, with unit tests --- gtsam/sfm/TranslationFactor.h | 76 ++++- gtsam/sfm/TranslationRecovery.cpp | 18 +- gtsam/sfm/TranslationRecovery.h | 8 +- gtsam/sfm/sfm.i | 7 +- gtsam/sfm/tests/testTranslationFactor.cpp | 75 ++++- tests/testTranslationRecovery.cpp | 334 ++++++++++++++++++++++ 6 files changed, 506 insertions(+), 12 deletions(-) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 8850d7d2d0..ccd0b03ffb 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -18,6 +18,7 @@ * @brief Binary factor for a relative translation direction measurement. */ +#include #include #include #include @@ -36,8 +37,6 @@ namespace gtsam { * normalized(Tb - Ta) - w_aZb.point3() * * @ingroup sfm - * - * */ class TranslationFactor : public NoiseModelFactorN { private: @@ -56,7 +55,7 @@ class TranslationFactor : public NoiseModelFactorN { * @brief Caclulate error: (norm(Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is * the Unit3 translation direction from a to b. - * + * * @param Ta translation for key a * @param Tb translation for key b * @param H1 optional jacobian in Ta @@ -69,7 +68,8 @@ class TranslationFactor : public NoiseModelFactorN { boost::optional H2 = boost::none) const override { const Point3 dir = Tb - Ta; Matrix33 H_predicted_dir; - const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); + const Point3 predicted = + normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); if (H1) *H1 = -H_predicted_dir; if (H2) *H2 = H_predicted_dir; return predicted - measured_w_aZb_; @@ -83,4 +83,72 @@ class TranslationFactor : public NoiseModelFactorN { "Base", boost::serialization::base_object(*this)); } }; // \ TranslationFactor + +/** + * Binary factor for a relative translation direction measurement + * w_aZb. The measurement equation is + * w_aZb = scale * (Tb - Ta) + * i.e., w_aZb is the translation direction from frame A to B, in world + * coordinates. + * + * Instead of normalizing the Tb - Ta vector, we multiply it by a scalar + * which is also jointly optimized. This is inspired by the work on + * BATA (Zhuang et al, CVPR 2018). + * + * @ingroup sfm + */ +class BilinearAngleTranslationFactor + : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + Point3 measured_w_aZb_; + + public: + /// default constructor + BilinearAngleTranslationFactor() {} + + BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, const Unit3& w_aZb, + const SharedNoiseModel& noiseModel) + : Base(noiseModel, a, b, scale_key), + measured_w_aZb_(w_aZb.point3()) {} + + /** + * @brief Caclulate error: (scale * (Tb - Ta) - measurement) + * where Tb and Ta are Point3 translations and measurement is + * the Unit3 translation direction from a to b. + * + * @param Ta translation for key a + * @param Tb translation for key b + * @param H1 optional jacobian in Ta + * @param H2 optional jacobian in Tb + * @return * Vector + */ + Vector evaluateError( + const Point3& Ta, const Point3& Tb, const Vector1& scale, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override { + // Ideally we should use a positive real valued scalar datatype for scale. + const double abs_scale = abs(scale[0]); + const Point3 predicted = (Tb - Ta) * abs_scale; + if (H1) { + *H1 = -Matrix3::Identity() * abs_scale; + } + if (H2) { + *H2 = Matrix3::Identity() * abs_scale; + } + if (H3) { + *H3 = scale[0] >= 0 ? (Tb - Ta) : (Ta - Tb); + } + return predicted - measured_w_aZb_; + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ BilinearAngleTranslationFactor } // namespace gtsam diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index c7ef2e526d..4c7bb24da7 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -101,9 +101,17 @@ NonlinearFactorGraph TranslationRecovery::buildGraph( NonlinearFactorGraph graph; // Add translation factors for input translation directions. + uint i = 0; for (auto edge : relativeTranslations) { - graph.emplace_shared(edge.key1(), edge.key2(), - edge.measured(), edge.noiseModel()); + if (use_bilinear_translation_factor_) { + graph.emplace_shared( + edge.key1(), edge.key2(), Symbol('S', i), edge.measured(), + edge.noiseModel()); + } else { + graph.emplace_shared( + edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); + } + i++; } return graph; } @@ -163,6 +171,12 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + + if (use_bilinear_translation_factor_) { + for (uint i = 0; i < relativeTranslations.size(); i++) { + initial.insert(Symbol('S', i), Vector1(1.0)); + } + } return initial; } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 44a5ef43ef..e5b8f106f5 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -60,14 +60,18 @@ class TranslationRecovery { // Parameters. LevenbergMarquardtParams lmParams_; + const bool use_bilinear_translation_factor_ = false; + public: /** * @brief Construct a new Translation Recovery object * * @param lmParams parameters for optimization. */ - TranslationRecovery(const LevenbergMarquardtParams &lmParams) - : lmParams_(lmParams) {} + TranslationRecovery(const LevenbergMarquardtParams &lmParams, + bool use_bilinear_translation_factor) + : lmParams_(lmParams), + use_bilinear_translation_factor_(use_bilinear_translation_factor) {} /** * @brief Default constructor. diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index c57e03174f..abf63b4cfb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -24,7 +24,7 @@ virtual class SfmTrack : gtsam::SfmTrack2d { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; - + Point3 p; double r; @@ -38,8 +38,8 @@ virtual class SfmTrack : gtsam::SfmTrack2d { bool equals(const gtsam::SfmTrack& expected, double tol) const; }; -#include #include +#include #include class SfmData { SfmData(); @@ -288,7 +288,8 @@ class MFAS { #include class TranslationRecovery { - TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams); + TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams, + const bool use_bilinear_translation_factor); TranslationRecovery(); // default params. void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 818f2bce51..b257456929 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -30,7 +30,7 @@ using namespace gtsam; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05)); // Keys are deliberately *not* in sorted order to test that case. -static const Key kKey1(2), kKey2(1); +static const Key kKey1(2), kKey2(1), kEdgeKey(3); static const Unit3 kMeasured(1, 0, 0); /* ************************************************************************* */ @@ -99,6 +99,79 @@ TEST(TranslationFactor, Jacobian) { EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); } + +/* ************************************************************************* */ +TEST(BilinearAngleTranslationFactor, Constructor) { + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); +} + +/* ************************************************************************* */ +TEST(BilinearAngleTranslationFactor, ZeroError) { + // Create a factor + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + Vector1 scale(1.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2, scale)); + + // Verify we get the expected error + Vector expected = (Vector3() << 0, 0, 0).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST(BilinearAngleTranslationFactor, NonZeroError) { + // create a factor + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); + + // set the linearization + Point3 T1(0, 1, 1), T2(0, 2, 2); + Vector1 scale(1.0 / sqrt(2)); + + // use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2, scale)); + + // verify we get the expected error + Vector expected = (Vector3() << -1, 1 / sqrt(2), 1 / sqrt(2)).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); +} + +/* ************************************************************************* */ +Vector bilinearAngleFactorError(const Point3 &T1, const Point3 &T2, const Vector1 &scale, + const BilinearAngleTranslationFactor &factor) { + return factor.evaluateError(T1, T2, scale); +} + +TEST(BilinearAngleTranslationFactor, Jacobian) { + // Create a factor + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + Vector1 scale(1.0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(T1, T2, scale, H1Actual, H2Actual, H3Actual); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected; + H1Expected = numericalDerivative11( + std::bind(&bilinearAngleFactorError, std::placeholders::_1, T2, scale, factor), T1); + H2Expected = numericalDerivative11( + std::bind(&bilinearAngleFactorError, T1, std::placeholders::_1, scale, factor), T2); + H3Expected = numericalDerivative11( + std::bind(&bilinearAngleFactorError, T1, T2, std::placeholders::_1, factor), scale); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 15f1caa1b4..6e9c3ed240 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -353,6 +353,340 @@ TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) { EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); } +/* ************************************************************************* +* Repeat all tests, but with the Bilinear Angle Translation Factor. +* ************************************************************************* */ + + +/* ************************************************************************* */ +// We read the BAL file, which has 3 cameras in it, with poses. We then assume +// the rotations are correct, but translations have to be estimated from +// translation directions only. Since we have 3 cameras, A, B, and C, we can at +// most create three relative measurements, let's call them w_aZb, w_aZc, and +// bZc. These will be of type Unit3. We then call `recoverTranslations` which +// sets up an optimization problem for the three unknown translations. +TEST(TranslationRecovery, BALBATA) { + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData db = SfmData::FromBalFile(filename); + + // Get camera poses, as Values + size_t j = 0; + Values poses; + for (auto camera : db.cameras) { + poses.insert(j++, camera.pose()); + } + + // Simulate measurements + const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 2}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + const auto graph = algorithm.buildGraph(relativeTranslations); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Run translation recovery + const double scale = 2.0; + const auto result = algorithm.run(relativeTranslations, scale); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal( + Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])), + result.at(1))); + + // Check that the third translations is correct + Point3 Ta = poses.at(0).translation(); + Point3 Tb = poses.at(1).translation(); + Point3 Tc = poses.at(2).translation(); + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + EXPECT(assert_equal(expected, result.at(2), 1e-4)); + + // TODO(frank): how to get stats back? + // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); +} + +TEST(TranslationRecovery, TwoPoseTestBATA) { + // Create a dataset with 2 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // + // 0 and 1 face in the same direction but have a translation offset. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + const auto graph = algorithm.buildGraph(relativeTranslations); + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); +} + +TEST(TranslationRecovery, ThreePoseTestBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + const auto graph = algorithm.buildGraph(relativeTranslations); + EXPECT_LONGS_EQUAL(3, graph.size()); + + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3), 1e-8)); +} + +TEST(TranslationRecovery, ThreePosesIncludingZeroTranslationBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // 2 <| + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with little FOV overlap. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(2), 1e-8)); +} + +TEST(TranslationRecovery, FourPosesIncludingZeroTranslationBATA) { + // Create a dataset with 4 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ 2 <| + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with very little FOV overlap. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(2), 1e-8)); + EXPECT(assert_equal(Point3(2, -2, 0), result.at(3), 1e-8)); +} + +TEST(TranslationRecovery, ThreePosesWithZeroTranslationBATA) { + Values poses; + poses.insert(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {2, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); +} + +TEST(TranslationRecovery, ThreePosesWithOneSoftConstraintBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), + noiseModel::Isotropic::Sigma(3, 1e-2)); + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + +TEST(TranslationRecovery, ThreePosesWithOneHardConstraintBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + +TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurementsBATA) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + + + /* ************************************************************************* */ int main() { TestResult tr; From 9021c888ef88e09c20bd8bb22bdcf904887cf779 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 8 Sep 2024 14:55:10 -0400 Subject: [PATCH 2/7] add the bata translation factor, with unit tests --- gtsam/sfm/TranslationFactor.h | 76 ++++- gtsam/sfm/TranslationRecovery.cpp | 18 +- gtsam/sfm/TranslationRecovery.h | 8 +- gtsam/sfm/sfm.i | 7 +- gtsam/sfm/tests/testTranslationFactor.cpp | 75 ++++- tests/testTranslationRecovery.cpp | 334 ++++++++++++++++++++++ 6 files changed, 506 insertions(+), 12 deletions(-) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index a7289f759d..f11d6a0d67 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -18,6 +18,7 @@ * @brief Binary factor for a relative translation direction measurement. */ +#include #include #include #include @@ -36,8 +37,6 @@ namespace gtsam { * normalized(Tb - Ta) - w_aZb.point3() * * @ingroup sfm - * - * */ class TranslationFactor : public NoiseModelFactorN { private: @@ -60,7 +59,7 @@ class TranslationFactor : public NoiseModelFactorN { * @brief Caclulate error: (norm(Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is * the Unit3 translation direction from a to b. - * + * * @param Ta translation for key a * @param Tb translation for key b * @param H1 optional jacobian in Ta @@ -73,7 +72,8 @@ class TranslationFactor : public NoiseModelFactorN { OptionalMatrixType H2) const override { const Point3 dir = Tb - Ta; Matrix33 H_predicted_dir; - const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); + const Point3 predicted = + normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); if (H1) *H1 = -H_predicted_dir; if (H2) *H2 = H_predicted_dir; return predicted - measured_w_aZb_; @@ -89,4 +89,72 @@ class TranslationFactor : public NoiseModelFactorN { } #endif }; // \ TranslationFactor + +/** + * Binary factor for a relative translation direction measurement + * w_aZb. The measurement equation is + * w_aZb = scale * (Tb - Ta) + * i.e., w_aZb is the translation direction from frame A to B, in world + * coordinates. + * + * Instead of normalizing the Tb - Ta vector, we multiply it by a scalar + * which is also jointly optimized. This is inspired by the work on + * BATA (Zhuang et al, CVPR 2018). + * + * @ingroup sfm + */ +class BilinearAngleTranslationFactor + : public NoiseModelFactorN { + private: + typedef NoiseModelFactorN Base; + Point3 measured_w_aZb_; + + public: + /// default constructor + BilinearAngleTranslationFactor() {} + + BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, const Unit3& w_aZb, + const SharedNoiseModel& noiseModel) + : Base(noiseModel, a, b, scale_key), + measured_w_aZb_(w_aZb.point3()) {} + + /** + * @brief Caclulate error: (scale * (Tb - Ta) - measurement) + * where Tb and Ta are Point3 translations and measurement is + * the Unit3 translation direction from a to b. + * + * @param Ta translation for key a + * @param Tb translation for key b + * @param H1 optional jacobian in Ta + * @param H2 optional jacobian in Tb + * @return * Vector + */ + Vector evaluateError( + const Point3& Ta, const Point3& Tb, const Vector1& scale, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override { + // Ideally we should use a positive real valued scalar datatype for scale. + const double abs_scale = abs(scale[0]); + const Point3 predicted = (Tb - Ta) * abs_scale; + if (H1) { + *H1 = -Matrix3::Identity() * abs_scale; + } + if (H2) { + *H2 = Matrix3::Identity() * abs_scale; + } + if (H3) { + *H3 = scale[0] >= 0 ? (Tb - Ta) : (Ta - Tb); + } + return predicted - measured_w_aZb_; + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ BilinearAngleTranslationFactor } // namespace gtsam diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index c7ef2e526d..4c7bb24da7 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -101,9 +101,17 @@ NonlinearFactorGraph TranslationRecovery::buildGraph( NonlinearFactorGraph graph; // Add translation factors for input translation directions. + uint i = 0; for (auto edge : relativeTranslations) { - graph.emplace_shared(edge.key1(), edge.key2(), - edge.measured(), edge.noiseModel()); + if (use_bilinear_translation_factor_) { + graph.emplace_shared( + edge.key1(), edge.key2(), Symbol('S', i), edge.measured(), + edge.noiseModel()); + } else { + graph.emplace_shared( + edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); + } + i++; } return graph; } @@ -163,6 +171,12 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + + if (use_bilinear_translation_factor_) { + for (uint i = 0; i < relativeTranslations.size(); i++) { + initial.insert(Symbol('S', i), Vector1(1.0)); + } + } return initial; } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 4848d7cfaf..0765077c01 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -60,14 +60,18 @@ class GTSAM_EXPORT TranslationRecovery { // Parameters. LevenbergMarquardtParams lmParams_; + const bool use_bilinear_translation_factor_ = false; + public: /** * @brief Construct a new Translation Recovery object * * @param lmParams parameters for optimization. */ - TranslationRecovery(const LevenbergMarquardtParams &lmParams) - : lmParams_(lmParams) {} + TranslationRecovery(const LevenbergMarquardtParams &lmParams, + bool use_bilinear_translation_factor) + : lmParams_(lmParams), + use_bilinear_translation_factor_(use_bilinear_translation_factor) {} /** * @brief Default constructor. diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index ba25cf7938..32b087b2d8 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -23,7 +23,7 @@ virtual class SfmTrack : gtsam::SfmTrack2d { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; - + Point3 p; double r; @@ -37,8 +37,8 @@ virtual class SfmTrack : gtsam::SfmTrack2d { bool equals(const gtsam::SfmTrack& expected, double tol) const; }; -#include #include +#include #include class SfmData { SfmData(); @@ -268,7 +268,8 @@ class MFAS { #include class TranslationRecovery { - TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams); + TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams, + const bool use_bilinear_translation_factor); TranslationRecovery(); // default params. void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 818f2bce51..b257456929 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -30,7 +30,7 @@ using namespace gtsam; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05)); // Keys are deliberately *not* in sorted order to test that case. -static const Key kKey1(2), kKey2(1); +static const Key kKey1(2), kKey2(1), kEdgeKey(3); static const Unit3 kMeasured(1, 0, 0); /* ************************************************************************* */ @@ -99,6 +99,79 @@ TEST(TranslationFactor, Jacobian) { EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); } + +/* ************************************************************************* */ +TEST(BilinearAngleTranslationFactor, Constructor) { + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); +} + +/* ************************************************************************* */ +TEST(BilinearAngleTranslationFactor, ZeroError) { + // Create a factor + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + Vector1 scale(1.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2, scale)); + + // Verify we get the expected error + Vector expected = (Vector3() << 0, 0, 0).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST(BilinearAngleTranslationFactor, NonZeroError) { + // create a factor + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); + + // set the linearization + Point3 T1(0, 1, 1), T2(0, 2, 2); + Vector1 scale(1.0 / sqrt(2)); + + // use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2, scale)); + + // verify we get the expected error + Vector expected = (Vector3() << -1, 1 / sqrt(2), 1 / sqrt(2)).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); +} + +/* ************************************************************************* */ +Vector bilinearAngleFactorError(const Point3 &T1, const Point3 &T2, const Vector1 &scale, + const BilinearAngleTranslationFactor &factor) { + return factor.evaluateError(T1, T2, scale); +} + +TEST(BilinearAngleTranslationFactor, Jacobian) { + // Create a factor + BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + Vector1 scale(1.0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(T1, T2, scale, H1Actual, H2Actual, H3Actual); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected; + H1Expected = numericalDerivative11( + std::bind(&bilinearAngleFactorError, std::placeholders::_1, T2, scale, factor), T1); + H2Expected = numericalDerivative11( + std::bind(&bilinearAngleFactorError, T1, std::placeholders::_1, scale, factor), T2); + H3Expected = numericalDerivative11( + std::bind(&bilinearAngleFactorError, T1, T2, std::placeholders::_1, factor), scale); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 15f1caa1b4..6e9c3ed240 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -353,6 +353,340 @@ TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) { EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); } +/* ************************************************************************* +* Repeat all tests, but with the Bilinear Angle Translation Factor. +* ************************************************************************* */ + + +/* ************************************************************************* */ +// We read the BAL file, which has 3 cameras in it, with poses. We then assume +// the rotations are correct, but translations have to be estimated from +// translation directions only. Since we have 3 cameras, A, B, and C, we can at +// most create three relative measurements, let's call them w_aZb, w_aZc, and +// bZc. These will be of type Unit3. We then call `recoverTranslations` which +// sets up an optimization problem for the three unknown translations. +TEST(TranslationRecovery, BALBATA) { + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData db = SfmData::FromBalFile(filename); + + // Get camera poses, as Values + size_t j = 0; + Values poses; + for (auto camera : db.cameras) { + poses.insert(j++, camera.pose()); + } + + // Simulate measurements + const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 2}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + const auto graph = algorithm.buildGraph(relativeTranslations); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Run translation recovery + const double scale = 2.0; + const auto result = algorithm.run(relativeTranslations, scale); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal( + Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])), + result.at(1))); + + // Check that the third translations is correct + Point3 Ta = poses.at(0).translation(); + Point3 Tb = poses.at(1).translation(); + Point3 Tc = poses.at(2).translation(); + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + EXPECT(assert_equal(expected, result.at(2), 1e-4)); + + // TODO(frank): how to get stats back? + // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); +} + +TEST(TranslationRecovery, TwoPoseTestBATA) { + // Create a dataset with 2 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // + // 0 and 1 face in the same direction but have a translation offset. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + const auto graph = algorithm.buildGraph(relativeTranslations); + EXPECT_LONGS_EQUAL(1, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); +} + +TEST(TranslationRecovery, ThreePoseTestBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + const auto graph = algorithm.buildGraph(relativeTranslations); + EXPECT_LONGS_EQUAL(3, graph.size()); + + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at(3), 1e-8)); +} + +TEST(TranslationRecovery, ThreePosesIncludingZeroTranslationBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // 2 <| + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with little FOV overlap. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + + auto relativeTranslations = + TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(3, 0, 0), result.at(2), 1e-8)); +} + +TEST(TranslationRecovery, FourPosesIncludingZeroTranslationBATA) { + // Create a dataset with 4 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ 2 <| + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 2 is at + // the same point as 1 but is rotated, with very little FOV overlap. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(4, 0, 0), result.at(2), 1e-8)); + EXPECT(assert_equal(Point3(2, -2, 0), result.at(3), 1e-8)); +} + +TEST(TranslationRecovery, ThreePosesWithZeroTranslationBATA) { + Values poses; + poses.insert(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {2, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + // Run translation recovery + const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(1), 1e-8)); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(2), 1e-8)); +} + +TEST(TranslationRecovery, ThreePosesWithOneSoftConstraintBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), + noiseModel::Isotropic::Sigma(3, 1e-2)); + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + +TEST(TranslationRecovery, ThreePosesWithOneHardConstraintBATA) { + // Create a dataset with 3 poses. + // __ __ + // \/ \/ + // 0 _____ 1 + // \ __ / + // \\// + // 3 + // + // 0 and 1 face in the same direction but have a translation offset. 3 is in + // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset. + + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); +} + +TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurementsBATA) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + LevenbergMarquardtParams params; + TranslationRecovery algorithm(params, true); + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + + + /* ************************************************************************* */ int main() { TestResult tr; From a6861392c9b564758937621ee341c1ac0aa3ea84 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 8 Sep 2024 19:44:28 -0400 Subject: [PATCH 3/7] rebase and remove boost --- gtsam/sfm/TranslationFactor.h | 28 ++++++++++++---------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index f11d6a0d67..40227eef6f 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -44,7 +44,6 @@ class TranslationFactor : public NoiseModelFactorN { Point3 measured_w_aZb_; public: - // Provide access to the Matrix& version of evaluateError: using NoiseModelFactor2::evaluateError; @@ -66,10 +65,9 @@ class TranslationFactor : public NoiseModelFactorN { * @param H2 optional jacobian in Tb * @return * Vector */ - Vector evaluateError( - const Point3& Ta, const Point3& Tb, - OptionalMatrixType H1, - OptionalMatrixType H2) const override { + Vector evaluateError(const Point3& Ta, const Point3& Tb, + OptionalMatrixType H1, + OptionalMatrixType H2) const override { const Point3 dir = Tb - Ta; Matrix33 H_predicted_dir; const Point3 predicted = @@ -95,11 +93,11 @@ class TranslationFactor : public NoiseModelFactorN { * w_aZb. The measurement equation is * w_aZb = scale * (Tb - Ta) * i.e., w_aZb is the translation direction from frame A to B, in world - * coordinates. - * + * coordinates. + * * Instead of normalizing the Tb - Ta vector, we multiply it by a scalar * which is also jointly optimized. This is inspired by the work on - * BATA (Zhuang et al, CVPR 2018). + * BATA (Zhuang et al, CVPR 2018). * * @ingroup sfm */ @@ -113,10 +111,10 @@ class BilinearAngleTranslationFactor /// default constructor BilinearAngleTranslationFactor() {} - BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, const Unit3& w_aZb, + BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, + const Unit3& w_aZb, const SharedNoiseModel& noiseModel) - : Base(noiseModel, a, b, scale_key), - measured_w_aZb_(w_aZb.point3()) {} + : Base(noiseModel, a, b, scale_key), measured_w_aZb_(w_aZb.point3()) {} /** * @brief Caclulate error: (scale * (Tb - Ta) - measurement) @@ -129,11 +127,9 @@ class BilinearAngleTranslationFactor * @param H2 optional jacobian in Tb * @return * Vector */ - Vector evaluateError( - const Point3& Ta, const Point3& Tb, const Vector1& scale, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const override { + Vector evaluateError(const Point3& Ta, const Point3& Tb, const Vector1& scale, + OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3) const override { // Ideally we should use a positive real valued scalar datatype for scale. const double abs_scale = abs(scale[0]); const Point3 predicted = (Tb - Ta) * abs_scale; From 26a1ccabfc614f4fd82c4ed645c2dbedb9bef30b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 8 Sep 2024 20:17:06 -0400 Subject: [PATCH 4/7] add no jacobian version of evaluateerror --- gtsam/sfm/TranslationFactor.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 40227eef6f..9af3b184e8 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -116,6 +116,9 @@ class BilinearAngleTranslationFactor const SharedNoiseModel& noiseModel) : Base(noiseModel, a, b, scale_key), measured_w_aZb_(w_aZb.point3()) {} + // Provide access to the Matrix& version of evaluateError: + using NoiseModelFactor2::evaluateError; + /** * @brief Caclulate error: (scale * (Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is From 3319400625306c18ed4ca12dfb81164b7574de3b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 8 Sep 2024 22:21:49 -0400 Subject: [PATCH 5/7] add the default option wrapper --- gtsam/sfm/sfm.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 32b087b2d8..142e65d7e1 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -270,6 +270,7 @@ class MFAS { class TranslationRecovery { TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams, const bool use_bilinear_translation_factor); + TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams); TranslationRecovery(); // default params. void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const double scale, From 551643bf6e25ead5e2d50ed1a61b15d212b5cc90 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 8 Sep 2024 22:22:04 -0400 Subject: [PATCH 6/7] minor uint change --- gtsam/sfm/TranslationRecovery.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 4c7bb24da7..02c78133e8 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -101,7 +101,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph( NonlinearFactorGraph graph; // Add translation factors for input translation directions. - uint i = 0; + uint64_t i = 0; for (auto edge : relativeTranslations) { if (use_bilinear_translation_factor_) { graph.emplace_shared( @@ -173,7 +173,7 @@ Values TranslationRecovery::initializeRandomly( } if (use_bilinear_translation_factor_) { - for (uint i = 0; i < relativeTranslations.size(); i++) { + for (uint64_t i = 0; i < relativeTranslations.size(); i++) { initial.insert(Symbol('S', i), Vector1(1.0)); } } From 14d8870c6b6acfcd30536ecae907522a7e07f564 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 9 Sep 2024 07:21:28 -0700 Subject: [PATCH 7/7] make boolean parameter optional --- gtsam/sfm/TranslationRecovery.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 0765077c01..a91ef01f94 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -69,7 +69,7 @@ class GTSAM_EXPORT TranslationRecovery { * @param lmParams parameters for optimization. */ TranslationRecovery(const LevenbergMarquardtParams &lmParams, - bool use_bilinear_translation_factor) + bool use_bilinear_translation_factor = false) : lmParams_(lmParams), use_bilinear_translation_factor_(use_bilinear_translation_factor) {}