diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py new file mode 100644 index 000000000..f3f5354bb --- /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 (Rot3, 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, Rot3(R1.between(R2).matrix()), 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/cython/gtsam/tests/test_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py index c857a6f7d..69665db65 100644 --- a/cython/gtsam/tests/test_logging_optimizer.py +++ b/cython/gtsam/tests/test_logging_optimizer.py @@ -4,6 +4,12 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name +import sys +if sys.version_info.major >= 3: + from io import StringIO +else: + from cStringIO import StringIO + import unittest from datetime import datetime @@ -37,11 +43,19 @@ class TestOptimizeComet(GtsamTestCase): self.optimizer = gtsam.GaussNewtonOptimizer( graph, initial, self.params) + # setup output capture + self.capturedOutput = StringIO() + sys.stdout = self.capturedOutput + + def tearDown(self): + """Reset print capture.""" + sys.stdout = sys.__stdout__ + def test_simple_printing(self): """Test with a simple hook.""" # Provide a hook that just prints - def hook(_, error: float): + def hook(_, error): print(error) # Only thing we require from optimizer is an iterate method @@ -65,7 +79,7 @@ class TestOptimizeComet(GtsamTestCase): + str(time.hour)+":"+str(time.minute)+":"+str(time.second)) # I want to do some comet thing here - def hook(optimizer, error: float): + def hook(optimizer, error): comet.log_metric("Karcher error", error, optimizer.iterations()) @@ -76,4 +90,4 @@ class TestOptimizeComet(GtsamTestCase): self.gtsamAssertEquals(actual.atRot3(KEY), self.expected) if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() diff --git a/cython/gtsam/utils/logging_optimizer.py b/cython/gtsam/utils/logging_optimizer.py index b201bb8aa..939453927 100644 --- a/cython/gtsam/utils/logging_optimizer.py +++ b/cython/gtsam/utils/logging_optimizer.py @@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name -from typing import TypeVar - from gtsam import NonlinearOptimizer, NonlinearOptimizerParams import gtsam -T = TypeVar('T') - -def optimize(optimizer: T, check_convergence, hook): +def optimize(optimizer, check_convergence, hook): """ Given an optimizer and a convergence check, iterate until convergence. After each iteration, hook(optimizer, error) is called. After the function, use values and errors to get the result. @@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook): current_error = new_error -def gtsam_optimize(optimizer: NonlinearOptimizer, - params: NonlinearOptimizerParams, +def gtsam_optimize(optimizer, + params, hook): """ Given an optimizer and params, iterate until convergence. After each iteration, hook(optimizer) is called. diff --git a/gtsam.h b/gtsam.h index cf6f658a1..37d56bce9 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/base/types.h b/gtsam/base/types.h index c5dac9ab7..aaada3cee 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -28,6 +28,7 @@ #include #include +#include #ifdef GTSAM_USE_TBB #include @@ -54,7 +55,7 @@ namespace gtsam { /// Function to demangle type name of variable, e.g. demangle(typeid(x).name()) - std::string demangle(const char* name); + std::string GTSAM_EXPORT demangle(const char* name); /// Integer nonlinear key type typedef std::uint64_t Key; diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ace0aaea8..2943b5e68 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -169,6 +169,12 @@ class ExecutionTrace { content.ptr->reverseAD2(dTdA, jacobians); } + ~ExecutionTrace() { + if (kind == Function) { + content.ptr->~CallRecord(); + } + } + /// Define type so we can apply it as a meta-function typedef ExecutionTrace type; }; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index c2b245780..58f76089a 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -16,6 +16,7 @@ * @brief unit tests for Expression internals */ +#include #include #include diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h new file mode 100644 index 000000000..d63633d7e --- /dev/null +++ b/gtsam/sfm/TranslationFactor.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include + +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 { + private: + typedef NoiseModelFactor2 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 H1 = boost::none, + boost::optional H2 = boost::none) const override { + const Point3 dir = Tb - Ta; + Matrix33 H_predicted_dir; + const Point3 predicted = normalize(dir, 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 + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ TranslationFactor +} // namespace gtsam diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp new file mode 100644 index 000000000..aeeae688f --- /dev/null +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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 >(a, Point3(0, 0, 0), noiseModel); + graph->emplace_shared >(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(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& edges) { + TranslationEdges relativeTranslations; + for (auto edge : edges) { + Key a, b; + tie(a, b) = edge; + const Pose3 wTa = poses.at(a), wTb = poses.at(b); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb(Tb - Ta); + relativeTranslations[edge] = w_aZb; + } + return relativeTranslations; +} diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h new file mode 100644 index 000000000..bb3c3cdb1 --- /dev/null +++ b/gtsam/sfm/TranslationRecovery.h @@ -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 +#include +#include + +#include +#include + +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; + using TranslationEdges = std::map; + + 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& edges); +}; +} // namespace gtsam diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp new file mode 100644 index 000000000..37e8b6c0f --- /dev/null +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -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 +#include +#include + +#include + +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( + boost::bind(&factorError, _1, T2, factor), T1); + H2Expected = numericalDerivative11( + 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); +} +/* ************************************************************************* */ 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..9cb0c19fa --- /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 Matrix3 E3(factor3.evaluateError(R1, R2).data()); + const Matrix43 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/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp new file mode 100644 index 000000000..5a98c3bf5 --- /dev/null +++ b/tests/testTranslationRecovery.cpp @@ -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 + +#include +#include + +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(0), wTb = poses.at(1), + wTc = poses.at(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(0))); + EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at(1))); + + // Check that the third translations is correct + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + EXPECT(assert_equal(expected, result.at(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); +} +/* ************************************************************************* */ 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; +}