diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h new file mode 100644 index 000000000..cfe071ee5 --- /dev/null +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 KarcherMeanFactor.cpp + * @author Frank Dellaert + * @date March 2019 + */ + +#pragma once + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +template +T FindKarcherMean(const vector& rotations) { + // Cost function C(R) = \sum PriorFactor(R_i)::error(R) + // No closed form solution. + NonlinearFactorGraph graph; + static const Key kKey(0); + for (const auto& R : rotations) { + graph.emplace_shared >(kKey, R); + } + Values initial; + initial.insert(kKey, T()); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + return result.at(kKey); +} + +template +template +KarcherMeanFactor::KarcherMeanFactor(const CONTAINER& keys, int d) + : NonlinearFactor(keys) { + if (d <= 0) { + throw std::invalid_argument( + "KarcherMeanFactor needs dimension for dynamic types."); + } + // Create the constant Jacobian made of D*D identity matrices, + // where D is the dimensionality of the manifold. + const auto I = Eigen::MatrixXd::Identity(d, d); + std::map terms; + for (Key j : keys) { + terms[j] = I; + } + jacobian_ = + boost::make_shared(terms, Eigen::VectorXd::Zero(d)); +} +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h new file mode 100644 index 000000000..d0f2f57db --- /dev/null +++ b/gtsam/slam/KarcherMeanFactor.h @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 KarcherMeanFactor.h + * @author Frank Dellaert + * @date March 2019ƒ + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { +/** + * Optimize for the Karcher mean, minimizing the geodesic distance to each of + * the given rotations, ,by constructing a factor graph out of simple + * PriorFactors. + */ +template +T FindKarcherMean(const std::vector& rotations); + +/** + * The KarcherMeanFactor creates a constraint on all SO(3) variables with + * given keys that the Karcher mean (see above) will stay the same. Note the + * mean itself is irrelevant to the constraint and is not a parameter: the + * constraint is implemented as enforcing that the sum of local updates is + * equal to zero, hence creating a rank-3 constraint. Note it is implemented as + * a soft constraint, as typically it is used to fix a gauge freedom. + * */ +template +class KarcherMeanFactor : public NonlinearFactor { + /// Constant Jacobian made of d*d identity matrices + boost::shared_ptr jacobian_; + + enum {D = traits::dimension}; + + public: + /// Construct from given keys. + template + KarcherMeanFactor(const CONTAINER& keys, int d=D); + + /// Destructor + virtual ~KarcherMeanFactor() {} + + /// Calculate the error of the factor: always zero + double error(const Values& c) const override { return 0; } + + /// get the dimension of the factor (number of rows on linearization) + size_t dim() const override { return D; } + + /// linearize to a GaussianFactor + boost::shared_ptr linearize(const Values& c) const override { + return jacobian_; + } +}; +// \KarcherMeanFactor +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/tests/testKarcherMeanFactor.cpp b/gtsam/slam/tests/testKarcherMeanFactor.cpp new file mode 100644 index 000000000..bcc20c62a --- /dev/null +++ b/gtsam/slam/tests/testKarcherMeanFactor.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testKarcherMeanFactor.cpp + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Rot3 version +/* ************************************************************************* */ +static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0)); + +/* ************************************************************************* */ +// Check that optimizing for Karcher mean (which minimizes Between distance) +// gets correct result. +TEST(KarcherMean, FindRot3) { + std::vector rotations = {R, R.inverse()}; + Rot3 expected; + auto actual = FindKarcherMean(rotations); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +// Check that the InnerConstraint factor leaves the mean unchanged. +TEST(KarcherMean, FactorRot3) { + // Make a graph with two variables, one between, and one InnerConstraint + // The optimal result should satisfy the between, while moving the other + // variable to make the mean the same as before. + // Mean of R and R' is identity. Let's make a BetweenFactor making R21 = + // R*R*R, i.e. geodesic length is 3 rather than 2. + NonlinearFactorGraph graph; + graph.emplace_shared>(1, 2, R * R * R); + std::vector keys{1, 2}; + graph.emplace_shared>(keys); + + Values initial; + initial.insert(1, R.inverse()); + initial.insert(2, R); + const auto expected = FindKarcherMean({R, R.inverse()}); + + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + const auto actual = + FindKarcherMean({result.at(1), result.at(2)}); + EXPECT(assert_equal(expected, actual)); + EXPECT( + assert_equal(R * R * R, result.at(1).between(result.at(2)))); +} + +/* ************************************************************************* */ +// SO(4) version +/* ************************************************************************* */ +static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished()); + +/* ************************************************************************* */ +TEST(KarcherMean, FindSO4) { + std::vector rotations = {Q, Q.inverse()}; + auto expected = SO4(); //::ChordalMean(rotations); + auto actual = FindKarcherMean(rotations); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(KarcherMean, FactorSO4) { + NonlinearFactorGraph graph; + graph.emplace_shared>(1, 2, Q * Q * Q); + std::vector keys{1, 2}; + graph.emplace_shared>(keys); + + Values initial; + initial.insert(1, Q.inverse()); + initial.insert(2, Q); + const auto expected = FindKarcherMean({Q, Q.inverse()}); + + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + const auto actual = + FindKarcherMean({result.at(1), result.at(2)}); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(), + result.at(1).between(result.at(2)))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */