diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 904addb03..6981a6a1c 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -52,23 +52,40 @@ boost::shared_ptr 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 &G) : 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); + 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(); + *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::Print(M_, " M: "); + noiseModel_->print(" noise model: "); +} + +//****************************************************************************** +bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected, + double tol) const { + auto e = dynamic_cast(&expected); + return e != nullptr && NoiseModelFactor2::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; diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 6b2ef67fc..f254b4b81 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -92,14 +92,17 @@ class FrobeniusFactor : public NoiseModelFactor2 { * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. */ template -class FrobeniusBetweenFactor : public NoiseModelFactor2 { +GTSAM_EXPORT 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 + /// @name Constructor + /// @{ + + /// Construct from two keys and measured rotation FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, const SharedNoiseModel& model = nullptr) : NoiseModelFactor2( @@ -107,6 +110,33 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { 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::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(&expected); + return e != nullptr && NoiseModelFactor2::equals(*e, tol) && + traits::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 H1 = boost::none, @@ -117,6 +147,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; return error; } + /// @} }; /** @@ -125,21 +156,46 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { * 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 { - 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 { + Matrix M_; ///< measured rotation between R1 and R2 + size_t p_, pp_; ///< dimensionality constants + boost::shared_ptr 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(SOn::VectorizedGenerators(p)); + FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4, + const SharedNoiseModel &model = nullptr, + const boost::shared_ptr &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 H1 = boost::none, boost::optional H2 = boost::none) const override; + /// @} }; } // namespace gtsam diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp index 8bd754de6..8bdda968f 100644 --- a/timing/timeFrobeniusFactor.cpp +++ b/timing/timeFrobeniusFactor.cpp @@ -13,12 +13,11 @@ * @file timeFrobeniusFactor.cpp * @brief time FrobeniusFactor with BAL file * @author Frank Dellaert - * @date June 6, 2015 + * @date 2019 */ #include #include -#include #include #include #include @@ -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(0, SO4())); + // graph.add(NonlinearEquality(0, SOn::identity(4))); auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); - graph.add(PriorFactor(0, SO4(), priorModel)); + graph.add(PriorFactor(0, SOn::identity(4), priorModel)); + auto G = boost::make_shared(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( - 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();