Merge branch 'develop' into feature/python-install
						commit
						db40cd71fc
					
				|  | @ -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() | ||||
|  | @ -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() | ||||
|     unittest.main() | ||||
|  |  | |||
|  | @ -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. | ||||
|  |  | |||
							
								
								
									
										29
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										29
									
								
								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/slam/FrobeniusFactor.h> | ||||
| gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel( | ||||
|     gtsam::noiseModel::Base* model, size_t d); | ||||
| 
 | ||||
| template<T = {gtsam::SO3, gtsam::SO4}> | ||||
| 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<T = {gtsam::SO3, gtsam::SO4}> | ||||
| 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
 | ||||
| //*************************************************************************
 | ||||
|  |  | |||
|  | @ -28,6 +28,7 @@ | |||
| #include <cstdint> | ||||
| 
 | ||||
| #include <exception> | ||||
| #include <string> | ||||
| 
 | ||||
| #ifdef GTSAM_USE_TBB | ||||
| #include <tbb/scalable_allocator.h> | ||||
|  | @ -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; | ||||
|  |  | |||
|  | @ -169,6 +169,12 @@ class ExecutionTrace { | |||
|       content.ptr->reverseAD2(dTdA, jacobians); | ||||
|   } | ||||
| 
 | ||||
|   ~ExecutionTrace() { | ||||
|     if (kind == Function) { | ||||
|       content.ptr->~CallRecord<Dim>(); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Define type so we can apply it as a meta-function
 | ||||
|   typedef ExecutionTrace<T> type; | ||||
| }; | ||||
|  |  | |||
|  | @ -16,6 +16,7 @@ | |||
|  * @brief unit tests for Expression internals | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/nonlinear/internal/CallRecord.h> | ||||
| #include <gtsam/nonlinear/internal/ExecutionTrace.h> | ||||
| #include <gtsam/geometry/Point2.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 <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 = 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 <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
 | ||||
|  | @ -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; | ||||
| } | ||||
|  | @ -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
 | ||||
|  | @ -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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -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 <gtsam/slam/FrobeniusFactor.h> | ||||
| 
 | ||||
| #include <gtsam/base/timing.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| 
 | ||||
| #include <cmath> | ||||
| #include <iostream> | ||||
| #include <vector> | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| boost::shared_ptr<noiseModel::Isotropic> 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<SOn, SOn>(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<const Matrix>(X.data(), pp_, 1); | ||||
|   } | ||||
|   assert(noiseModel()->dim() == 3 * p_); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| Vector FrobeniusWormholeFactor::evaluateError( | ||||
|     const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> 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<const Matrix>(M2.data(), dim, 1); | ||||
| 
 | ||||
|   // Vectorize M1*P*R12
 | ||||
|   const Matrix Q1PR12 = M1.leftCols<3>() * M_; | ||||
|   hQ1 << Eigen::Map<const Matrix>(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
 | ||||
|  | @ -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 <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/geometry/SOn.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| 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<noiseModel::Isotropic> 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 Rot> | ||||
| class FrobeniusPrior : public NoiseModelFactor1<Rot> { | ||||
|   enum { Dim = Rot::VectorN2::RowsAtCompileTime }; | ||||
|   using MatrixNN = typename Rot::MatrixNN; | ||||
|   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 | ||||
| 
 | ||||
|  public: | ||||
|   /// Constructor
 | ||||
|   FrobeniusPrior(Key j, const MatrixNN& M, | ||||
|                  const SharedNoiseModel& model = nullptr) | ||||
|       : NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) { | ||||
|     vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1); | ||||
|   } | ||||
| 
 | ||||
|   /// Error is just Frobenius norm between Rot element and vectorized matrix M.
 | ||||
|   Vector evaluateError(const Rot& R, | ||||
|                        boost::optional<Matrix&> 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<N>. | ||||
|  */ | ||||
| template <class Rot> | ||||
| class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> { | ||||
|   enum { Dim = Rot::VectorN2::RowsAtCompileTime }; | ||||
| 
 | ||||
|  public: | ||||
|   /// Constructor
 | ||||
|   FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) | ||||
|       : NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1, | ||||
|                                     j2) {} | ||||
| 
 | ||||
|   /// Error is just Frobenius norm between rotation matrices.
 | ||||
|   Vector evaluateError(const Rot& R1, const Rot& R2, | ||||
|                        boost::optional<Matrix&> H1 = boost::none, | ||||
|                        boost::optional<Matrix&> 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<N>::AdjointMap. | ||||
|  */ | ||||
| template <class Rot> | ||||
| class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> { | ||||
|   Rot R12_;  ///< measured rotation between R1 and R2
 | ||||
|   Eigen::Matrix<double, Rot::dimension, Rot::dimension> | ||||
|       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<Rot, Rot>( | ||||
|             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<Matrix&> H1 = boost::none, | ||||
|                        boost::optional<Matrix&> H2 = boost::none) const { | ||||
|     const Rot R2hat = R1.compose(R12_); | ||||
|     Eigen::Matrix<double, Dim, Rot::dimension> 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<SOn, SOn> { | ||||
|   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<Matrix&> H1 = boost::none, | ||||
|                        boost::optional<Matrix&> H2 = boost::none) const; | ||||
| }; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -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 <gtsam/base/lieProxies.h> | ||||
| #include <gtsam/base/testLie.h> | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/geometry/SO3.h> | ||||
| #include <gtsam/geometry/SO4.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/factorTesting.h> | ||||
| #include <gtsam/slam/FrobeniusFactor.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| 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<SO3>(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<FrobeniusPrior<SO3> >(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<SO3>(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<FrobeniusPrior<SO3> >(1, R1.matrix()); | ||||
|   graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix().transpose()); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<SO3>(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<SO3>(1), 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(FrobeniusFactorSO3, evaluateError) { | ||||
|   using namespace ::so3; | ||||
|   auto factor = FrobeniusFactor<SO3>(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<SOn>(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<SO3>(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<SO4>(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<SO4>(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<SO3>(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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -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 <gtsam/base/timing.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/geometry/SO4.h> | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| #include <gtsam/linear/PCGSolver.h> | ||||
| #include <gtsam/linear/SubgraphPreconditioner.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/NonlinearEquality.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/slam/FrobeniusFactor.h> | ||||
| 
 | ||||
| #include <iostream> | ||||
| #include <string> | ||||
| #include <vector> | ||||
| 
 | ||||
| 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<SO4>(0, SO4()));
 | ||||
|   auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); | ||||
|   graph.add(PriorFactor<SO4>(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<FrobeniusWormholeFactor>( | ||||
|         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<PCGSolverParameters>();
 | ||||
|   // pcg->preconditioner_ =
 | ||||
|   // boost::make_shared<SubgraphPreconditionerParameters>();
 | ||||
|   // boost::make_shared<BlockJacobiPreconditionerParameters>();
 | ||||
|   // 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; | ||||
| } | ||||
		Loading…
	
		Reference in New Issue