FastSync.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/FastMap.h>
#include <gtsam/base/MatrixLieGroup.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SL4.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
 
#include <Eigen/SVD>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <vector>
 
namespace gtsam {
 
class GaussianConditional;

template <class T>
struct FastSyncProjection;

template <>
struct FastSyncProjection<Rot2> {
  static Rot2 project(const Matrix2& matrix) { return Rot2::ClosestTo(matrix); }
};

template <>
struct FastSyncProjection<Rot3> {
  static Rot3 project(const Matrix3& matrix) { return Rot3::ClosestTo(matrix); }
};

template <>
struct FastSyncProjection<Pose2> {
  static Pose2 project(const Matrix3& matrix) {
    return Pose2(Rot2::ClosestTo(matrix.topLeftCorner<2, 2>()),
                 matrix.topRightCorner<2, 1>());
  }
};

template <>
struct FastSyncProjection<Pose3> {
  static Pose3 project(const Matrix4& matrix) {
    return Pose3(Rot3::ClosestTo(matrix.topLeftCorner<3, 3>()),
                 matrix.topRightCorner<3, 1>());
  }
};

template <>
struct FastSyncProjection<Similarity2> {
  static Similarity2 project(const Matrix3& matrix) {
    double inverseScale = matrix(2, 2);
    if (!std::isfinite(inverseScale)) inverseScale = 1.0;
    inverseScale = std::abs(inverseScale);
    if (inverseScale <= std::numeric_limits<double>::epsilon()) {
      inverseScale = 1.0;
    }
    return Similarity2(Rot2::ClosestTo(matrix.topLeftCorner<2, 2>()),
                       matrix.topRightCorner<2, 1>(), 1.0 / inverseScale);
  }
};

template <>
struct FastSyncProjection<Similarity3> {
  static Similarity3 project(const Matrix4& matrix) {
    double inverseScale = matrix(3, 3);
    if (!std::isfinite(inverseScale)) inverseScale = 1.0;
    inverseScale = std::abs(inverseScale);
    if (inverseScale <= std::numeric_limits<double>::epsilon()) {
      inverseScale = 1.0;
    }
    return Similarity3(Rot3::ClosestTo(matrix.topLeftCorner<3, 3>()),
                       matrix.topRightCorner<3, 1>(), 1.0 / inverseScale);
  }
};

template <>
struct FastSyncProjection<SL4> {
  static SL4 project(const Matrix4& matrix) {
    const Eigen::JacobiSVD<Matrix4> svd(
        matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
    const auto singularValues = svd.singularValues();
    const double determinantMagnitude = singularValues.prod();
    if (!std::isfinite(determinantMagnitude) || determinantMagnitude <= 1e-12) {
      return SL4::Identity();
    }
    return SL4(matrix);
  }
};

template <class T>
struct FastSync {
  using LieAlgebra = typename T::LieAlgebra;
  static constexpr int N = LieAlgebra::RowsAtCompileTime;
  static_assert(N != Eigen::Dynamic && N > 0,
                "FastSync requires a positive compile-time matrix dimension");
  static_assert(LieAlgebra::ColsAtCompileTime == N,
                "FastSync requires a square matrix representation");
 
  using MatrixN = Eigen::Matrix<double, N, N>;
  using VectorN = Eigen::Matrix<double, N, 1>;

  explicit FastSync(const NonlinearFactorGraph& graph);

  Values solve(Ordering::OrderingType orderingType = Ordering::METIS) const;

  Values solve(const Ordering& ordering) const;

  Values projectAndAlign(const Values& relaxed) const;
 
 private:
  size_t priorCount_ = 0;
  Key priorKey_ = 0;
  T priorValue_ = traits<T>::Identity();
  GaussianFactorGraph reducedGraph_;

  static double isotropicSigma(const SharedNoiseModel& model);

  static void backSubstituteConditional(const GaussianConditional& conditional,
                                        const Key& gaugeKey, Values& solution);

  Values solveOrdered(const Ordering& ordering) const;
};

template <class T>
Values fastSync(const NonlinearFactorGraph& graph,
                Ordering::OrderingType orderingType = Ordering::METIS);

template <class T>
Values fastSync(const NonlinearFactorGraph& graph, const Ordering& ordering);
 
}  // namespace gtsam
 
#include <gtsam/slam/FastSync-inl.h>