ABC.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/base/GroupAction.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/MatrixLieGroup.h>
#include <gtsam/base/ProductLieGroup.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorConstants.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
 
#include <iostream>
#include <string>
#include <vector>
 
namespace gtsam {
namespace abc {

inline Vector6 toInputVector(const Vector3& w) {
  return (Vector6() << w, Z_3x1).finished();
}

template <size_t N>
using Calibrations = PowerLieGroup<Rot3, static_cast<int>(N)>;
 
//========================================================================
// State Manifold
//========================================================================

template <size_t N>
struct State {
  Rot3 R;             // Attitude rotation matrix R
  Vector3 b;          // Gyroscope bias b
  Calibrations<N> S;  // Sensor calibrations S
 
  static constexpr int dimension = 6 + 3 * static_cast<int>(N);
  using TangentVector = Eigen::Matrix<double, dimension, 1>;

  State(const Rot3& R = Rot3(), const Vector3& b = Z_3x1,
        const Calibrations<N>& S = Calibrations<N>())
      : R(R), b(b), S(S) {}

  static State identity() { return State(Rot3(), Z_3x1, Calibrations<N>()); }

  TangentVector localCoordinates(const State<N>& other) const {
    TangentVector eps(dimension);
 
    // First 3 elements - attitude
    eps.template head<3>() = R.logmap(other.R);
    // Next 3 elements - bias
    eps.template segment<3>(3) = other.b - b;
 
    // Remaining elements - calibrations
    eps.template segment<3 * N>(6) = S.logmap(other.S);
 
    return eps;
  }

  State retract(const TangentVector& v) const {
    Rot3 newR = R.expmap(v.template head<3>());
    Vector3 newB = b + v.template segment<3>(3);
    typename Calibrations<N>::TangentVector deltaS;
    deltaS = v.template segment<3 * N>(6);
    Calibrations<N> newS = S.expmap(deltaS);
    return State(newR, newB, newS);
  }
 
  void print(const std::string& s = "") const {
    if (!s.empty()) std::cout << s << " ";
    std::cout << "State<" << N << ">" << std::endl;
    R.print("  R");
    std::cout << "  b: " << b.transpose() << std::endl;
    for (size_t i = 0; i < N; ++i) {
      const std::string label = "  S[" + std::to_string(i) + "]";
      S[i].print(label);
    }
  }
 
  bool equals(const State<N>& other, double tol = 1e-9) const {
    if (!R.equals(other.R, tol)) return false;
    if (!equal_with_abs_tol(b, other.b, tol)) return false;
    return traits<Calibrations<N>>::Equals(S, other.S, tol);
  }
};
 
//========================================================================
// Symmetry Group
//========================================================================

template <size_t n>
using Group = ProductLieGroup<Pose3, Calibrations<n>>;

template <size_t N>
auto asTriple = [](const Group<N>& g)
    -> std::tuple<const Rot3&, const Vector3&, const Calibrations<N>&> {
  return std::tie(g.first.rotation(), g.first.translation(), g.second);
};
 
//========================================================================
// Group Actions on State, Input, and Output Manifolds
//========================================================================

template <size_t N>
struct Symmetry : public GroupAction<Symmetry<N>, Group<N>, State<N>> {
  using M = State<N>;
  using G = gtsam::abc::Group<N>;
  static constexpr ActionType type = ActionType::Right;

  M operator()(const M& xi, const G& g,
               OptionalJacobian<M::dimension, M::dimension> Hm = {},
               OptionalJacobian<M::dimension, G::dimension> Hg = {}) const {
    auto [A, a, B] = asTriple<N>(g);
    const Rot3 new_R = xi.R * A;
    Matrix3 skew_p, At;  // derivatives of unrotate
    const Point3 p = xi.b - a;
    const Vector3 new_b =
        A.unrotate(p, Hg ? &skew_p : nullptr, (Hg || Hm) ? &At : nullptr);
    Calibrations<N> new_S;
    Rot3 invA = A.inverse();  // derivative is (- A)
    for (size_t i = 0; i < N; i++) {
      Rot3 SB = xi.S[i].compose(B[i]);  // derivative in B[i] is identity
      new_S[i] = invA.compose(SB);      // derivative in invA is SB^{-1}.
    }
 
    if (Hm) {
      Hm->setZero();
 
      // d(R*A)/dR: right-multiplication by A maps a tangent vector δθ to
      // Ad_{A^{-1}} δθ = Aᵀ δθ.
      Hm->template block<3, 3>(0, 0) = At;
 
      // d(At*(b - a))/db = At.
      Hm->template block<3, 3>(3, 3) = At;
 
      // d(At * S[i] * B[i]) / dS[i] = Ad_{B[i]^{-1}} on so(3), which is just
      // multiplication by B[i]ᵀ in vector form.
      for (size_t i = 0; i < N; ++i) {
        const size_t row = 6 + 3 * i;
        Hm->template block<3, 3>(row, row) = B[i].transpose();
      }
    }
    if (Hg) {
      Hg->setZero();
      // Rotation block: δθ maps directly to the state's rotational tangent.
      Hg->template block<3, 3>(0, 0) = I_3x3;
 
      // Bias block:
      Hg->template block<3, 3>(3, 0) = skew_p;
      Hg->template block<3, 3>(3, 3) = -I_3x3;  // - At * A (from translation()
 
      // Calibration blocks:
      Matrix3 A_matrix = A.matrix();
      for (size_t i = 0; i < N; ++i) {
        Rot3 SB = xi.S[i].compose(B[i]);
        const size_t row = 6 + 3 * i;
        const size_t col = 6 + 3 * i;
        Hg->template block<3, 3>(row, 0) = -SB.transpose() * A_matrix;
        Hg->template block<3, 3>(row, col) = I_3x3;
      }
    }
    return {new_R, new_b, new_S};
  }
 
