Merge pull request #453 from borglab/feature/better_frobenius_factors
Better frobenius factorsrelease/4.3a0
						commit
						0e6b208276
					
				
							
								
								
									
										1
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										1
									
								
								gtsam.h
								
								
								
								
							|  | @ -2369,6 +2369,7 @@ virtual class NonlinearOptimizer { | |||
|   double error() const; | ||||
|   int iterations() const; | ||||
|   gtsam::Values values() const; | ||||
|   gtsam::NonlinearFactorGraph graph() const; | ||||
|   gtsam::GaussianFactorGraph* iterate() const; | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -26,13 +26,13 @@ using namespace std; | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| // Implementation for N>5 just uses dynamic version
 | ||||
| // Implementation for N>=5 just uses dynamic version
 | ||||
| template <int N> | ||||
| typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) { | ||||
|   return SOn::Hat(xi); | ||||
| } | ||||
| 
 | ||||
| // Implementation for N>5 just uses dynamic version
 | ||||
| // Implementation for N>=5 just uses dynamic version
 | ||||
| template <int N> | ||||
| typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) { | ||||
|   return SOn::Vee(X); | ||||
|  | @ -99,12 +99,8 @@ typename SO<N>::VectorN2 SO<N>::vec( | |||
|   if (H) { | ||||
|     // Calculate P matrix of vectorized generators
 | ||||
|     // TODO(duy): Should we refactor this as the jacobian of Hat?
 | ||||
|     Matrix P = VectorizedGenerators(n); | ||||
|     const size_t d = dim(); | ||||
|     Matrix P(n2, d); | ||||
|     for (size_t j = 0; j < d; j++) { | ||||
|       const auto X = Hat(Eigen::VectorXd::Unit(d, j)); | ||||
|       P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1); | ||||
|     } | ||||
|     H->resize(n2, d); | ||||
|     for (size_t i = 0; i < n; i++) { | ||||
|       H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); | ||||
|  |  | |||
|  | @ -290,7 +290,34 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | |||
|    * */ | ||||
|   VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H = | ||||
|                    boost::none) const; | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
 | ||||
|   template <int N_ = N, typename = IsFixed<N_>> | ||||
|   static Matrix VectorizedGenerators() { | ||||
|     constexpr size_t N2 = static_cast<size_t>(N * N); | ||||
|     Matrix G(N2, dimension); | ||||
|     for (size_t j = 0; j < dimension; j++) { | ||||
|       const auto X = Hat(Vector::Unit(dimension, j)); | ||||
|       G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1); | ||||
|     } | ||||
|     return G; | ||||
|   } | ||||
| 
 | ||||
|   /// Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
 | ||||
|   template <int N_ = N, typename = IsDynamic<N_>> | ||||
|   static Matrix VectorizedGenerators(size_t n = 0) { | ||||
|     const size_t n2 = n * n, dim = Dimension(n); | ||||
|     Matrix G(n2, dim); | ||||
|     for (size_t j = 0; j < dim; j++) { | ||||
|       const auto X = Hat(Vector::Unit(dim, j)); | ||||
|       G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1); | ||||
|     } | ||||
|     return G; | ||||
|   } | ||||
| 
 | ||||
|   /// @{
 | ||||
|   /// @name Serialization
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   template <class Archive> | ||||
|   friend void save(Archive&, SO&, const unsigned int); | ||||
|  | @ -300,6 +327,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | |||
|   friend void serialize(Archive&, SO&, const unsigned int); | ||||
|   friend class boost::serialization::access; | ||||
|   friend class Rot3;  // for serialize
 | ||||
| 
 | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| using SOn = SO<Eigen::Dynamic>; | ||||
|  |  | |||
|  | @ -296,6 +296,8 @@ protected: | |||
|   typedef NoiseModelFactor1<VALUE> This; | ||||
| 
 | ||||
| public: | ||||
|   /// @name Constructors
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** Default constructor for I/O only */ | ||||
|   NoiseModelFactor1() {} | ||||
|  | @ -309,16 +311,23 @@ public: | |||
|    *  @param noiseModel shared pointer to noise model | ||||
|    *  @param key1 by which to look up X value in Values | ||||
|    */ | ||||
|   NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : | ||||
|     Base(noiseModel, cref_list_of<1>(key1)) {} | ||||
|   NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1) | ||||
|       : Base(noiseModel, cref_list_of<1>(key1)) {} | ||||
| 
 | ||||
|   /** Calls the 1-key specific version of evaluateError, which is pure virtual
 | ||||
|    *  so must be implemented in the derived class. | ||||
|   /// @}
 | ||||
|   /// @name NoiseModelFactor methods
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * Calls the 1-key specific version of evaluateError below, which is pure | ||||
|    * virtual so must be implemented in the derived class. | ||||
|    */ | ||||
|   Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override { | ||||
|     if(this->active(x)) { | ||||
|       const X& x1 = x.at<X>(keys_[0]); | ||||
|       if(H) { | ||||
|   Vector unwhitenedError( | ||||
|       const Values &x, | ||||
|       boost::optional<std::vector<Matrix> &> H = boost::none) const override { | ||||
|     if (this->active(x)) { | ||||
|       const X &x1 = x.at<X>(keys_[0]); | ||||
|       if (H) { | ||||
|         return evaluateError(x1, (*H)[0]); | ||||
|       } else { | ||||
|         return evaluateError(x1); | ||||
|  | @ -328,16 +337,22 @@ public: | |||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Virtual methods
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Override this method to finish implementing a unary factor. | ||||
|    *  If the optional Matrix reference argument is specified, it should compute | ||||
|    *  both the function evaluation and its derivative in X. | ||||
|    */ | ||||
|   virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H = | ||||
|       boost::none) const = 0; | ||||
|   virtual Vector | ||||
|   evaluateError(const X &x, | ||||
|                 boost::optional<Matrix &> H = boost::none) const = 0; | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|  |  | |||
|  | @ -105,14 +105,17 @@ public: | |||
|    */ | ||||
|   const Values& optimizeSafely(); | ||||
| 
 | ||||
|   /// return error
 | ||||
|   /// return error in current optimizer state
 | ||||
|   double error() const; | ||||
| 
 | ||||
|   /// return number of iterations
 | ||||
|   /// return number of iterations in current optimizer state
 | ||||
|   size_t iterations() const; | ||||
| 
 | ||||
|   /// return values
 | ||||
|   const Values& values() const; | ||||
|   /// return values in current optimizer state
 | ||||
|   const Values &values() const; | ||||
| 
 | ||||
|   /// return the graph with nonlinear factors
 | ||||
|   const NonlinearFactorGraph &graph() const { return graph_; } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -67,9 +67,11 @@ namespace gtsam { | |||
|       return boost::static_pointer_cast<gtsam::NonlinearFactor>( | ||||
|           gtsam::NonlinearFactor::shared_ptr(new This(*this))); } | ||||
| 
 | ||||
|     /** implement functions needed for Testable */ | ||||
|     /// @}
 | ||||
|     /// @name Testable
 | ||||
|     /// @{
 | ||||
| 
 | ||||
|     /** print */ | ||||
|     /// print with optional string
 | ||||
|     void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { | ||||
|       std::cout << s << "BetweenFactor(" | ||||
|           << keyFormatter(this->key1()) << "," | ||||
|  | @ -78,15 +80,17 @@ namespace gtsam { | |||
|       this->noiseModel_->print("  noise model: "); | ||||
|     } | ||||
| 
 | ||||
|     /** equals */ | ||||
|     /// assert equality up to a tolerance
 | ||||
|     bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { | ||||
|       const This *e =  dynamic_cast<const This*> (&expected); | ||||
|       return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol); | ||||
|     } | ||||
| 
 | ||||
|     /** implement functions needed to derive from Factor */ | ||||
|     /// @}
 | ||||
|     /// @name NoiseModelFactor2 methods 
 | ||||
|     /// @{
 | ||||
| 
 | ||||
|     /** vector of errors */ | ||||
|     /// evaluate error, returns vector of errors size of tangent space
 | ||||
|     Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 = | ||||
|       boost::none, boost::optional<Matrix&> H2 = boost::none) const override { | ||||
|       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
 | ||||
|  | @ -102,15 +106,15 @@ namespace gtsam { | |||
| #endif | ||||
|     } | ||||
| 
 | ||||
|     /** return the measured */ | ||||
|     /// @}
 | ||||
|     /// @name Standard interface 
 | ||||
|     /// @{
 | ||||
| 
 | ||||
|     /// return the measurement
 | ||||
|     const VALUE& measured() const { | ||||
|       return measured_; | ||||
|     } | ||||
| 
 | ||||
|     /** number of variables attached to this factor */ | ||||
|     std::size_t size() const { | ||||
|       return 2; | ||||
|     } | ||||
|     /// @}
 | ||||
| 
 | ||||
|   private: | ||||
| 
 | ||||
|  |  | |||
|  | @ -87,11 +87,6 @@ public: | |||
|     return measuredE_; | ||||
|   } | ||||
| 
 | ||||
|   /** number of variables attached to this factor */ | ||||
|   std::size_t size() const { | ||||
|     return 2; | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   /** Serialization function */ | ||||
|  |  | |||
|  | @ -52,23 +52,40 @@ boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel( | |||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, | ||||
|                                                  size_t p, | ||||
|                                                  const SharedNoiseModel& model) | ||||
| FrobeniusWormholeFactor::FrobeniusWormholeFactor( | ||||
|     Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model, | ||||
|     const boost::shared_ptr<Matrix> &G) | ||||
|     : 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); | ||||
|       M_(R12.matrix()), // 3*3 in all cases
 | ||||
|       p_(p),            // 4 for SO(4)
 | ||||
|       pp_(p * p),       // 16 for SO(4)
 | ||||
|       G_(G) { | ||||
|   if (noiseModel()->dim() != 3 * p_) | ||||
|     throw std::invalid_argument( | ||||
|         "FrobeniusWormholeFactor: model with incorrect dimension."); | ||||
|   if (!G) { | ||||
|     G_ = boost::make_shared<Matrix>(); | ||||
|     *G_ = SOn::VectorizedGenerators(p); // expensive!
 | ||||
|   } | ||||
|   assert(noiseModel()->dim() == 3 * p_); | ||||
|   if (G_->rows() != pp_ || G_->cols() != SOn::Dimension(p)) | ||||
|     throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators " | ||||
|                                 "of incorrect dimension."); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const { | ||||
|   std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << "," | ||||
|             << keyFormatter(key2()) << ")\n"; | ||||
|   traits<Matrix>::Print(M_, "  M: "); | ||||
|   noiseModel_->print("  noise model: "); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected, | ||||
|                                      double tol) const { | ||||
|   auto e = dynamic_cast<const FrobeniusWormholeFactor *>(&expected); | ||||
|   return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) && | ||||
|          p_ == e->p_ && M_ == e->M_; | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  | @ -98,7 +115,7 @@ Vector FrobeniusWormholeFactor::evaluateError( | |||
|     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_; | ||||
|     *H1 = -RPxQ * (*G_); | ||||
|   } | ||||
|   if (H2) { | ||||
|     const size_t p2 = 2 * p_; | ||||
|  | @ -106,7 +123,7 @@ Vector FrobeniusWormholeFactor::evaluateError( | |||
|     PxQ.block(0, 0, p_, p_) = M2; | ||||
|     PxQ.block(p_, p_, p_, p_) = M2; | ||||
|     PxQ.block(p2, p2, p_, p_) = M2; | ||||
|     *H2 = PxQ * G_; | ||||
|     *H2 = PxQ * (*G_); | ||||
|   } | ||||
| 
 | ||||
|   return fQ2 - hQ1; | ||||
|  |  | |||
|  | @ -92,14 +92,17 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> { | |||
|  * and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap. | ||||
|  */ | ||||
| template <class Rot> | ||||
| class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> { | ||||
| GTSAM_EXPORT 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
 | ||||
|   /// @name Constructor
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// Construct from two keys and measured rotation
 | ||||
|   FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, | ||||
|                          const SharedNoiseModel& model = nullptr) | ||||
|       : NoiseModelFactor2<Rot, Rot>( | ||||
|  | @ -107,6 +110,33 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> { | |||
|         R12_(R12), | ||||
|         R2hat_H_R1_(R12.inverse().AdjointMap()) {} | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Testable
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// print with optional string
 | ||||
|   void | ||||
|   print(const std::string &s, | ||||
|         const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { | ||||
|     std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name()) | ||||
|               << ">(" << keyFormatter(this->key1()) << "," | ||||
|               << keyFormatter(this->key2()) << ")\n"; | ||||
|     traits<Rot>::Print(R12_, "  R12: "); | ||||
|     this->noiseModel_->print("  noise model: "); | ||||
|   } | ||||
| 
 | ||||
|   /// assert equality up to a tolerance
 | ||||
|   bool equals(const NonlinearFactor &expected, | ||||
|               double tol = 1e-9) const override { | ||||
|     auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected); | ||||
|     return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) && | ||||
|            traits<Rot>::Equals(this->R12_, e->R12_, tol); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name NoiseModelFactor2 methods 
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// Error is Frobenius norm between R1*R12 and R2.
 | ||||
|   Vector evaluateError(const Rot& R1, const Rot& R2, | ||||
|                        boost::optional<Matrix&> H1 = boost::none, | ||||
|  | @ -117,6 +147,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> { | |||
|     if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; | ||||
|     return error; | ||||
|   } | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  | @ -125,21 +156,46 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> { | |||
|  * the SO(p) matrices down to a Stiefel manifold of p*d matrices. | ||||
|  * TODO(frank): template on D=2 or 3 | ||||
|  */ | ||||
| class GTSAM_EXPORT 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
 | ||||
| class GTSAM_EXPORT FrobeniusWormholeFactor | ||||
|     : public NoiseModelFactor2<SOn, SOn> { | ||||
|   Matrix M_;                    ///< measured rotation between R1 and R2
 | ||||
|   size_t p_, pp_;               ///< dimensionality constants
 | ||||
|   boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
 | ||||
| 
 | ||||
| public: | ||||
|   /// @name Constructor
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  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); | ||||
|   /// To save memory and mallocs, pass in the vectorized Lie algebra generators:
 | ||||
|   ///    G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
 | ||||
|   FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4, | ||||
|                           const SharedNoiseModel &model = nullptr, | ||||
|                           const boost::shared_ptr<Matrix> &G = nullptr); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Testable
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// print with optional string
 | ||||
|   void | ||||
|   print(const std::string &s, | ||||
|         const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override; | ||||
| 
 | ||||
|   /// assert equality up to a tolerance
 | ||||
|   bool equals(const NonlinearFactor &expected, | ||||
|               double tol = 1e-9) const override; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name NoiseModelFactor2 methods 
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// 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 override; | ||||
|   /// @}
 | ||||
| }; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -403,11 +403,6 @@ public: | |||
|     return measured_; | ||||
|   } | ||||
| 
 | ||||
|   /** number of variables attached to this factor */ | ||||
|   std::size_t size() const { | ||||
|     return 2; | ||||
|   } | ||||
| 
 | ||||
|   size_t dim() const override { | ||||
|     return model_inlier_->R().rows() + model_inlier_->R().cols(); | ||||
|   } | ||||
|  |  | |||
|  | @ -203,11 +203,6 @@ namespace gtsam { | |||
| 
 | ||||
|     /* ************************************************************************* */ | ||||
| 
 | ||||
|     /** number of variables attached to this factor */ | ||||
|     std::size_t size() const { | ||||
|       return 1; | ||||
|     } | ||||
| 
 | ||||
|     size_t dim() const override { | ||||
|       return model_->R().rows() + model_->R().cols(); | ||||
|     } | ||||
|  |  | |||
|  | @ -401,11 +401,6 @@ namespace gtsam { | |||
| 
 | ||||
|     /* ************************************************************************* */ | ||||
| 
 | ||||
|     /** number of variables attached to this factor */ | ||||
|     std::size_t size() const { | ||||
|       return 1; | ||||
|     } | ||||
| 
 | ||||
|     size_t dim() const override { | ||||
|       return model_inlier_->R().rows() + model_inlier_->R().cols(); | ||||
|     } | ||||
|  |  | |||
|  | @ -13,12 +13,11 @@ | |||
|  * @file    timeFrobeniusFactor.cpp | ||||
|  * @brief   time FrobeniusFactor with BAL file | ||||
|  * @author  Frank Dellaert | ||||
|  * @date    June 6, 2015 | ||||
|  * @date    2019 | ||||
|  */ | ||||
| 
 | ||||
| #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> | ||||
|  | @ -51,10 +50,7 @@ int main(int argc, char* argv[]) { | |||
|     if (argc > 1) | ||||
|       g2oFile = argv[argc - 1]; | ||||
|     else | ||||
|       g2oFile = | ||||
|           "/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/" | ||||
|           "datasets/randomTorus3D.g2o"; | ||||
|     // g2oFile = findExampleDataFile("sphere_smallnoise.graph");
 | ||||
|       g2oFile = findExampleDataFile("sphere_smallnoise.graph"); | ||||
|   } catch (const exception& e) { | ||||
|     cerr << e.what() << '\n'; | ||||
|     exit(1); | ||||
|  | @ -66,15 +62,16 @@ int main(int argc, char* argv[]) { | |||
| 
 | ||||
|   // Build graph
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   // graph.add(NonlinearEquality<SO4>(0, SO4()));
 | ||||
|   // graph.add(NonlinearEquality<SOn>(0, SOn::identity(4)));
 | ||||
|   auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); | ||||
|   graph.add(PriorFactor<SO4>(0, SO4(), priorModel)); | ||||
|   graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel)); | ||||
|   auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4)); | ||||
|   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], Rot3(Tij.rotation().matrix()), 4, model); | ||||
|         keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model, G); | ||||
|   } | ||||
| 
 | ||||
|   std::mt19937 rng(42); | ||||
|  | @ -95,9 +92,9 @@ int main(int argc, char* argv[]) { | |||
|   for (size_t i = 0; i < 100; i++) { | ||||
|     gttic_(optimize); | ||||
|     Values initial; | ||||
|     initial.insert(0, SO4()); | ||||
|     initial.insert(0, SOn::identity(4)); | ||||
|     for (size_t j = 1; j < poses.size(); j++) { | ||||
|       initial.insert(j, SO4::Random(rng)); | ||||
|       initial.insert(j, SOn::Random(rng, 4)); | ||||
|     } | ||||
|     LevenbergMarquardtOptimizer lm(graph, initial, params); | ||||
|     Values result = lm.optimize(); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue