From 851d4a4af4732ef6ebbca0583b592ad393e8006c Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 30 Jul 2020 00:12:13 -0700 Subject: [PATCH 1/5] add binary measurement class --- gtsam/sfm/BinaryMeasurement.h | 92 +++++++++++++++++++++++ gtsam/sfm/TranslationFactor.h | 2 + gtsam/sfm/TranslationRecovery.cpp | 28 +++---- gtsam/sfm/TranslationRecovery.h | 13 ++-- gtsam/sfm/tests/testBinaryMeasurement.cpp | 61 +++++++++++++++ 5 files changed, 174 insertions(+), 22 deletions(-) create mode 100644 gtsam/sfm/BinaryMeasurement.h create mode 100644 gtsam/sfm/tests/testBinaryMeasurement.cpp diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h new file mode 100644 index 000000000..41ffa38d6 --- /dev/null +++ b/gtsam/sfm/BinaryMeasurement.h @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file BinaryMeasurement.h + * @author Akshay Krishnan + * @date July 2020 + * @brief Binary measurement for representing edges in the epipolar graph + */ + +#include +#include + +namespace gtsam { + +template +class BinaryMeasurement { + // Check that VALUE type is testable + BOOST_CONCEPT_ASSERT((IsTestable)); + +public: + typedef VALUE T; + + // shorthand for a smart pointer to a measurement + typedef typename boost::shared_ptr shared_ptr; + +private: + Key key1_, key2_; /** Keys */ + + VALUE measured_; /** The measurement */ + + SharedNoiseModel noiseModel_; /** Noise model */ + +public: + /** default constructor - only use for serialization */ + BinaryMeasurement() {} + + /** Constructor */ + BinaryMeasurement(Key key1, Key key2, const VALUE& measured, + const SharedNoiseModel& model = nullptr) : + key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) { + } + + virtual ~BinaryMeasurement() {} + + Key key1() const { return key1_; } + + Key key2() const { return key2_; } + + const SharedNoiseModel& noiseModel() const { return noiseModel_; } + + /** implement functions needed for Testable */ + + /** print */ + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BinaryMeasurement(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + traits::Print(measured_, " measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + bool equals(const BetweenMeasurement& expected, double tol=1e-9) const { + const BetweenMeasurement *e = dynamic_cast*> (&expected); + return e != nullptr && key1_ == expected.key1() && + key2_ == expected.key2() + && traits::Equals(this->measured_, e->measured_, tol) && + noiseModel_.equals(expected.noiseModel()); + } + + /** return the measured */ + const VALUE& measured() const { + return measured_; + } + + /** number of variables attached to this measurement */ + std::size_t size() const { + return 2; + } + }; // \class BetweenMeasurement +} \ No newline at end of file diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index d63633d7e..d1f85cf01 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -36,6 +36,8 @@ namespace gtsam { * normalized(Tb - Ta) - w_aZb.point3() * * @addtogroup SFM + * + * */ class TranslationFactor : public NoiseModelFactor2 { private: diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index aeeae688f..c3be003ba 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -33,15 +33,12 @@ using namespace gtsam; using namespace std; NonlinearFactorGraph TranslationRecovery::buildGraph() const { - auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); NonlinearFactorGraph graph; // Add all relative translation edges for (auto edge : relativeTranslations_) { - Key a, b; - tie(a, b) = edge.first; - const Unit3 w_aZb = edge.second; - graph.emplace_shared(a, b, w_aZb, noiseModel); + graph.emplace_shared(edge.key1(), edge.key2(), + edge.measured(), edge.noiseModel()); } return graph; @@ -49,14 +46,12 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { void TranslationRecovery::addPrior(const double scale, NonlinearFactorGraph* graph) const { - auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + //TODO(akshay-krishnan): make this an input argument + auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01); auto edge = relativeTranslations_.begin(); - Key a, b; - tie(a, b) = edge->first; - const Unit3 w_aZb = edge->second; - graph->emplace_shared >(a, Point3(0, 0, 0), noiseModel); - graph->emplace_shared >(b, scale * w_aZb.point3(), - noiseModel); + graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); + graph->emplace_shared >(edge->key2(), scale * edge->measured().point3(), + edge->noiseModel()); } Values TranslationRecovery::initalizeRandomly() const { @@ -71,10 +66,8 @@ Values TranslationRecovery::initalizeRandomly() const { // Loop over measurements and add a random translation for (auto edge : relativeTranslations_) { - Key a, b; - tie(a, b) = edge.first; - insert(a); - insert(b); + insert(edge.key1()); + insert(edge.key2()); } return initial; } @@ -90,6 +83,7 @@ Values TranslationRecovery::run(const double scale) const { TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( const Values& poses, const vector& edges) { + auto edgeNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01); TranslationEdges relativeTranslations; for (auto edge : edges) { Key a, b; @@ -97,7 +91,7 @@ TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( const Pose3 wTa = poses.at(a), wTb = poses.at(b); const Point3 Ta = wTa.translation(), Tb = wTb.translation(); const Unit3 w_aZb(Tb - Ta); - relativeTranslations[edge] = w_aZb; + relativeTranslations.emplace_back(a, b, w_aZb, edgeNoiseModel); } return relativeTranslations; } diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index bb3c3cdb1..1392817b2 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -19,9 +19,10 @@ #include #include #include +#include -#include #include +#include namespace gtsam { @@ -48,7 +49,7 @@ namespace gtsam { class TranslationRecovery { public: using KeyPair = std::pair; - using TranslationEdges = std::map; + using TranslationEdges = std::vector>; private: TranslationEdges relativeTranslations_; @@ -59,7 +60,8 @@ class TranslationRecovery { * @brief Construct a new Translation Recovery object * * @param relativeTranslations the relative translations, in world coordinate - * frames, indexed in a map by a pair of Pose keys. + * frames, vector of BinaryMeasurements of Unit3, where each key of a measurement + * is a point in 3D. * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be * used to modify the parameters for the LM optimizer. By default, uses the * default LM parameters. @@ -105,8 +107,9 @@ class TranslationRecovery { * * @param poses SE(3) ground truth poses stored as Values * @param edges pairs (a,b) for which a measurement w_aZb will be generated. - * @return TranslationEdges map from a KeyPair to the simulated Unit3 - * translation direction measurement between the cameras in KeyPair. + * @return TranslationEdges vector of binary measurements where the keys are + * the cameras and the measurement is the simulated Unit3 translation + * direction between the cameras. */ static TranslationEdges SimulateMeasurements( const Values& poses, const std::vector& edges); diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp new file mode 100644 index 000000000..ae3ba23e5 --- /dev/null +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testBinaryMeasurement.cpp + * @brief Unit tests for BinaryMeasurement class + * @author Akshay Krishnan + * @date July 2020 + */ + +#include + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Keys are deliberately *not* in sorted order to test that case. +static const Key kKey1(2), kKey2(1); + +// Create noise models for unit3 and rot3 +static SharedNoiseModel unit3_model(noiseModel::Isotropic::Sigma(2, 0.05)); +static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05)); + +const Unit3 unit3Measured(Vector3(1, 1, 1)); +const Rot3 rot3Measured; + +TEST(BinaryMeasurement, Unit3) { + BinaryMeasurement unit3Measurement(kKey1, kKey2, unit3Measured, + unit3_model); + EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1); + EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2); + EXPECT(unit3Measurement.measured().equals(unit3Measured)); +} + +TEST(BinaryMeasurement, Rot3) { + BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, + rot3_model); + EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); + EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); + EXPECT(rot3Measurement.measured().equals(rot3Measured)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From ae5956bd10edd2d0e1b92f5778f12b4ac6ff0dd7 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 5 Aug 2020 00:18:42 -0700 Subject: [PATCH 2/5] changes with passing unit tests --- gtsam/sfm/BinaryMeasurement.h | 24 +++++++++++++----------- tests/testTranslationRecovery.cpp | 28 +++++++++++++++++----------- 2 files changed, 30 insertions(+), 22 deletions(-) diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 41ffa38d6..99f0e6882 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -18,6 +18,13 @@ * @brief Binary measurement for representing edges in the epipolar graph */ +#include + + +#include +#include +#include + #include #include @@ -71,22 +78,17 @@ public: } /** equals */ - bool equals(const BetweenMeasurement& expected, double tol=1e-9) const { - const BetweenMeasurement *e = dynamic_cast*> (&expected); - return e != nullptr && key1_ == expected.key1() && - key2_ == expected.key2() + bool equals(const BinaryMeasurement& expected, double tol=1e-9) const { + const BinaryMeasurement *e = dynamic_cast*> (&expected); + return e != nullptr && key1_ == e->key1_ && + key2_ == e->key2_ && traits::Equals(this->measured_, e->measured_, tol) && - noiseModel_.equals(expected.noiseModel()); + noiseModel_->equals(expected.noiseModel()); } /** return the measured */ - const VALUE& measured() const { + VALUE measured() const { return measured_; } - - /** number of variables attached to this measurement */ - std::size_t size() const { - return 2; - } }; // \class BetweenMeasurement } \ No newline at end of file diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 5a98c3bf5..eb34ba803 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -49,15 +49,17 @@ TEST(TranslationRecovery, BAL) { poses, {{0, 1}, {0, 2}, {1, 2}}); // Check - const Pose3 wTa = poses.at(0), wTb = poses.at(1), - wTc = poses.at(2); - const Point3 Ta = wTa.translation(), Tb = wTb.translation(), - Tc = wTc.translation(); - const Rot3 aRw = wTa.rotation().inverse(); - const Unit3 w_aZb = relativeTranslations.at({0, 1}); - EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); - const Unit3 w_aZc = relativeTranslations.at({0, 2}); - EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc)); + Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test + for(auto& unitTranslation : relativeTranslations) { + const Pose3 wTa = poses.at(unitTranslation.key1()), + wTb = poses.at(unitTranslation.key2()); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb = unitTranslation.measured(); + EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) { + w_aZb_stored = unitTranslation.measured(); + } + } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); @@ -69,10 +71,14 @@ TEST(TranslationRecovery, BAL) { // Check result for first two translations, determined by prior EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); - EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at(1))); + EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at(1))); // Check that the third translations is correct - Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + 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? From 667fb9a4b956f04baf619126a4e33d4a4869f207 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 6 Aug 2020 06:28:34 -0700 Subject: [PATCH 3/5] binary measurement wrap --- gtsam.h | 44 +++++++++++++++++++++++++++++++---- gtsam/sfm/BinaryMeasurement.h | 3 +++ 2 files changed, 43 insertions(+), 4 deletions(-) diff --git a/gtsam.h b/gtsam.h index 2cd30be42..80fbddec7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2568,10 +2568,12 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { void serialize() const; }; - - #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2582,7 +2584,6 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { void serialize() const; }; - #include template virtual class RangeFactor : gtsam::NoiseModelFactor { @@ -2880,6 +2881,41 @@ virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor { Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); }; +#include +template +class BinaryMeasurement { + BinaryMeasurement(size_t key1, size_t key2, const T& measured, + const gtsam::noiseModel::Base* model); + size_t key1() const; + size_t key2() const; + T measured() const; +}; + +typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; +typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; + +// std::vector::shared_ptr> +class BinaryMeasurementUnit3s +{ + BinaryMeasurementUnit3s(); + size_t size() const; + gtsam::BinaryMeasurementUnit3* at(size_t i) const; + void push_back(const gtsam::BinaryMeasurementUnit3* measurement); +}; + +// std::vector::shared_ptr> +class BinaryMeasurementRot3s +{ + BinaryMeasurementRot3s(); + size_t size() const; + gtsam::BinaryMeasurementRot3* at(size_t i) const; + void push_back(const gtsam::BinaryMeasurementRot3* measurement); +}; + +#include +class TranslationRecovery { + TranslationRecovery() +} //************************************************************************* // Navigation //************************************************************************* diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 99f0e6882..c5bb9c625 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -91,4 +91,7 @@ public: return measured_; } }; // \class BetweenMeasurement + + using BinaryMeasurementUnit3s = std::vector::shared_ptr>; + using BinaryMeasurementRot3s = std::vector::shared_ptr>; } \ No newline at end of file From 2540285afe0ab2d252fba83ea7afd1ea4a3ad88a Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sat, 8 Aug 2020 12:09:30 -0700 Subject: [PATCH 4/5] sfm changes to docs, format, wrapper --- gtsam.h | 22 ----- gtsam/sfm/BinaryMeasurement.h | 97 +++++++++++------------ gtsam/sfm/TranslationRecovery.cpp | 12 +-- gtsam/sfm/TranslationRecovery.h | 10 +-- gtsam/sfm/tests/testBinaryMeasurement.cpp | 13 ++- gtsam/sfm/tests/testTranslationFactor.cpp | 8 +- 6 files changed, 71 insertions(+), 91 deletions(-) diff --git a/gtsam.h b/gtsam.h index 80fbddec7..4cac956b6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2894,28 +2894,6 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; -// std::vector::shared_ptr> -class BinaryMeasurementUnit3s -{ - BinaryMeasurementUnit3s(); - size_t size() const; - gtsam::BinaryMeasurementUnit3* at(size_t i) const; - void push_back(const gtsam::BinaryMeasurementUnit3* measurement); -}; - -// std::vector::shared_ptr> -class BinaryMeasurementRot3s -{ - BinaryMeasurementRot3s(); - size_t size() const; - gtsam::BinaryMeasurementRot3* at(size_t i) const; - void push_back(const gtsam::BinaryMeasurementRot3* measurement); -}; - -#include -class TranslationRecovery { - TranslationRecovery() -} //************************************************************************* // Navigation //************************************************************************* diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index c5bb9c625..9b0874106 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -15,12 +15,15 @@ * @file BinaryMeasurement.h * @author Akshay Krishnan * @date July 2020 - * @brief Binary measurement for representing edges in the epipolar graph + * @brief Binary measurement for representing edges in the epipolar graph. + * A binary measurement is similar to a BetweenFactor, except that it does not contain + * an error function. It is a measurement (along with a noise model) from one key to + * another. Note that the direction is important. A measurement from key1 to key2 is not + * the same as the same measurement from key2 to key1. */ #include - #include #include #include @@ -30,68 +33,60 @@ namespace gtsam { -template +template class BinaryMeasurement { - // Check that VALUE type is testable - BOOST_CONCEPT_ASSERT((IsTestable)); + // Check that VALUE type is testable + BOOST_CONCEPT_ASSERT((IsTestable)); -public: - typedef VALUE T; + public: + typedef VALUE T; - // shorthand for a smart pointer to a measurement - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a measurement + typedef typename boost::shared_ptr shared_ptr; -private: - Key key1_, key2_; /** Keys */ + private: + Key key1_, key2_; /** Keys */ - VALUE measured_; /** The measurement */ + VALUE measured_; /** The measurement */ - SharedNoiseModel noiseModel_; /** Noise model */ + SharedNoiseModel noiseModel_; /** Noise model */ -public: - /** default constructor - only use for serialization */ - BinaryMeasurement() {} - - /** Constructor */ - BinaryMeasurement(Key key1, Key key2, const VALUE& measured, - const SharedNoiseModel& model = nullptr) : + public: + /** Constructor */ + BinaryMeasurement(Key key1, Key key2, const VALUE &measured, + const SharedNoiseModel &model = nullptr) : key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) { - } + } - virtual ~BinaryMeasurement() {} + Key key1() const { return key1_; } - Key key1() const { return key1_; } + Key key2() const { return key2_; } - Key key2() const { return key2_; } + const SharedNoiseModel &noiseModel() const { return noiseModel_; } - const SharedNoiseModel& noiseModel() const { return noiseModel_; } + /** implement functions needed for Testable */ - /** implement functions needed for Testable */ + /** print */ + void print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BinaryMeasurement(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + traits::Print(measured_, " measured: "); + this->noiseModel_->print(" noise model: "); + } - /** print */ - void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BinaryMeasurement(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; - traits::Print(measured_, " measured: "); - this->noiseModel_->print(" noise model: "); - } + /** equals */ + bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const { + const BinaryMeasurement *e = dynamic_cast *> (&expected); + return e != nullptr && key1_ == e->key1_ && + key2_ == e->key2_ + && traits::Equals(this->measured_, e->measured_, tol) && + noiseModel_->equals(*expected.noiseModel()); + } - /** equals */ - bool equals(const BinaryMeasurement& expected, double tol=1e-9) const { - const BinaryMeasurement *e = dynamic_cast*> (&expected); - return e != nullptr && key1_ == e->key1_ && - key2_ == e->key2_ - && traits::Equals(this->measured_, e->measured_, tol) && - noiseModel_->equals(expected.noiseModel()); - } - - /** return the measured */ - VALUE measured() const { - return measured_; - } - }; // \class BetweenMeasurement - - using BinaryMeasurementUnit3s = std::vector::shared_ptr>; - using BinaryMeasurementRot3s = std::vector::shared_ptr>; + /** return the measured value */ + VALUE measured() const { + return measured_; + } +}; // \class BetweenMeasurement } \ No newline at end of file diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index c3be003ba..665336cc4 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file TranslationRecovery.h + * @file TranslationRecovery.cpp * @author Frank Dellaert * @date March 2020 - * @brief test recovering translations when rotations are given. + * @brief Source code for recovering translations when rotations are given */ #include @@ -37,15 +37,15 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { // Add all relative translation edges for (auto edge : relativeTranslations_) { - graph.emplace_shared(edge.key1(), edge.key2(), - edge.measured(), edge.noiseModel()); + graph.emplace_shared(edge.key1(), edge.key2(), + edge.measured(), edge.noiseModel()); } return graph; } void TranslationRecovery::addPrior(const double scale, - NonlinearFactorGraph* graph) const { + NonlinearFactorGraph *graph) const { //TODO(akshay-krishnan): make this an input argument auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01); auto edge = relativeTranslations_.begin(); @@ -82,7 +82,7 @@ Values TranslationRecovery::run(const double scale) const { } TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( - const Values& poses, const vector& edges) { + const Values &poses, const vector &edges) { auto edgeNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01); TranslationEdges relativeTranslations; for (auto edge : edges) { diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 1392817b2..4c6a95cbe 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -13,7 +13,7 @@ * @file TranslationRecovery.h * @author Frank Dellaert * @date March 2020 - * @brief test recovering translations when rotations are given. + * @brief Recovering translations in an epipolar graph when rotations are given. */ #include @@ -66,8 +66,8 @@ class TranslationRecovery { * used to modify the parameters for the LM optimizer. By default, uses the * default LM parameters. */ - TranslationRecovery(const TranslationEdges& relativeTranslations, - const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams()) + TranslationRecovery(const TranslationEdges &relativeTranslations, + const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()) : relativeTranslations_(relativeTranslations), params_(lmParams) { params_.setVerbosityLM("Summary"); } @@ -85,7 +85,7 @@ class TranslationRecovery { * @param scale scale for first relative translation which fixes gauge. * @param graph factor graph to which prior is added. */ - void addPrior(const double scale, NonlinearFactorGraph* graph) const; + void addPrior(const double scale, NonlinearFactorGraph *graph) const; /** * @brief Create random initial translations. @@ -112,6 +112,6 @@ class TranslationRecovery { * direction between the cameras. */ static TranslationEdges SimulateMeasurements( - const Values& poses, const std::vector& edges); + const Values &poses, const std::vector &edges); }; } // namespace gtsam diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp index ae3ba23e5..3dd81c2c1 100644 --- a/gtsam/sfm/tests/testBinaryMeasurement.cpp +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -27,7 +27,6 @@ using namespace std; using namespace gtsam; -// Keys are deliberately *not* in sorted order to test that case. static const Key kKey1(2), kKey2(1); // Create noise models for unit3 and rot3 @@ -43,14 +42,24 @@ TEST(BinaryMeasurement, Unit3) { EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2); EXPECT(unit3Measurement.measured().equals(unit3Measured)); + + BinaryMeasurement unit3MeasurementCopy(kKey1, kKey2, unit3Measured, + unit3_model); + EXPECT(unit3Measurement.equals(unit3MeasurementCopy)); } TEST(BinaryMeasurement, Rot3) { + // testing the accessors BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, - rot3_model); + rot3_model); EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); EXPECT(rot3Measurement.measured().equals(rot3Measured)); + + // testing the equals function + BinaryMeasurement rot3MeasurementCopy(kKey1, kKey2, rot3Measured, + rot3_model); + EXPECT(rot3Measurement.equals(rot3MeasurementCopy)); } /* ************************************************************************* */ diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 37e8b6c0f..3ff76ac5c 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -51,8 +51,6 @@ TEST(TranslationFactor, ZeroError) { // Verify we get the expected error Vector expected = (Vector3() << 0, 0, 0).finished(); EXPECT(assert_equal(expected, actualError, 1e-9)); - - } /* ************************************************************************* */ @@ -67,13 +65,13 @@ TEST(TranslationFactor, NonZeroError) { Vector actualError(factor.evaluateError(T1, T2)); // verify we get the expected error - Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished(); + Vector expected = (Vector3() << -1, 1 / sqrt(2), 1 / sqrt(2)).finished(); EXPECT(assert_equal(expected, actualError, 1e-9)); } /* ************************************************************************* */ -Vector factorError(const Point3& T1, const Point3& T2, - const TranslationFactor& factor) { +Vector factorError(const Point3 &T1, const Point3 &T2, + const TranslationFactor &factor) { return factor.evaluateError(T1, T2); } From 1159032d89f39b791e2b557f841349babbf4422f Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 9 Aug 2020 22:03:01 -0700 Subject: [PATCH 5/5] update binary measurement brief --- gtsam/sfm/BinaryMeasurement.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 9b0874106..c4b4a9804 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -15,7 +15,7 @@ * @file BinaryMeasurement.h * @author Akshay Krishnan * @date July 2020 - * @brief Binary measurement for representing edges in the epipolar graph. + * @brief Binary measurement represents a measurement between two keys in a graph. * A binary measurement is similar to a BetweenFactor, except that it does not contain * an error function. It is a measurement (along with a noise model) from one key to * another. Note that the direction is important. A measurement from key1 to key2 is not