  struct Orbit : public group_action::Orbit<Symmetry<N>> {
    using Base = group_action::Orbit<Symmetry<N>>;
    using Base::Base;  // Inherit constructors
  };
};

template <size_t N>
inline typename State<N>::TangentVector dynamics(const Vector3& omega,
                                                 const State<N>& xi) {
  typename State<N>::TangentVector xi_dot;
  xi_dot.setZero();
  xi_dot.template head<3>() = omega - xi.b;
  // Remaining components are already zero.
  return xi_dot;
}

template <size_t N>
struct Lift {
  using M = State<N>;
  using G = Group<N>;
 
  explicit Lift(const Vector6& u) : u_(u) {}
 
  typename G::TangentVector operator()(
      const M& xi, OptionalJacobian<G::dimension, M::dimension> H = {}) const {
    typename G::TangentVector L;
    Vector3 w = u_.head<3>();  // w = omega
    Vector3 corrected_w = w - xi.b;
    L.template head<3>() = corrected_w;
    L.template segment<3>(3) = -Rot3::Hat(w) * xi.b;
    if (H) {
      H->setZero();
      // corrected_w / xi:
      H->template block<3, 3>(0, 3) = -I_3x3;
 
      // next segment:
      H->template block<3, 3>(3, 3) = -Rot3::Hat(w);
    }
    for (size_t i = 0; i < N; i++) {
      DenseIndex k = 6 + 3 * i;
      Vector3 v_i = xi.S[i].unrotate(corrected_w);
      L.template segment<3>(k) = v_i;
      if (H) {
        H->template block<3, 3>(k, 3) = -xi.S[i].transpose();
        H->template block<3, 3>(k, k) = Rot3::Hat(v_i);
      }
    }
 
    return L;
  }
 
 private:
  Vector6 u_;
};

template <size_t N>
struct InputAction : public GroupAction<InputAction<N>, Group<N>, Vector6> {
  using G = Group<N>;
  static constexpr ActionType type = ActionType::Right;
 
  Vector6 operator()(const Vector6& u, const G& X) const {
    const Rot3& A = X.first.rotation();
    const Vector3& a = X.first.translation();
    Vector6 result;
    result.head<3>() = A.unrotate(u.head<3>() - a);
    result.tail<3>() = Z_3x1;
    return result;
  }
};
 
// Embed a 6x6 Sigma into full DimU by appending small calibration noise.
template <size_t N>
inline Matrix inputProcessNoise(const Matrix& Sigma6) {
  std::vector<Matrix> blocks{Sigma6};
  blocks.insert(blocks.end(), N, 1e-9 * I_3x3);
  return gtsam::diag(blocks);
}

template <size_t N>
inline Matrix stateMatrixA(const typename InputAction<N>::Orbit& psi_u,
                           const Group<N>& X_hat) {
  const Vector6 u0 = psi_u(X_hat.inverse());  // ψ_u(X)^ω (omega, 0)
  Matrix3 W0 = Rot3::Hat(u0.template head<3>());
 
  Matrix A1 = Matrix::Zero(6, 6);
  A1.block<3, 3>(0, 3) = -I_3x3;
  A1.block<3, 3>(3, 3) = W0;
 
  std::vector<Matrix> blocks{A1};
  blocks.insert(blocks.end(), N, W0);
  return gtsam::diag(blocks);
}

template <size_t N>
inline Matrix inputMatrixB(const Group<N>& g) {
  const Rot3& A = g.first.rotation();
  const Calibrations<N>& B = g.second;
  const Matrix3 A_matrix = A.matrix();
  Matrix B1 = gtsam::diag({A_matrix, A_matrix});
  Matrix B2(3 * N, 3 * N);
  B2.setZero();
  for (size_t i = 0; i < N; ++i) {
    B2.block<3, 3>(3 * i, 3 * i) = B[i].matrix();
  }
  return gtsam::diag({B1, B2});
}

template <size_t N>
struct OutputAction : public GroupAction<OutputAction<N>, Group<N>, Vector3> {
  using G = Group<N>;
  static constexpr ActionType type = ActionType::Right;
 
  explicit OutputAction(int index = -1) : index_(index) {}
 
  Vector3 operator()(const Vector3& y, const G& X,
                     OptionalJacobian<3, 3> H_y = {},
                     OptionalJacobian<3, G::dimension> H_X = {}) const {
    if (H_X) H_X->setZero();
    auto [A, a, B] = asTriple<N>(X);
    if (index_ == -1) {
      Matrix3 H_rot;
      Vector3 res = A.unrotate(y, H_X ? &H_rot : nullptr, H_y);
      if (H_X) H_X->template block<3, 3>(0, 0) = H_rot;
      return res;
    } else {
      Matrix3 H_rot;
      Vector3 res = B[index_].unrotate(y, H_X ? &H_rot : nullptr, H_y);
      if (H_X) H_X->template block<3, 3>(0, 6 + 3 * index_) = H_rot;
      return res;
    }
  }
 
  int index_;
};

template <size_t N>
inline Matrix measurementMatrixC(const Unit3& d, int index) {
  Matrix Cc = Matrix::Zero(3, 3 * N);
 
  Matrix3 wedge_d = Rot3::Hat(d.unitVector());
  if (index >= 0) {
    Cc.block<3, 3>(0, 3 * index) = wedge_d;
  }
 
  Matrix temp(3, 6 + 3 * N);
  temp.block<3, 3>(0, 0) = wedge_d;
  temp.block<3, 3>(0, 3) = Matrix3::Zero();
  temp.block(0, 6, 3, 3 * N) = Cc;
 
  return wedge_d * temp;
}
 
template <size_t N>
struct Innovation {
  using M = State<N>;

  Innovation(const Unit3& y, const Unit3& d, int index)
      : y_(y), d_(d), xi_ref_(M::identity()), index_(index) {}
 
  Innovation(const Unit3& y, const Unit3& d, int index, const M& xi_ref)
      : y_(y), d_(d), xi_ref_(xi_ref), index_(index) {}
 
  Vector3 operator()(const M& xi_hat,
                     OptionalJacobian<3, M::dimension> H = {}) const {
    // Recover A and B_i from (xi_ref_, xi_hat) using the symmetry formulas:
    //   R_hat = R0 * A,   S_hat[i] = Aᵀ * S0[i] * B[i]
    const Rot3 R0 = xi_ref_.R;
    const Rot3 R_hat = xi_hat.R;
    const Rot3 A = R0.inverse() * R_hat;
 
    Vector3 transformed_y;
    if (index_ == -1) {
      // Uncalibrated sensor: transformed_y = A * y.
      transformed_y = A.rotate(y_.unitVector());
    } else {
      // Calibrated sensor i: B_i = S0[i]^{-1} * A * S_hat[i]
      const Rot3& S0i = xi_ref_.S[index_];
      const Rot3& Shati = xi_hat.S[index_];
      const Rot3 Bi = S0i.inverse() * A * Shati;
      transformed_y = Bi.rotate(y_.unitVector());
    }
 
    if (H) {
      *H = measurementMatrixC<N>(d_, index_);
    }
 
    const Matrix3 wedge_d = Rot3::Hat(d_.unitVector());
    return -wedge_d * transformed_y;
  }
 
  Unit3 y_;   // measured direction
  Unit3 d_;   // reference direction
  M xi_ref_;  // reference state on the manifold
  int index_;
};
 
template <size_t N>
inline Matrix3 outputMatrixD(const Group<N>& X_hat, int index) {
  auto [A, a, B] = asTriple<N>(X_hat);
  if (index >= 0) {
    return B[index].matrix();
  } else {
    return A.matrix();
  }
}
 
}  // namespace abc
 
template <size_t N>
struct traits<abc::State<N>> : public internal::Manifold<abc::State<N>> {};
 
template <size_t N>
struct traits<const abc::State<N>> : public internal::Manifold<abc::State<N>> {
};
 
}  // namespace gtsam