From 0bd81430573a32712642a546880600f498cac745 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 19 Jun 2020 23:33:29 -0400 Subject: [PATCH] Importing Frobenius error factors from Shonan effort --- cython/gtsam/tests/test_FrobeniusFactor.py | 56 +++++ gtsam.h | 29 +++ gtsam/slam/FrobeniusFactor.cpp | 117 ++++++++++ gtsam/slam/FrobeniusFactor.h | 145 ++++++++++++ gtsam/slam/tests/testFrobeniusFactor.cpp | 244 +++++++++++++++++++++ timing/timeFrobeniusFactor.cpp | 110 ++++++++++ 6 files changed, 701 insertions(+) create mode 100644 cython/gtsam/tests/test_FrobeniusFactor.py create mode 100644 gtsam/slam/FrobeniusFactor.cpp create mode 100644 gtsam/slam/FrobeniusFactor.h create mode 100644 gtsam/slam/tests/testFrobeniusFactor.cpp create mode 100644 timing/timeFrobeniusFactor.cpp diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py new file mode 100644 index 000000000..82d1fb410 --- /dev/null +++ b/cython/gtsam/tests/test_FrobeniusFactor.py @@ -0,0 +1,56 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +FrobeniusFactor unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error, invalid-name +import unittest + +import numpy as np +from gtsam import (SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, + FrobeniusWormholeFactor, SOn) + +id = SO4() +v1 = np.array([0, 0, 0, 0.1, 0, 0]) +Q1 = SO4.Expmap(v1) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q2 = SO4.Expmap(v2) + + +class TestFrobeniusFactorSO4(unittest.TestCase): + """Test FrobeniusFactor factors.""" + + def test_frobenius_factor(self): + """Test creation of a factor that calculates the Frobenius norm.""" + factor = FrobeniusFactorSO4(1, 2) + actual = factor.evaluateError(Q1, Q2) + expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,)) + np.testing.assert_array_equal(actual, expected) + + def test_frobenius_between_factor(self): + """Test creation of a Frobenius BetweenFactor.""" + factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2)) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((16,)) + np.testing.assert_array_almost_equal(actual, expected) + + def test_frobenius_wormhole_factor(self): + """Test creation of a factor that calculates Shonan error.""" + R1 = SO3.Expmap(v1[3:]) + R2 = SO3.Expmap(v2[3:]) + factor = FrobeniusWormholeFactor(1, 2, R1.between(R2), p=4) + I4 = SOn(4) + Q1 = I4.retract(v1) + Q2 = I4.retract(v2) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((12,)) + np.testing.assert_array_almost_equal(actual, expected, decimal=4) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index 614db91c7..cbf737390 100644 --- a/gtsam.h +++ b/gtsam.h @@ -598,6 +598,7 @@ class SOn { // Standard Constructors SOn(size_t n); static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); // Testable void print(string s) const; @@ -2835,6 +2836,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; +#include +gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel( + gtsam::noiseModel::Base* model, size_t d); + +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor { + FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12, + size_t p); + FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12, + size_t p, gtsam::noiseModel::Base* model); + Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); +}; + //************************************************************************* // Navigation //************************************************************************* diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp new file mode 100644 index 000000000..904addb03 --- /dev/null +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 FrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Various factors that minimize some Frobenius norm + */ + +#include + +#include +#include +#include + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +//****************************************************************************** +boost::shared_ptr ConvertPose3NoiseModel( + const SharedNoiseModel& model, size_t d, bool defaultToUnit) { + double sigma = 1.0; + if (model != nullptr) { + if (model->dim() != 6) { + if (!defaultToUnit) + throw std::runtime_error("Can only convert Pose3 noise models"); + } else { + auto sigmas = model->sigmas().head(3).eval(); + if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) { + if (!defaultToUnit) + throw std::runtime_error("Can only convert isotropic rotation noise"); + } else { + sigma = sigmas(0); + } + } + } + return noiseModel::Isotropic::Sigma(d, sigma); +} + +//****************************************************************************** +FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, + size_t p, + const SharedNoiseModel& model) + : NoiseModelFactor2(ConvertPose3NoiseModel(model, p * 3), j1, j2), + M_(R12.matrix()), // 3*3 in all cases + p_(p), // 4 for SO(4) + pp_(p * p), // 16 for SO(4) + dimension_(SOn::Dimension(p)), // 6 for SO(4) + G_(pp_, dimension_) // 16*6 for SO(4) +{ + // Calculate G matrix of vectorized generators + Matrix Z = Matrix::Zero(p, p); + for (size_t j = 0; j < dimension_; j++) { + const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j)); + G_.col(j) = Eigen::Map(X.data(), pp_, 1); + } + assert(noiseModel()->dim() == 3 * p_); +} + +//****************************************************************************** +Vector FrobeniusWormholeFactor::evaluateError( + const SOn& Q1, const SOn& Q2, boost::optional H1, + boost::optional H2) const { + gttic(FrobeniusWormholeFactorP_evaluateError); + + const Matrix& M1 = Q1.matrix(); + const Matrix& M2 = Q2.matrix(); + assert(M1.rows() == p_ && M2.rows() == p_); + + const size_t dim = 3 * p_; // Stiefel manifold dimension + Vector fQ2(dim), hQ1(dim); + + // Vectorize and extract only d leftmost columns, i.e. vec(M2*P) + fQ2 << Eigen::Map(M2.data(), dim, 1); + + // Vectorize M1*P*R12 + const Matrix Q1PR12 = M1.leftCols<3>() * M_; + hQ1 << Eigen::Map(Q1PR12.data(), dim, 1); + + // If asked, calculate Jacobian as (M \otimes M1) * G + if (H1) { + const size_t p2 = 2 * p_; + Matrix RPxQ = Matrix::Zero(dim, pp_); + RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0); + RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1); + RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2); + *H1 = -RPxQ * G_; + } + if (H2) { + const size_t p2 = 2 * p_; + Matrix PxQ = Matrix::Zero(dim, pp_); + PxQ.block(0, 0, p_, p_) = M2; + PxQ.block(p_, p_, p_, p_) = M2; + PxQ.block(p2, p2, p_, p_) = M2; + *H2 = PxQ * G_; + } + + return fQ2 - hQ1; +} + +//****************************************************************************** + +} // namespace gtsam diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h new file mode 100644 index 000000000..60fa82ab4 --- /dev/null +++ b/gtsam/slam/FrobeniusFactor.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 FrobeniusFactor.h + * @date March 2019 + * @author Frank Dellaert + * @brief Various factors that minimize some Frobenius norm + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3 + * BetweenFactor noise model into an 9 or 16-dimensional isotropic noise + * model used to weight the Frobenius norm. If the noise model passed is + * null we return a Dim-dimensional isotropic noise model with sigma=1.0. If + * not, we we check if the 3-dimensional noise model on rotations is + * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an + * error. If defaultToUnit == false throws an exception on unexepcted input. + */ +boost::shared_ptr ConvertPose3NoiseModel( + const SharedNoiseModel& model, size_t d, bool defaultToUnit = true); + +/** + * FrobeniusPrior calculates the Frobenius norm between a given matrix and an + * element of SO(3) or SO(4). + */ +template +class FrobeniusPrior : public NoiseModelFactor1 { + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + using MatrixNN = typename Rot::MatrixNN; + Eigen::Matrix vecM_; ///< vectorized matrix to approximate + + public: + /// Constructor + FrobeniusPrior(Key j, const MatrixNN& M, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactor1(ConvertPose3NoiseModel(model, Dim), j) { + vecM_ << Eigen::Map(M.data(), Dim, 1); + } + + /// Error is just Frobenius norm between Rot element and vectorized matrix M. + Vector evaluateError(const Rot& R, + boost::optional H = boost::none) const { + return R.vec(H) - vecM_; // Jacobian is computed only when needed. + } +}; + +/** + * FrobeniusFactor calculates the Frobenius norm between rotation matrices. + * The template argument can be any fixed-size SO. + */ +template +class FrobeniusFactor : public NoiseModelFactor2 { + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + + public: + /// Constructor + FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) + : NoiseModelFactor2(ConvertPose3NoiseModel(model, Dim), j1, + j2) {} + + /// Error is just Frobenius norm between rotation matrices. + Vector evaluateError(const Rot& R1, const Rot& R2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Vector error = R2.vec(H2) - R1.vec(H1); + if (H1) *H1 = -*H1; + return error; + } +}; + +/** + * FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm + * of the rotation error between measured and predicted (rather than the + * Logmap of the error). This factor is only defined for fixed-dimension types, + * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. + */ +template +class FrobeniusBetweenFactor : public NoiseModelFactor2 { + Rot R12_; ///< measured rotation between R1 and R2 + Eigen::Matrix + R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + + public: + /// Constructor + FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactor2( + ConvertPose3NoiseModel(model, Dim), j1, j2), + R12_(R12), + R2hat_H_R1_(R12.inverse().AdjointMap()) {} + + /// Error is Frobenius norm between R1*R12 and R2. + Vector evaluateError(const Rot& R1, const Rot& R2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + const Rot R2hat = R1.compose(R12_); + Eigen::Matrix vec_H_R2hat; + Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); + if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; + return error; + } +}; + +/** + * FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will + * land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects + * the SO(p) matrices down to a Stiefel manifold of p*d matrices. + * TODO(frank): template on D=2 or 3 + */ +class FrobeniusWormholeFactor : public NoiseModelFactor2 { + Matrix M_; ///< measured rotation between R1 and R2 + size_t p_, pp_, dimension_; ///< dimensionality constants + Matrix G_; ///< matrix of vectorized generators + + public: + /// Constructor. Note we convert to 3*p-dimensional noise model. + FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4, + const SharedNoiseModel& model = nullptr); + + /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] + /// projects down from SO(p) to the Stiefel manifold of px3 matrices. + Vector evaluateError(const SOn& Q1, const SOn& Q2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; +}; + +} // namespace gtsam diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp new file mode 100644 index 000000000..3aefeeb1a --- /dev/null +++ b/gtsam/slam/tests/testFrobeniusFactor.cpp @@ -0,0 +1,244 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 + + * -------------------------------------------------------------------------- */ + +/** + * testFrobeniusFactor.cpp + * + * @file testFrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Check evaluateError for various Frobenius norm + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +namespace so3 { +SO3 id; +Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished(); +SO3 R1 = SO3::Expmap(v1); +Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished(); +SO3 R2 = SO3::Expmap(v2); +SO3 R12 = R1.between(R2); +} // namespace so3 + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusPrior(1, R2.matrix()); + Vector actual = factor.evaluateError(R1); + Vector expected = R1.vec() - R2.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, ClosestTo) { + // Example top-left of SO(4) matrix not quite on SO(3) manifold + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + SO3 expected = SO3::ClosestTo(M); + + // manifold optimization gets same result as SVD solution in ClosestTo + NonlinearFactorGraph graph; + graph.emplace_shared >(1, M); + + Values initial; + initial.insert(1, SO3(I_3x3)); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6); + EXPECT(assert_equal(expected, result.at(1), 1e-6)); +} + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, ChordalL2mean) { + // See Hartley13ijcv: + // Cost function C(R) = \sum FrobeniusPrior(R,R_i) + // Closed form solution = ClosestTo(C_e), where + // C_e = \sum R_i !!!! + + // We will test by computing mean of R1=exp(v1) R1^T=exp(-v1): + using namespace ::so3; + SO3 expected; // identity + Matrix3 M = R1.matrix() + R1.matrix().transpose(); + EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6)); + EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6)); + + // manifold optimization gets same result as ChordalMean + NonlinearFactorGraph graph; + graph.emplace_shared >(1, R1.matrix()); + graph.emplace_shared >(1, R1.matrix().transpose()); + + Values initial; + initial.insert(1, R1.inverse()); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1); // Why so loose? + EXPECT(assert_equal(expected, result.at(1), 1e-5)); +} + +/* ************************************************************************* */ +TEST(FrobeniusFactorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusFactor(1, 2); + Vector actual = factor.evaluateError(R1, R2); + Vector expected = R2.vec() - R1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + values.insert(2, R2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +// Commented out as SO(n) not yet supported (and might never be) +// TEST(FrobeniusBetweenFactorSOn, evaluateError) { +// using namespace ::so3; +// auto factor = +// FrobeniusBetweenFactor(1, 2, SOn::FromMatrix(R12.matrix())); +// Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()), +// SOn::FromMatrix(R2.matrix())); +// Vector expected = Vector9::Zero(); +// EXPECT(assert_equal(expected, actual, 1e-9)); + +// Values values; +// values.insert(1, R1); +// values.insert(2, R2); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +// } + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusBetweenFactor(1, 2, R12); + Vector actual = factor.evaluateError(R1, R2); + Vector expected = Vector9::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + values.insert(2, R2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +//****************************************************************************** +namespace so4 { +SO4 id; +Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished(); +SO4 Q2 = SO4::Expmap(v2); +} // namespace so4 + +/* ************************************************************************* */ +TEST(FrobeniusFactorSO4, evaluateError) { + using namespace ::so4; + auto factor = FrobeniusFactor(1, 2, noiseModel::Unit::Create(6)); + Vector actual = factor.evaluateError(Q1, Q2); + Vector expected = Q2.vec() - Q1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorSO4, evaluateError) { + using namespace ::so4; + Matrix4 M{I_4x4}; + M.topLeftCorner<3, 3>() = ::so3::R12.matrix(); + auto factor = FrobeniusBetweenFactor(1, 2, Q1.between(Q2)); + Matrix H1, H2; + Vector actual = factor.evaluateError(Q1, Q2, H1, H2); + Vector expected = SO4::VectorN2::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +//****************************************************************************** +namespace submanifold { +SO4 id; +Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); +SO3 R1 = SO3::Expmap(v1.tail<3>()); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished(); +SO3 R2 = SO3::Expmap(v2.tail<3>()); +SO4 Q2 = SO4::Expmap(v2); +SO3 R12 = R1.between(R2); +} // namespace submanifold + +/* ************************************************************************* */ +TEST(FrobeniusWormholeFactor, evaluateError) { + auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16 + for (const size_t p : {5, 4, 3}) { + Matrix M = Matrix::Identity(p, p); + M.topLeftCorner(3, 3) = submanifold::R1.matrix(); + SOn Q1(M); + M.topLeftCorner(3, 3) = submanifold::R2.matrix(); + SOn Q2(M); + auto factor = + FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model); + Matrix H1, H2; + factor.evaluateError(Q1, Q2, H1, H2); + + // Test derivatives + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + } +} + +/* ************************************************************************* */ +TEST(FrobeniusWormholeFactor, equivalenceToSO3) { + using namespace ::submanifold; + auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1)); + auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension + auto factor3 = FrobeniusBetweenFactor(1, 2, R12, model); + auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model); + const Eigen::Map E3(factor3.evaluateError(R1, R2).data()); + const Eigen::Map E4( + factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data()); + EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9)); + EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp new file mode 100644 index 000000000..c8bdd8fec --- /dev/null +++ b/timing/timeFrobeniusFactor.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeFrobeniusFactor.cpp + * @brief time FrobeniusFactor with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); + +int main(int argc, char* argv[]) { + // primitive argument parsing: + if (argc > 3) { + throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]"); + } + + string g2oFile; + try { + if (argc > 1) + g2oFile = argv[argc - 1]; + else + g2oFile = + "/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/" + "datasets/randomTorus3D.g2o"; + // g2oFile = findExampleDataFile("sphere_smallnoise.graph"); + } catch (const exception& e) { + cerr << e.what() << '\n'; + exit(1); + } + + // Read G2O file + const auto factors = parse3DFactors(g2oFile); + const auto poses = parse3DPoses(g2oFile); + + // Build graph + NonlinearFactorGraph graph; + // graph.add(NonlinearEquality(0, SO4())); + auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); + graph.add(PriorFactor(0, SO4(), priorModel)); + for (const auto& factor : factors) { + const auto& keys = factor->keys(); + const auto& Tij = factor->measured(); + const auto& model = factor->noiseModel(); + graph.emplace_shared( + keys[0], keys[1], SO3(Tij.rotation().matrix()), model); + } + + boost::mt19937 rng(42); + + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetCeresDefaults(¶ms); + params.setLinearSolverType("MULTIFRONTAL_QR"); + // params.setVerbosityLM("SUMMARY"); + // params.linearSolverType = LevenbergMarquardtParams::Iterative; + // auto pcg = boost::make_shared(); + // pcg->preconditioner_ = + // boost::make_shared(); + // boost::make_shared(); + // params.iterativeParams = pcg; + + // Optimize + for (size_t i = 0; i < 100; i++) { + gttic_(optimize); + Values initial; + initial.insert(0, SO4()); + for (size_t j = 1; j < poses.size(); j++) { + initial.insert(j, SO4::Random(rng)); + } + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + cout << "cost = " << graph.error(result) << endl; + } + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +}