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); +} +/* ************************************************************************* */