sfm changes to docs, format, wrapper

release/4.3a0
akrishnan86 2020-08-08 12:09:30 -07:00
parent 667fb9a4b9
commit 2540285afe
6 changed files with 71 additions and 91 deletions

22
gtsam.h
View File

@ -2894,28 +2894,6 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3; typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
// std::vector<gtsam::BinaryMeasurement<Unit3>::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<gtsam::BinaryMeasurement<Rot3>::shared_ptr>
class BinaryMeasurementRot3s
{
BinaryMeasurementRot3s();
size_t size() const;
gtsam::BinaryMeasurementRot3* at(size_t i) const;
void push_back(const gtsam::BinaryMeasurementRot3* measurement);
};
#include <gtsam/sfm/TranslationRecovery.h>
class TranslationRecovery {
TranslationRecovery()
}
//************************************************************************* //*************************************************************************
// Navigation // Navigation
//************************************************************************* //*************************************************************************

View File

@ -15,12 +15,15 @@
* @file BinaryMeasurement.h * @file BinaryMeasurement.h
* @author Akshay Krishnan * @author Akshay Krishnan
* @date July 2020 * @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 <ostream> #include <ostream>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -49,17 +52,12 @@ private:
SharedNoiseModel noiseModel_; /** Noise model */ SharedNoiseModel noiseModel_; /** Noise model */
public: public:
/** default constructor - only use for serialization */
BinaryMeasurement() {}
/** Constructor */ /** Constructor */
BinaryMeasurement(Key key1, Key key2, const VALUE &measured, BinaryMeasurement(Key key1, Key key2, const VALUE &measured,
const SharedNoiseModel &model = nullptr) : const SharedNoiseModel &model = nullptr) :
key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) { 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_; }
@ -83,15 +81,12 @@ public:
return e != nullptr && key1_ == e->key1_ && return e != nullptr && key1_ == e->key1_ &&
key2_ == e->key2_ key2_ == e->key2_
&& traits<VALUE>::Equals(this->measured_, e->measured_, tol) && && traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_->equals(expected.noiseModel()); noiseModel_->equals(*expected.noiseModel());
} }
/** return the measured */ /** return the measured value */
VALUE measured() const { VALUE measured() const {
return measured_; return measured_;
} }
}; // \class BetweenMeasurement }; // \class BetweenMeasurement
using BinaryMeasurementUnit3s = std::vector<gtsam::BinaryMeasurement<Unit3>::shared_ptr>;
using BinaryMeasurementRot3s = std::vector<gtsam::BinaryMeasurement<Rot3>::shared_ptr>;
} }

View File

@ -10,10 +10,10 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file TranslationRecovery.h * @file TranslationRecovery.cpp
* @author Frank Dellaert * @author Frank Dellaert
* @date March 2020 * @date March 2020
* @brief test recovering translations when rotations are given. * @brief Source code for recovering translations when rotations are given
*/ */
#include <gtsam/sfm/TranslationRecovery.h> #include <gtsam/sfm/TranslationRecovery.h>

View File

@ -13,7 +13,7 @@
* @file TranslationRecovery.h * @file TranslationRecovery.h
* @author Frank Dellaert * @author Frank Dellaert
* @date March 2020 * @date March 2020
* @brief test recovering translations when rotations are given. * @brief Recovering translations in an epipolar graph when rotations are given.
*/ */
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>

View File

@ -27,7 +27,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// 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);
// Create noise models for unit3 and rot3 // Create noise models for unit3 and rot3
@ -43,14 +42,24 @@ TEST(BinaryMeasurement, Unit3) {
EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1);
EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2); EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2);
EXPECT(unit3Measurement.measured().equals(unit3Measured)); EXPECT(unit3Measurement.measured().equals(unit3Measured));
BinaryMeasurement<Unit3> unit3MeasurementCopy(kKey1, kKey2, unit3Measured,
unit3_model);
EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
} }
TEST(BinaryMeasurement, Rot3) { TEST(BinaryMeasurement, Rot3) {
// testing the accessors
BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured, BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
rot3_model); rot3_model);
EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
EXPECT(rot3Measurement.measured().equals(rot3Measured)); EXPECT(rot3Measurement.measured().equals(rot3Measured));
// testing the equals function
BinaryMeasurement<Rot3> rot3MeasurementCopy(kKey1, kKey2, rot3Measured,
rot3_model);
EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
} }
/* ************************************************************************* */ /* ************************************************************************* */

View File

@ -51,8 +51,6 @@ TEST(TranslationFactor, ZeroError) {
// Verify we get the expected error // Verify we get the expected error
Vector expected = (Vector3() << 0, 0, 0).finished(); Vector expected = (Vector3() << 0, 0, 0).finished();
EXPECT(assert_equal(expected, actualError, 1e-9)); EXPECT(assert_equal(expected, actualError, 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */