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

Merge pull request #463 from borglab/feature/sfm_binary_measurement 

Update translation averaging to use vector of binary measurements instead of map
release/4.3a0
Frank Dellaert 2020-08-13 09:35:22 -04:00 committed by GitHub
parent de49ef6ff0 1159032d89
commit 9bcdbe8b78
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 230 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam.h
View File

@ -2569,10 +2569,12 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
void serialize() const;
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2,
gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
@ -2583,7 +2585,6 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
void serialize() const;
};
#include <gtsam/sam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NoiseModelFactor {
@ -2881,6 +2882,19 @@ virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
};
#include <gtsam/sfm/BinaryMeasurement.h>
template<T>
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<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
//*************************************************************************
// Navigation
//*************************************************************************

92
gtsam/sfm/BinaryMeasurement.h Normal file
View File

@ -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 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
* 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>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/NoiseModel.h>
namespace gtsam {
template<class VALUE>
class BinaryMeasurement {
// Check that VALUE type is testable
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
public:
typedef VALUE T;
// shorthand for a smart pointer to a measurement
typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr;
private:
Key key1_, key2_; /** Keys */
VALUE measured_; /** The measurement */
SharedNoiseModel noiseModel_; /** Noise model */
public:
/** Constructor */
BinaryMeasurement(Key key1, Key key2, const VALUE &measured,
const SharedNoiseModel &model = nullptr) :
key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) {
}
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<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());
}
/** return the measured value */
VALUE measured() const {
return measured_;
}
}; // \class BetweenMeasurement
}

2
gtsam/sfm/TranslationFactor.h
View File

@ -36,6 +36,8 @@ namespace gtsam {
* normalized(Tb - Ta) - w_aZb.point3()
*
* @addtogroup SFM
*
*
*/
class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
private:

36
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>
@ -33,30 +33,25 @@ 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<TranslationFactor>(a, b, w_aZb, noiseModel);
graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
edge.measured(), edge.noiseModel());
}
return graph;
}
void TranslationRecovery::addPrior(const double scale,
NonlinearFactorGraph* graph) const {
auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
NonlinearFactorGraph *graph) const {
//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<PriorFactor<Point3> >(a, Point3(0, 0, 0), noiseModel);
graph->emplace_shared<PriorFactor<Point3> >(b, scale * w_aZb.point3(),
noiseModel);
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0), priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(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;
}
@ -89,7 +82,8 @@ 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) {
Key a, b;
@ -97,7 +91,7 @@ TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
const Pose3 wTa = poses.at<Pose3>(a), wTb = poses.at<Pose3>(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;
}

23
gtsam/sfm/TranslationRecovery.h
View File

@ -13,15 +13,16 @@
* @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>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <map>
#include <utility>
#include <vector>
namespace gtsam {
@ -48,7 +49,7 @@ namespace gtsam {
class TranslationRecovery {
public:
using KeyPair = std::pair<Key, Key>;
using TranslationEdges = std::map<KeyPair, Unit3>;
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
private:
TranslationEdges relativeTranslations_;
@ -59,13 +60,14 @@ 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.
*/
TranslationRecovery(const TranslationEdges& relativeTranslations,
const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams())
TranslationRecovery(const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
: relativeTranslations_(relativeTranslations), params_(lmParams) {
params_.setVerbosityLM("Summary");
}
@ -83,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.
@ -105,10 +107,11 @@ 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<KeyPair>& edges);
const Values &poses, const std::vector<KeyPair> &edges);
};
} // namespace gtsam

70
gtsam/sfm/tests/testBinaryMeasurement.cpp Normal file
View File

@ -0,0 +1,70 @@
/* ----------------------------------------------------------------------------
* 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 <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
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<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured,
unit3_model);
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);
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));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

28
tests/testTranslationRecovery.cpp
View File

@ -49,15 +49,17 @@ TEST(TranslationRecovery, BAL) {
poses, {{0, 1}, {0, 2}, {1, 2}});
// Check
const Pose3 wTa = poses.at<Pose3>(0), wTb = poses.at<Pose3>(1),
wTc = poses.at<Pose3>(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<Pose3>(unitTranslation.key1()),
wTb = poses.at<Pose3>(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<Point3>(0)));
EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at<Point3>(1)));
// Check that the third translations is correct
Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
Point3 Ta = poses.at<Pose3>(0).translation();
Point3 Tb = poses.at<Pose3>(1).translation();
Point3 Tc = poses.at<Pose3>(2).translation();
Point3 expected =
(Tc - Ta) * (scale / (Tb - Ta).norm());
EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
// TODO(frank): how to get stats back?