12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam.h
View File

@ -2894,28 +2894,6 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
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
//*************************************************************************

95
gtsam/sfm/BinaryMeasurement.h
View File

@ -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 <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -32,66 +35,58 @@ namespace gtsam {
template<class VALUE>
class BinaryMeasurement {
// Check that VALUE type is testable
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
// Check that VALUE type is testable
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
public:
typedef VALUE T;
public:
typedef VALUE T;
// shorthand for a smart pointer to a measurement
typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr;
// shorthand for a smart pointer to a measurement
typedef typename boost::shared_ptr<BinaryMeasurement> 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<T>::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<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE> *> (&expected);
return e != nullptr && key1_ == e->key1_ &&
key2_ == e->key2_
&& traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_->equals(*expected.noiseModel());
}
/** equals */
bool equals(const BinaryMeasurement& expected, double tol=1e-9) const {
const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE>*> (&expected);
return e != nullptr && key1_ == e->key1_ &&
key2_ == e->key2_
&& traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_->equals(expected.noiseModel());
}
/** return the measured */
VALUE measured() const {
return measured_;
}
}; // \class BetweenMeasurement
using BinaryMeasurementUnit3s = std::vector<gtsam::BinaryMeasurement<Unit3>::shared_ptr>;
using BinaryMeasurementRot3s = std::vector<gtsam::BinaryMeasurement<Rot3>::shared_ptr>;
/** return the measured value */
VALUE measured() const {
return measured_;
}
}; // \class BetweenMeasurement
}

10
gtsam/sfm/TranslationRecovery.cpp
View File

@ -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 <gtsam/sfm/TranslationRecovery.h>
@ -38,14 +38,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
// Add all relative translation edges
for (auto edge : relativeTranslations_) {
graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
edge.measured(), edge.noiseModel());
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<KeyPair>& edges) {
const Values &poses, const vector<KeyPair> &edges) {
auto edgeNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
TranslationEdges relativeTranslations;
for (auto edge : edges) {

10
gtsam/sfm/TranslationRecovery.h
View File

@ -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 <gtsam/geometry/Unit3.h>
@ -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<KeyPair>& edges);
const Values &poses, const std::vector<KeyPair> &edges);
};
} // namespace gtsam

13
gtsam/sfm/tests/testBinaryMeasurement.cpp
View File

@ -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<Unit3> unit3MeasurementCopy(kKey1, kKey2, unit3Measured,
unit3_model);
EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
}
TEST(BinaryMeasurement, Rot3) {
// testing the accessors
BinaryMeasurement<Rot3> 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<Rot3> rot3MeasurementCopy(kKey1, kKey2, rot3Measured,
rot3_model);
EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
}
/* ************************************************************************* */

8
gtsam/sfm/tests/testTranslationFactor.cpp
View File

@ -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);
}