Merge pull request #367 from borglab/feature/1dsfm

Translation averaging
release/4.3a0
Akshay Krishnan 2020-06-30 08:17:48 -07:00 committed by GitHub
commit 7c53235fdb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 497 additions and 0 deletions

View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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 TranslationFactor.h
* @author Frank Dellaert
* @date March 2020
* @brief Binary factor for a relative translation direction measurement.
*/
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for a relative translation direction measurement
* w_aZb. The measurement equation is
* w_aZb = Unit3(Tb - Ta)
* i.e., w_aZb is the translation direction from frame A to B, in world
* coordinates. Although Unit3 instances live on a manifold, following
* Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
* ambient world coordinate frame:
* normalized(Tb - Ta) - w_aZb.point3()
*
* @addtogroup SFM
*/
class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
private:
typedef NoiseModelFactor2<Point3, Point3> Base;
Point3 measured_w_aZb_;
public:
/// default constructor
TranslationFactor() {}
TranslationFactor(Key a, Key b, const Unit3& w_aZb,
const SharedNoiseModel& noiseModel)
: Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
/**
* @brief Caclulate error: (norm(Tb - Ta) - measurement)
* where Tb and Ta are Point3 translations and measurement is
* the Unit3 translation direction from a to b.
*
* @param Ta translation for key a
* @param Tb translation for key b
* @param H1 optional jacobian in Ta
* @param H2 optional jacobian in Tb
* @return * Vector
*/
Vector evaluateError(
const Point3& Ta, const Point3& Tb,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir;
const Point3 predicted =
dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr);
if (H1) *H1 = -H_predicted_dir;
if (H2) *H2 = H_predicted_dir;
return predicted - measured_w_aZb_;
}
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Base", boost::serialization::base_object<Base>(*this));
}
}; // \ TranslationFactor
} // namespace gtsam

View File

@ -0,0 +1,103 @@
/* ----------------------------------------------------------------------------
* 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 TranslationRecovery.h
* @author Frank Dellaert
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/slam/PriorFactor.h>
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);
}
return graph;
}
void TranslationRecovery::addPrior(const double scale,
NonlinearFactorGraph* graph) const {
auto noiseModel = 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);
}
Values TranslationRecovery::initalizeRandomly() const {
// Create a lambda expression that checks whether value exists and randomly
// initializes if not.
Values initial;
auto insert = [&initial](Key j) {
if (!initial.exists(j)) {
initial.insert<Point3>(j, Vector3::Random());
}
};
// Loop over measurements and add a random translation
for (auto edge : relativeTranslations_) {
Key a, b;
tie(a, b) = edge.first;
insert(a);
insert(b);
}
return initial;
}
Values TranslationRecovery::run(const double scale) const {
auto graph = buildGraph();
addPrior(scale, &graph);
const Values initial = initalizeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();
return result;
}
TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
const Values& poses, const vector<KeyPair>& edges) {
TranslationEdges relativeTranslations;
for (auto edge : edges) {
Key a, b;
tie(a, b) = edge;
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;
}
return relativeTranslations;
}

View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* 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 TranslationRecovery.h
* @author Frank Dellaert
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <map>
#include <utility>
namespace gtsam {
// Set up an optimization problem for the unknown translations Ti in the world
// coordinate frame, given the known camera attitudes wRi with respect to the
// world frame, and a set of (noisy) translation directions of type Unit3,
// w_aZb. The measurement equation is
// w_aZb = Unit3(Tb - Ta) (1)
// i.e., w_aZb is the translation direction from frame A to B, in world
// coordinates. Although Unit3 instances live on a manifold, following
// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
// ambient world coordinate frame.
//
// It is clear that we cannot recover the scale, nor the absolute position,
// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
// the translations Ta and Tb associated with the first measurement w_aZb,
// clamping them to their initial values as given to this method. If no initial
// values are given, we use the origin for Tb and set Tb to make (1) come
// through, i.e.,
// Tb = s * wRa * Point3(w_aZb) (2)
// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
// versions are supplied below corresponding to whether we have initial values
// or not.
class TranslationRecovery {
public:
using KeyPair = std::pair<Key, Key>;
using TranslationEdges = std::map<KeyPair, Unit3>;
private:
TranslationEdges relativeTranslations_;
LevenbergMarquardtParams params_;
public:
/**
* @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.
* @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())
: relativeTranslations_(relativeTranslations), params_(lmParams) {
params_.setVerbosityLM("Summary");
}
/**
* @brief Build the factor graph to do the optimization.
*
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph buildGraph() const;
/**
* @brief Add priors on ednpoints of first measurement edge.
*
* @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;
/**
* @brief Create random initial translations.
*
* @return Values
*/
Values initalizeRandomly() const;
/**
* @brief Build and optimize factor graph.
*
* @param scale scale for first relative translation which fixes gauge.
* @return Values
*/
Values run(const double scale = 1.0) const;
/**
* @brief Simulate translation direction measurements
*
* @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.
*/
static TranslationEdges SimulateMeasurements(
const Values& poses, const std::vector<KeyPair>& edges);
};
} // namespace gtsam

View File

@ -0,0 +1,108 @@
/* ----------------------------------------------------------------------------
* 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 testTranslationFactor.cpp
* @brief Unit tests for TranslationFactor Class
* @author Frank dellaert
* @date March 2020
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
// Create a noise model for the chordal error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05));
// Keys are deliberately *not* in sorted order to test that case.
static const Key kKey1(2), kKey2(1);
static const Unit3 kMeasured(1, 0, 0);
/* ************************************************************************* */
TEST(TranslationFactor, Constructor) {
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
}
/* ************************************************************************* */
TEST(TranslationFactor, ZeroError) {
// Create a factor
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
// Set the linearization
Point3 T1(1, 0, 0), T2(2, 0, 0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(T1, T2));
// Verify we get the expected error
Vector expected = (Vector3() << 0, 0, 0).finished();
EXPECT(assert_equal(expected, actualError, 1e-9));
}
/* ************************************************************************* */
TEST(TranslationFactor, NonZeroError) {
// create a factor
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
// set the linearization
Point3 T1(0, 1, 1), T2(0, 2, 2);
// use the factor to calculate the error
Vector actualError(factor.evaluateError(T1, T2));
// verify we get the expected error
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) {
return factor.evaluateError(T1, T2);
}
TEST(TranslationFactor, Jacobian) {
// Create a factor
TranslationFactor factor(kKey1, kKey2, kMeasured, model);
// Set the linearization
Point3 T1(1, 0, 0), T2(2, 0, 0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(T1, T2, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError, _1, T2, factor), T1);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError, T1, _1, factor), T2);
// Verify the Jacobians are correct
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -0,0 +1,87 @@
/* ----------------------------------------------------------------------------
* 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 testTranslationRecovery.cpp
* @author Frank Dellaert
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/sfm/TranslationRecovery.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// We read the BAL file, which has 3 cameras in it, with poses. We then assume
// the rotations are correct, but translations have to be estimated from
// translation directions only. Since we have 3 cameras, A, B, and C, we can at
// most create three relative measurements, let's call them w_aZb, w_aZc, and
// bZc. These will be of type Unit3. We then call `recoverTranslations` which
// sets up an optimization problem for the three unknown translations.
TEST(TranslationRecovery, BAL) {
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData db;
bool success = readBAL(filename, db);
if (!success) throw runtime_error("Could not access file!");
// Get camera poses, as Values
size_t j = 0;
Values poses;
for (auto camera : db.cameras) {
poses.insert(j++, camera.pose());
}
// Simulate measurements
const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
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));
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
// Translation recovery, version 1
const double scale = 2.0;
const auto result = algorithm.run(scale);
// 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)));
// Check that the third translations is correct
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?
// EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */