diff --git a/.gitignore b/.gitignore index 96b9dd0c7a..8f8187ee76 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ *.pyc *.DS_Store *.swp +*:Zone.Identifier /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt @@ -25,3 +26,5 @@ xcode/ _build GEMINI.md doc/#*.lyx# +/cmake-build-debug/ +.venv/ \ No newline at end of file diff --git a/examples/ABC.h b/examples/ABC.h deleted file mode 100644 index a7c029c98a..0000000000 --- a/examples/ABC.h +++ /dev/null @@ -1,259 +0,0 @@ -/** - * @file ABC.h - * @brief Core components for Attitude-Bias-Calibration systems - * - * This file contains fundamental components and utilities for the ABC system - * based on the paper "Overcoming Bias: Equivariant Filter Design for Biased - * Attitude Estimation with Online Calibration" by Fornasier et al. - * Authors: Darshan Rajasekaran & Jennifer Oum - */ -#ifndef ABC_H -#define ABC_H -#include -#include -#include -#include - -namespace gtsam { -namespace abc_eqf_lib { -using namespace std; -using namespace gtsam; -//======================================================================== -// Utility Functions -//======================================================================== - -//======================================================================== -// Utility Functions -//======================================================================== - -/// Check if a vector is a unit vector - -bool checkNorm(const Vector3& x, double tol = 1e-3); - -/// Check if vector contains NaN values - -bool hasNaN(const Vector3& vec); - -/// Create a block diagonal matrix from two matrices - -Matrix blockDiag(const Matrix& A, const Matrix& B); - -/// Repeat a block matrix n times along the diagonal - -Matrix repBlock(const Matrix& A, int n); - -// Utility Functions Implementation - -/** - * @brief Verifies if a vector has unit norm within tolerance - * @param x 3d vector - * @param tol optional tolerance - * @return Bool indicating that the vector norm is approximately 1 - */ -bool checkNorm(const Vector3& x, double tol) { - return abs(x.norm() - 1) < tol || std::isnan(x.norm()); -} - -/** - * @brief Checks if the input vector has any NaNs - * @param vec A 3-D vector - * @return true if present, false otherwise - */ -bool hasNaN(const Vector3& vec) { - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); -} - -/** - * @brief Creates a block diagonal matrix from input matrices - * @param A Matrix A - * @param B Matrix B - * @return A single consolidated matrix with A in the top left and B in the - * bottom right - */ -Matrix blockDiag(const Matrix& A, const Matrix& B) { - if (A.size() == 0) { - return B; - } else if (B.size() == 0) { - return A; - } else { - Matrix result(A.rows() + B.rows(), A.cols() + B.cols()); - result.setZero(); - result.block(0, 0, A.rows(), A.cols()) = A; - result.block(A.rows(), A.cols(), B.rows(), B.cols()) = B; - return result; - } -} - -/** - * @brief Creates a block diagonal matrix by repeating a matrix 'n' times - * @param A A matrix - * @param n Number of times to be repeated - * @return Block diag matrix with A repeated 'n' times - */ -Matrix repBlock(const Matrix& A, int n) { - if (n <= 0) return Matrix(); - - Matrix result = A; - for (int i = 1; i < n; i++) { - result = blockDiag(result, A); - } - return result; -} - -//======================================================================== -// Core Data Types -//======================================================================== - -/// Input struct for the Biased Attitude System - -struct Input { - Vector3 w; /// Angular velocity (3-vector) - Matrix Sigma; /// Noise covariance (6x6 matrix) - static Input random(); /// Random input - Matrix3 W() const { /// Return w as a skew symmetric matrix - return Rot3::Hat(w); - } -}; - -/// Measurement struct -struct Measurement { - Unit3 y; /// Measurement direction in sensor frame - Unit3 d; /// Known direction in global frame - Matrix3 Sigma; /// Covariance matrix of the measurement - int cal_idx = -1; /// Calibration index (-1 for calibrated sensor) -}; - -/// State class representing the state of the Biased Attitude System -template -class State { - public: - Rot3 R; // Attitude rotation matrix R - Vector3 b; // Gyroscope bias b - std::array S; // Sensor calibrations S - - /// Constructor - State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(), - const std::array& S = std::array{}) - : R(R), b(b), S(S) {} - - /// Identity function - static State identity() { - std::array S_id{}; - S_id.fill(Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), S_id); - } - /** - * Compute Local coordinates in the state relative to another state. - * @param other The other state - * @return Local coordinates in the tangent space - */ - Vector localCoordinates(const State& other) const { - Vector eps(6 + 3 * N); - - // First 3 elements - attitude - eps.head<3>() = Rot3::Logmap(R.between(other.R)); - // Next 3 elements - bias - // Next 3 elements - bias - eps.segment<3>(3) = other.b - b; - - // Remaining elements - calibrations - for (size_t i = 0; i < N; i++) { - eps.segment<3>(6 + 3 * i) = Rot3::Logmap(S[i].between(other.S[i])); - } - - return eps; - } - - /** - * Retract from tangent space back to the manifold - * @param v Vector in the tangent space - * @return New state - */ - State retract(const Vector& v) const { - if (v.size() != static_cast(6 + 3 * N)) { - throw std::invalid_argument( - "Vector size does not match state dimensions"); - } - Rot3 newR = R * Rot3::Expmap(v.head<3>()); - Vector3 newB = b + v.segment<3>(3); - std::array newS; - for (size_t i = 0; i < N; i++) { - newS[i] = S[i] * Rot3::Expmap(v.segment<3>(6 + 3 * i)); - } - return State(newR, newB, newS); - } -}; - -//======================================================================== -// Symmetry Group -//======================================================================== - -/** - * Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3) - * Each element of the B list is associated with a calibration state - */ -template -struct G { - Rot3 A; /// First SO(3) element - Matrix3 a; /// so(3) element (skew-symmetric matrix) - std::array B; /// List of SO(3) elements for calibration - - /// Initialize the symmetry group G - G(const Rot3& A = Rot3::Identity(), const Matrix3& a = Matrix3::Zero(), - const std::array& B = std::array{}) - : A(A), a(a), B(B) {} - - /// Group multiplication - G operator*(const G& other) const { - std::array newB; - for (size_t i = 0; i < N; i++) { - newB[i] = B[i] * other.B[i]; - } - return G(A * other.A, a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), newB); - } - - /// Group inverse - G inv() const { - Matrix3 Ainv = A.inverse().matrix(); - std::array Binv; - for (size_t i = 0; i < N; i++) { - Binv[i] = B[i].inverse(); - } - return G(A.inverse(), -Rot3::Hat(Ainv * Rot3::Vee(a)), Binv); - } - - /// Identity element - static G identity(int n) { - std::array B; - B.fill(Rot3::Identity()); - return G(Rot3::Identity(), Matrix3::Zero(), B); - } - - /// Exponential map of the tangent space elements to the group - static G exp(const Vector& x) { - if (x.size() != static_cast(6 + 3 * N)) { - throw std::invalid_argument("Vector size mismatch for group exponential"); - } - Rot3 A = Rot3::Expmap(x.head<3>()); - Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3); - Matrix3 a = Rot3::Hat(a_vee); - std::array B; - for (size_t i = 0; i < N; i++) { - B[i] = Rot3::Expmap(x.segment<3>(6 + 3 * i)); - } - return G(A, a, B); - } -}; -} // namespace abc_eqf_lib - -template -struct traits> - : internal::LieGroupTraits> {}; - -template -struct traits> : internal::LieGroupTraits> { -}; - -} // namespace gtsam - -#endif // ABC_H diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h deleted file mode 100644 index 02b8dd4b9c..0000000000 --- a/examples/ABC_EQF.h +++ /dev/null @@ -1,519 +0,0 @@ -/** - * @file ABC_EQF.h - * @brief Header file for the Attitude-Bias-Calibration Equivariant Filter - * - * This file contains declarations for the Equivariant Filter (EqF) for attitude - * estimation with both gyroscope bias and sensor extrinsic calibration, based - * on the paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude - * Estimation with Online Calibration" by Fornasier et al. Authors: Darshan - * Rajasekaran & Jennifer Oum - */ - -#ifndef ABC_EQF_H -#define ABC_EQF_H -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include // For std::accumulate -#include -#include - -#include "ABC.h" - -// All implementations are wrapped in this namespace to avoid conflicts -namespace gtsam { -namespace abc_eqf_lib { - -using namespace std; -using namespace gtsam; - -//======================================================================== -// Helper Functions for EqF -//======================================================================== - -/// Calculate numerical differential - -Matrix numericalDifferential(std::function f, - const Vector& x); - -/** - * Compute the lift of the system (Theorem 3.8, Equation 7) - * @param xi State - * @param u Input - * @return Lift vector - */ -template -Vector lift(const State& xi, const Input& u); - -/** - * Action of the symmetry group on the state space (Equation 4) - * @param X Group element - * @param xi State - * @return New state after group action - */ -template -State operator*(const G& X, const State& xi); - -/** - * Action of the symmetry group on the input space (Equation 5) - * @param X Group element - * @param u Input - * @return New input after group action - */ -template -Input velocityAction(const G& X, const Input& u); - -/** - * Action of the symmetry group on the output space (Equation 6) - * @param X Group element - * @param y Direction measurement - * @param idx Calibration index - * @return New direction after group action - */ -template -Vector3 outputAction(const G& X, const Unit3& y, int idx); - -/** - * Differential of the phi action at E = Id in local coordinates - * @param xi State - * @return Differential matrix - */ -template -Matrix stateActionDiff(const State& xi); - -//======================================================================== -// Equivariant Filter (EqF) -//======================================================================== - -/// Equivariant Filter (EqF) implementation -template -class EqF { - private: - int dof; // Degrees of freedom - G X_hat; // Filter state - Matrix Sigma; // Error covariance - State xi_0; // Origin state - Matrix Dphi0; // Differential of phi at origin - Matrix InnovationLift; // Innovation lift matrix - - /** - * Return the state matrix A0t (Equation 14a) - * @param u Input - * @return State matrix A0t - */ - Matrix stateMatrixA(const Input& u) const; - - /** - * Return the state transition matrix Phi (Equation 17) - * @param u Input - * @param dt Time step - * @return State transition matrix Phi - */ - Matrix stateTransitionMatrix(const Input& u, double dt) const; - - /** - * Return the Input matrix Bt - * @return Input matrix Bt - */ - Matrix inputMatrixBt() const; - - /** - * Return the measurement matrix C0 (Equation 14b) - * @param d Known direction - * @param idx Calibration index - * @return Measurement matrix C0 - */ - Matrix measurementMatrixC(const Unit3& d, int idx) const; - - /** - * Return the measurement output matrix Dt - * @param idx Calibration index - * @return Measurement output matrix Dt - */ - Matrix outputMatrixDt(int idx) const; - - public: - /** - * Initialize EqF - * @param Sigma Initial covariance - * @param m Number of sensors - */ - EqF(const Matrix& Sigma, int m); - - /** - * Return estimated state - * @return Current state estimate - */ - State stateEstimate() const; - - /** - * Propagate the filter state - * @param u Angular velocity measurement - * @param dt Time step - */ - void propagation(const Input& u, double dt); - - /** - * Update the filter state with a measurement - * @param y Direction measurement - */ - void update(const Measurement& y); -}; - -//======================================================================== -// Helper Functions Implementation -//======================================================================== - -/** - * Maps system dynamics to the symmetry group - * @param xi State - * @param u Input - * @return Lifted input in Lie Algebra - * Uses Vector zero & Rot3 inverse, matrix functions - */ -template -Vector lift(const State& xi, const Input& u) { - Vector L = Vector::Zero(6 + 3 * N); - - // First 3 elements - L.head<3>() = u.w - xi.b; - - // Next 3 elements - L.segment<3>(3) = -u.W() * xi.b; - - // Remaining elements - for (size_t i = 0; i < N; i++) { - L.segment<3>(6 + 3 * i) = xi.S[i].inverse().matrix() * L.head<3>(); - } - - return L; -} -/** - * Implements group actions on the states - * @param X A symmetry group element G consisting of the attitude, bias and the - * calibration components X.a -> Rotation matrix containing the attitude X.b -> - * A skew-symmetric matrix representing bias X.B -> A vector of Rotation - * matrices for the calibration components - * @param xi State object - * xi.R -> Attitude (Rot3) - * xi.b -> Gyroscope Bias(Vector 3) - * xi.S -> Vector of calibration matrices(Rot3) - * @return Transformed state - * Uses the Rot3 inverse and Vee functions - */ -template -State operator*(const G& X, const State& xi) { - std::array new_S; - - for (size_t i = 0; i < N; i++) { - new_S[i] = X.A.inverse() * xi.S[i] * X.B[i]; - } - - return State(xi.R * X.A, X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)), - new_S); -} -/** - * Transforms the angular velocity measurements b/w frames - * @param X A symmetry group element X with the components - * @param u Inputs - * @return Transformed inputs - * Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining - * the input equivariance - */ -template -Input velocityAction(const G& X, const Input& u) { - return Input{X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma}; -} -/** - * Transforms the Direction measurements based on the calibration type ( Eqn 6) - * @param X Group element X - * @param y Direction measurement y - * @param idx Calibration index - * @return Transformed direction - * Uses Rot3 inverse, matric and Unit3 unitvector functions - */ -template -Vector3 outputAction(const G& X, const Unit3& y, int idx) { - if (idx == -1) { - return X.A.inverse().matrix() * y.unitVector(); - } else { - if (idx >= static_cast(N)) { - throw std::out_of_range("Calibration index out of range"); - } - return X.B[idx].inverse().matrix() * y.unitVector(); - } -} - -/** - * @brief Calculates the Jacobian matrix using central difference approximation - * @param f Vector function f - * @param x The point at which Jacobian is evaluated - * @return Matrix containing numerical partial derivatives of f at x - * Uses Vector's size() and Zero(), Matrix's Zero() and col() methods - */ -Matrix numericalDifferential(std::function f, - const Vector& x) { - double h = 1e-6; - Vector fx = f(x); - int n = fx.size(); - int m = x.size(); - Matrix Df = Matrix::Zero(n, m); - - for (int j = 0; j < m; j++) { - Vector ej = Vector::Zero(m); - ej(j) = 1.0; - - Vector fplus = f(x + h * ej); - Vector fminus = f(x - h * ej); - - Df.col(j) = (fplus - fminus) / (2 * h); - } - - return Df; -} - -/** - * Computes the differential of a state action at the identity of the symmetry - * group - * @param xi State object Xi representing the point at which to evaluate the - * differential - * @return A matrix representing the jacobian of the state action - * Uses numericalDifferential, and Rot3 expmap, logmap - */ -template -Matrix stateActionDiff(const State& xi) { - std::function coordsAction = [&xi](const Vector& U) { - G groupElement = G::exp(U); - State transformed = groupElement * xi; - return xi.localCoordinates(transformed); - }; - - Vector zeros = Vector::Zero(6 + 3 * N); - Matrix differential = numericalDifferential(coordsAction, zeros); - return differential; -} - -//======================================================================== -// Equivariant Filter (EqF) Implementation -//======================================================================== -/** - * Initializes the EqF with state dimension validation and computes lifted - * innovation mapping - * @param Sigma Initial covariance - * @param n Number of calibration states - * @param m Number of sensors - * Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse() - */ -template -EqF::EqF(const Matrix& Sigma, int m) - : dof(6 + 3 * N), - X_hat(G::identity(N)), - Sigma(Sigma), - xi_0(State::identity()) { - if (Sigma.rows() != dof || Sigma.cols() != dof) { - throw std::invalid_argument( - "Initial covariance dimensions must match the degrees of freedom"); - } - - // Check positive semi-definite - Eigen::SelfAdjointEigenSolver eigensolver(Sigma); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument( - "Covariance matrix must be semi-positive definite"); - } - - if (N < 0) { - throw std::invalid_argument( - "Number of calibration states must be non-negative"); - } - - if (m <= 1) { - throw std::invalid_argument( - "Number of direction sensors must be at least 2"); - } - - // Compute differential of phi - Dphi0 = stateActionDiff(xi_0); - InnovationLift = Dphi0.completeOrthogonalDecomposition().pseudoInverse(); -} -/** - * Computes the internal group state to a physical state estimate - * @return Current state estimate - */ -template -State EqF::stateEstimate() const { - return X_hat * xi_0; -} -/** - * Implements the prediction step of the EqF using system dynamics and - * covariance propagation and advances the filter state by symmtery-preserving - * dynamics.Uses a Lie group integrator scheme for discrete time propagation - * @param u Angular velocity measurements - * @param dt time steps - * Updated internal state and covariance - */ -template -void EqF::propagation(const Input& u, double dt) { - State state_est = stateEstimate(); - Vector L = lift(state_est, u); - - Matrix Phi_DT = stateTransitionMatrix(u, dt); - Matrix Bt = inputMatrixBt(); - - Matrix tempSigma = blockDiag(u.Sigma, repBlock(1e-9 * I_3x3, N)); - Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt; - - X_hat = X_hat * G::exp(L * dt); - Sigma = Phi_DT * Sigma * Phi_DT.transpose() + M_DT; -} -/** - * Implements the correction step of the filter using discrete measurements - * Computes the measurement residual, Kalman gain and the updates both the state - * and covariance - * - * @param y Measurements - */ -template -void EqF::update(const Measurement& y) { - if (y.cal_idx > static_cast(N)) { - throw std::invalid_argument("Calibration index out of range"); - } - - // Get vector representations for checking - Vector3 y_vec = y.y.unitVector(); - Vector3 d_vec = y.d.unitVector(); - - // Skip update if any NaN values are present - if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) || - std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) { - return; // Skip this measurement - } - - Matrix Ct = measurementMatrixC(y.d, y.cal_idx); - Vector3 action_result = outputAction(X_hat.inv(), y.y, y.cal_idx); - Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result; - Matrix Dt = outputMatrixDt(y.cal_idx); - Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose(); - Matrix K = Sigma * Ct.transpose() * S.inverse(); - Vector Delta = InnovationLift * K * delta_vec; - X_hat = G::exp(Delta) * X_hat; - Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma; -} -/** - * Computes linearized continuous time state matrix - * @param u Angular velocity - * @return Linearized state matrix - * Uses Matrix zero and Identity functions - */ -template -Matrix EqF::stateMatrixA(const Input& u) const { - Matrix3 W0 = velocityAction(X_hat.inv(), u).W(); - Matrix A1 = Matrix::Zero(6, 6); - A1.block<3, 3>(0, 3) = -I_3x3; - A1.block<3, 3>(3, 3) = W0; - Matrix A2 = repBlock(W0, N); - return blockDiag(A1, A2); -} - -/** - * Computes the discrete time state transition matrix - * @param u Angular velocity - * @param dt time step - * @return State transition matrix in discrete time - */ -template -Matrix EqF::stateTransitionMatrix(const Input& u, double dt) const { - Matrix3 W0 = velocityAction(X_hat.inv(), u).W(); - Matrix Phi1 = Matrix::Zero(6, 6); - - Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0); - Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0; - - Phi1.block<3, 3>(0, 0) = I_3x3; - Phi1.block<3, 3>(0, 3) = Phi12; - Phi1.block<3, 3>(3, 3) = Phi22; - Matrix Phi2 = repBlock(Phi22, N); - return blockDiag(Phi1, Phi2); -} -/** - * Computes the input uncertainty propagation matrix - * @return - * Uses the blockdiag matrix - */ -template -Matrix EqF::inputMatrixBt() const { - Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); - Matrix B2(3 * N, 3 * N); - - for (size_t i = 0; i < N; ++i) { - B2.block<3, 3>(3 * i, 3 * i) = X_hat.B[i].matrix(); - } - - return blockDiag(B1, B2); -} -/** - * Computes the linearized measurement matrix. The structure depends on whether - * the sensor has a calibration state - * @param d reference direction - * @param idx Calibration index - * @return Measurement matrix - * Uses the matrix zero, Rot3 hat and the Unitvector functions - */ -template -Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * N); - - // If the measurement is related to a sensor that has a calibration state - if (idx >= 0) { - // Set the correct 3x3 block in Cc - Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.unitVector()); - } - - Matrix3 wedge_d = Rot3::Hat(d.unitVector()); - - // Create the combined matrix - 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; -} -/** - * Computes the measurement uncertainty propagation matrix - * @param idx Calibration index - * @return Returns B[idx] for calibrated sensors, A for uncalibrated - */ -template -Matrix EqF::outputMatrixDt(int idx) const { - // If the measurement is related to a sensor that has a calibration state - if (idx >= 0) { - if (idx >= static_cast(N)) { - throw std::out_of_range("Calibration index out of range"); - } - return X_hat.B[idx].matrix(); - } else { - return X_hat.A.matrix(); - } -} - -} // namespace abc_eqf_lib - -template -struct traits> - : internal::LieGroupTraits> {}; -} // namespace gtsam - -#endif // ABC_EQF_H \ No newline at end of file diff --git a/examples/ABC_EQF_Demo.cpp b/examples/AbcEquivariantFilterExample.cpp similarity index 82% rename from examples/ABC_EQF_Demo.cpp rename to examples/AbcEquivariantFilterExample.cpp index 99b18d85f6..662c581ce9 100644 --- a/examples/ABC_EQF_Demo.cpp +++ b/examples/AbcEquivariantFilterExample.cpp @@ -1,34 +1,49 @@ /** - * @file ABC_EQF_Demo.cpp + * @file AbcEquivariantFilterExample.cpp * @brief Demonstration of the full Attitude-Bias-Calibration Equivariant Filter * * This demo shows the Equivariant Filter (EqF) for attitude estimation * with both gyroscope bias and sensor extrinsic calibration, based on the * paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude - * Estimation with Online Calibration" by Fornasier et al. Authors: Darshan - * Rajasekaran & Jennifer Oum + * Estimation with Online Calibration" by Fornasier et al. + * + * @author Darshan Rajasekaran + * @author Jennifer Oum + * @author Rohan Bansal + * @author Frank Dellaert + * @date 2025 */ - -#include "ABC_EQF.h" +#include +#include // Use namespace for convenience using namespace gtsam; -constexpr size_t N = 1; // Number of calibration states -using M = abc_eqf_lib::State; -using Group = abc_eqf_lib::G; -using EqFilter = abc_eqf_lib::EqF; -using gtsam::abc_eqf_lib::EqF; -using gtsam::abc_eqf_lib::Input; -using gtsam::abc_eqf_lib::Measurement; +constexpr size_t n = 1; // Number of calibration states +using M = abc::State; +using G = abc::Group; +using StateAction = abc::StateAction; +using EqFilter = gtsam::EqF; +using Lift = abc::Lift; +using InputAction = abc::InputAction; +using OutputAction = abc::OutputAction; + +/// Measurement struct +struct Measurement { + Unit3 y; /// Measurement direction in sensor frame + Unit3 d; /// Known direction in global frame + Matrix3 R; /// Covariance matrix of the measurement + int cal_idx = -1; /// Calibration index (-1 for calibrated sensor) +}; /// Data structure for ground-truth, input and output data struct Data { - M xi; /// Ground-truth state - Input u; /// Input measurements - std::vector y; /// Output measurements - int n_meas; /// Number of measurements - double t; /// Time - double dt; /// Time step + M xi; /// Ground-truth state + Vector3 omega; /// Angular velocity measurement + Matrix6 inputCovariance; /// Input noise covariance (6x6 matrix) + std::vector measurements; /// Output measurements + int numMeasurements; /// Number of measurements + double t; /// Time + double dt; /// Time step }; //======================================================================== @@ -139,25 +154,23 @@ std::vector loadDataFromCSV(const std::string& filename, int startRow, Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z - std::array S = {Rot3(calQuat)}; + std::array S = {Rot3(calQuat)}; M xi(R, b, S); // Create input - Vector3 w(values[12], values[13], values[14]); + Vector3 omega(values[12], values[13], values[14]); // Create input covariance matrix (6x6) // First 3x3 block for angular velocity, second 3x3 block for bias process // noise - Matrix inputCov = Matrix::Zero(6, 6); - inputCov(0, 0) = values[15] * values[15]; // std_w_x^2 - inputCov(1, 1) = values[16] * values[16]; // std_w_y^2 - inputCov(2, 2) = values[17] * values[17]; // std_w_z^2 - inputCov(3, 3) = values[18] * values[18]; // std_b_x^2 - inputCov(4, 4) = values[19] * values[19]; // std_b_y^2 - inputCov(5, 5) = values[20] * values[20]; // std_b_z^2 - - Input u{w, inputCov}; + Matrix6 inputCovariance = Matrix6::Zero(); + inputCovariance(0, 0) = values[15] * values[15]; // std_w_x^2 + inputCovariance(1, 1) = values[16] * values[16]; // std_w_y^2 + inputCovariance(2, 2) = values[17] * values[17]; // std_w_z^2 + inputCovariance(3, 3) = values[18] * values[18]; // std_b_x^2 + inputCovariance(4, 4) = values[19] * values[19]; // std_b_y^2 + inputCovariance(5, 5) = values[20] * values[20]; // std_b_z^2 // Create measurements std::vector measurements; @@ -197,7 +210,8 @@ std::vector loadDataFromCSV(const std::string& filename, int startRow, measurements.push_back(Measurement{Unit3(y1), Unit3(d1), covY1, -1}); // Create Data object and add to list - data_list.push_back(Data{xi, u, measurements, 2, t, dt}); + data_list.push_back( + Data{xi, omega, inputCovariance, measurements, 2, t, dt}); rowCount++; @@ -252,17 +266,18 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, for (size_t i = 0; i < data_list.size(); i++) { const Data& data = data_list[i]; - + Matrix Q = InputAction::processNoise(data.inputCovariance); // Propagate filter with current input and time step - filter.propagation(data.u, data.dt); + Vector6 u = abc::toInputVector(data.omega); + filter.predict(u, Q, data.dt); // Process all measurements - for (const auto& y : data.y) { + for (const auto& measurement : data.measurements) { totalMeasurements++; // Skip invalid measurements - Vector3 y_vec = y.y.unitVector(); - Vector3 d_vec = y.d.unitVector(); + Vector3 y_vec = measurement.y.unitVector(); + Vector3 d_vec = measurement.d.unitVector(); if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) || std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) { @@ -270,7 +285,8 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, } try { - filter.update(y); + OutputAction phi_y(measurement.y, measurement.d, measurement.cal_idx); + filter.update(phi_y, measurement.R); validMeasurements++; } catch (const std::exception& e) { std::cerr << "Error updating at t=" << data.t << ": " << e.what() @@ -284,7 +300,7 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, // Calculate errors Vector3 att_error = Rot3::Logmap(data.xi.R.between(estimate.R)); Vector3 bias_error = estimate.b - data.xi.b; - Vector3 cal_error = Vector3::Zero(); + Vector3 cal_error = Z_3x1; if (!data.xi.S.empty() && !estimate.S.empty()) { cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); } @@ -326,7 +342,7 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, Vector3 final_att_error = Rot3::Logmap(final_data.xi.R.between(final_estimate.R)); Vector3 final_bias_error = final_estimate.b - final_data.xi.b; - Vector3 final_cal_error = Vector3::Zero(); + Vector3 final_cal_error = Z_3x1; if (!final_data.xi.S.empty() && !final_estimate.S.empty()) { final_cal_error = Rot3::Logmap(final_data.xi.S[0].between(final_estimate.S[0])); @@ -417,10 +433,10 @@ int main(int argc, char* argv[]) { } // Initialize the EqF filter with one calibration state - int n_sensors = 2; + int N = 2; // number of sensors // Initial covariance - larger values allow faster convergence - Matrix initialSigma = Matrix::Identity(6 + 3 * N, 6 + 3 * N); + Matrix initialSigma = Matrix::Identity(6 + 3 * n, 6 + 3 * n); initialSigma.diagonal().head<3>() = Vector3::Constant(0.1); // Attitude uncertainty initialSigma.diagonal().segment<3>(3) = @@ -428,8 +444,11 @@ int main(int argc, char* argv[]) { initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Calibration uncertainty + G initialGroup = gtsam::traits::Identity(); + M initialState = M::identity(); + // Create filter - EqFilter filter(initialSigma, n_sensors); + EqFilter filter(initialGroup, initialState, initialSigma, N); // Process data processDataWithEqF(filter, data); @@ -441,4 +460,4 @@ int main(int argc, char* argv[]) { std::cout << "\nEqF demonstration completed successfully." << std::endl; return 0; -} \ No newline at end of file +} diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 1d4d58a6a1..c6043c6f7a 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -7,7 +7,7 @@ * See LICENSE for the license information - * -------------------------------1------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** * @file ProductLieGroup.h @@ -19,157 +19,234 @@ #pragma once #include +#include + +#include +#include +#include #include // pair namespace gtsam { -/// Template to construct the product Lie group of two other Lie groups -/// Assumes Lie group structure for G and H -template -class ProductLieGroup: public std::pair { +/** + * @brief Template to construct the product Lie group of two other Lie groups + * Assumes Lie group structure for G and H + */ +template +class ProductLieGroup : public std::pair { GTSAM_CONCEPT_ASSERT(IsLieGroup); GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsTestable); + GTSAM_CONCEPT_ASSERT(IsTestable); + + public: + /// Base pair type typedef std::pair Base; -protected: - constexpr static const size_t dimension1 = traits::dimension; - constexpr static const size_t dimension2 = traits::dimension; + protected: + /// Dimensions of the two subgroups + static constexpr size_t dimension1 = traits::dimension; + static constexpr size_t dimension2 = traits::dimension; + + public: + /// @name Standard Constructors + /// @{ -public: /// Default constructor yields identity - ProductLieGroup():Base(traits::Identity(),traits::Identity()) {} + ProductLieGroup() : Base(traits::Identity(), traits::Identity()) {} - // Construct from two subgroup elements - ProductLieGroup(const G& g, const H& h):Base(g,h) {} + /// Construct from two subgroup elements + ProductLieGroup(const G& g, const H& h) : Base(g, h) {} - // Construct from base - ProductLieGroup(const Base& base):Base(base) {} + /// Construct from base pair + ProductLieGroup(const Base& base) : Base(base) {} - /// @name Group + /// @} + /// @name Group Operations /// @{ + typedef multiplicative_group_tag group_flavor; - static ProductLieGroup Identity() {return ProductLieGroup();} + /// Identity element + static ProductLieGroup Identity() { return ProductLieGroup(); } + + /// Group multiplication ProductLieGroup operator*(const ProductLieGroup& other) const { - return ProductLieGroup(traits::Compose(this->first,other.first), - traits::Compose(this->second,other.second)); + return ProductLieGroup(traits::Compose(this->first, other.first), + traits::Compose(this->second, other.second)); } + + /// Group inverse ProductLieGroup inverse() const { - return ProductLieGroup(traits::Inverse(this->first), traits::Inverse(this->second)); + return ProductLieGroup(traits::Inverse(this->first), + traits::Inverse(this->second)); } + + /// Compose with another element (same as operator*) ProductLieGroup compose(const ProductLieGroup& g) const { return (*this) * g; } + + /// Calculate relative transformation ProductLieGroup between(const ProductLieGroup& g) const { return this->inverse() * g; } - /// @} - /// @name Manifold + /// @} + /// @name Manifold Operations /// @{ - inline constexpr static auto dimension = dimension1 + dimension2; - inline static size_t Dim() { return dimension; } - inline size_t dim() const { return dimension; } + /// Manifold dimension + static constexpr size_t dimension = dimension1 + dimension2; + + /// Return manifold dimension + static size_t Dim() { return dimension; } + + /// Return manifold dimension + size_t dim() const { return dimension; } + + /// Tangent vector type typedef Eigen::Matrix TangentVector; + + /// Chart Jacobian type typedef OptionalJacobian ChartJacobian; - ProductLieGroup retract(const TangentVector& v, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) const { - if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); + /// Retract to manifold + ProductLieGroup retract(const TangentVector& v, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const { + if (H1 || H2) { + throw std::runtime_error( + "ProductLieGroup::retract derivatives not implemented yet"); + } G g = traits::Retract(this->first, v.template head()); H h = traits::Retract(this->second, v.template tail()); - return ProductLieGroup(g,h); + return ProductLieGroup(g, h); } - TangentVector localCoordinates(const ProductLieGroup& g, // - ChartJacobian H1 = {}, ChartJacobian H2 = {}) const { - if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); - typename traits::TangentVector v1 = traits::Local(this->first, g.first); - typename traits::TangentVector v2 = traits::Local(this->second, g.second); + + /// Local coordinates on manifold + TangentVector localCoordinates(const ProductLieGroup& g, + ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const { + if (H1 || H2) { + throw std::runtime_error( + "ProductLieGroup::localCoordinates derivatives not implemented yet"); + } + typename traits::TangentVector v1 = + traits::Local(this->first, g.first); + typename traits::TangentVector v2 = + traits::Local(this->second, g.second); TangentVector v; v << v1, v2; return v; } - /// @} - /// @name Lie Group + /// @} + /// @name Lie Group Operations /// @{ -protected: + + protected: + /// Jacobian types for internal use typedef Eigen::Matrix Jacobian; typedef Eigen::Matrix Jacobian1; typedef Eigen::Matrix Jacobian2; -public: + public: + /// Compose with Jacobians ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = {}) const { - Jacobian1 D_g_first; Jacobian2 D_h_second; - G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); - H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); + ChartJacobian H2 = {}) const { + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Compose(this->first, other.first, H1 ? &D_g_first : 0); + H h = traits::Compose(this->second, other.second, H1 ? &D_h_second : 0); if (H1) { H1->setZero(); - H1->template topLeftCorner() = D_g_first; - H1->template bottomRightCorner() = D_h_second; + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; } if (H2) *H2 = Jacobian::Identity(); - return ProductLieGroup(g,h); + return ProductLieGroup(g, h); } + + /// Between with Jacobians ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = {}) const { - Jacobian1 D_g_first; Jacobian2 D_h_second; - G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); - H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); + ChartJacobian H2 = {}) const { + Jacobian1 D_g_first; + Jacobian2 D_h_second; + G g = traits::Between(this->first, other.first, H1 ? &D_g_first : 0); + H h = traits::Between(this->second, other.second, H1 ? &D_h_second : 0); if (H1) { H1->setZero(); - H1->template topLeftCorner() = D_g_first; - H1->template bottomRightCorner() = D_h_second; + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; } if (H2) *H2 = Jacobian::Identity(); - return ProductLieGroup(g,h); + return ProductLieGroup(g, h); } + + /// Inverse with Jacobian ProductLieGroup inverse(ChartJacobian D) const { - Jacobian1 D_g_first; Jacobian2 D_h_second; + Jacobian1 D_g_first; + Jacobian2 D_h_second; G g = traits::Inverse(this->first, D ? &D_g_first : 0); H h = traits::Inverse(this->second, D ? &D_h_second : 0); if (D) { D->setZero(); - D->template topLeftCorner() = D_g_first; - D->template bottomRightCorner() = D_h_second; + D->template topLeftCorner() = D_g_first; + D->template bottomRightCorner() = D_h_second; } - return ProductLieGroup(g,h); + return ProductLieGroup(g, h); } + + /// Exponential map static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) { - Jacobian1 D_g_first; Jacobian2 D_h_second; + Jacobian1 D_g_first; + Jacobian2 D_h_second; G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); - H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); + H h = + traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); if (Hv) { Hv->setZero(); - Hv->template topLeftCorner() = D_g_first; - Hv->template bottomRightCorner() = D_h_second; + Hv->template topLeftCorner() = D_g_first; + Hv->template bottomRightCorner() = D_h_second; } - return ProductLieGroup(g,h); + return ProductLieGroup(g, h); } + + /// Logarithmic map static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}) { - Jacobian1 D_g_first; Jacobian2 D_h_second; - typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); - typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); + Jacobian1 D_g_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Logmap(p.first, Hp ? &D_g_first : 0); + typename traits::TangentVector v2 = + traits::Logmap(p.second, Hp ? &D_h_second : 0); TangentVector v; v << v1, v2; if (Hp) { Hp->setZero(); - Hp->template topLeftCorner() = D_g_first; - Hp->template bottomRightCorner() = D_h_second; + Hp->template topLeftCorner() = D_g_first; + Hp->template bottomRightCorner() = D_h_second; } return v; } - static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = {}) { + + /// Local coordinates (same as Logmap) + static TangentVector LocalCoordinates(const ProductLieGroup& p, + ChartJacobian Hp = {}) { return Logmap(p, Hp); } + + /// Right multiplication by exponential map ProductLieGroup expmap(const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } + + /// Logarithmic map for relative transformation TangentVector logmap(const ProductLieGroup& g) const { return ProductLieGroup::Logmap(between(g)); } + + /// Adjoint map Jacobian AdjointMap() const { const auto& adjG = traits::AdjointMap(this->first); const auto& adjH = traits::AdjointMap(this->second); @@ -179,13 +256,305 @@ class ProductLieGroup: public std::pair { adj.block(d1, d1, d2, d2) = adjH; return adj; } + + /// @} + + /// @name Testable interface + /// @{ + void print(const std::string& s = "") const { + std::cout << s << "ProductLieGroup" << std::endl; + traits::Print(this->first, " first"); + traits::Print(this->second, " second"); + } + + bool equals(const ProductLieGroup& other, double tol = 1e-9) const { + return traits::Equals(this->first, other.first, tol) && + traits::Equals(this->second, other.second, tol); + } + /// @} +}; + +/** + * @brief Template to construct the N-fold power of a Lie group + * Represents the group G^N = G x G x ... x G (N times) + * Assumes Lie group structure for G and N >= 2 + */ +template +class PowerLieGroup : public std::array { + static_assert(N >= 1, "PowerLieGroup requires N >= 1"); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsTestable); + + public: + /// Base array type + typedef std::array Base; + + protected: + /// Dimension of the base group + static constexpr size_t baseDimension = traits::dimension; + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor yields identity + PowerLieGroup() { this->fill(traits::Identity()); } + + /// Construct from array of group elements + PowerLieGroup(const Base& elements) : Base(elements) {} + + /// Construct from initializer list + PowerLieGroup(const std::initializer_list& elements) { + if (elements.size() != N) { + throw std::invalid_argument( + "PowerLieGroup: initializer list size must equal N"); + } + std::copy(elements.begin(), elements.end(), this->begin()); + } + + /// @} + /// @name Group Operations + /// @{ + + typedef multiplicative_group_tag group_flavor; + + /// Identity element + static PowerLieGroup Identity() { return PowerLieGroup(); } + + /// Group multiplication + PowerLieGroup operator*(const PowerLieGroup& other) const { + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + result[i] = traits::Compose((*this)[i], other[i]); + } + return result; + } + + /// Group inverse + PowerLieGroup inverse() const { + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + result[i] = traits::Inverse((*this)[i]); + } + return result; + } + + /// Compose with another element (same as operator*) + PowerLieGroup compose(const PowerLieGroup& g) const { return (*this) * g; } + + /// Calculate relative transformation + PowerLieGroup between(const PowerLieGroup& g) const { + return this->inverse() * g; + } + + /// @} + /// @name Manifold Operations + /// @{ + + /// Manifold dimension + static constexpr size_t dimension = N * baseDimension; + + /// Return manifold dimension + static size_t Dim() { return dimension; } + + /// Return manifold dimension + size_t dim() const { return dimension; } + + /// Tangent vector type + typedef Eigen::Matrix TangentVector; + + /// Chart Jacobian type + typedef OptionalJacobian ChartJacobian; + + /// Retract to manifold + PowerLieGroup retract(const TangentVector& v, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const { + if (H1 || H2) { + throw std::runtime_error( + "PowerLieGroup::retract derivatives not implemented yet"); + } + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + const auto vi = v.template segment(i * baseDimension); + result[i] = traits::Retract((*this)[i], vi); + } + return result; + } + + /// Local coordinates on manifold + TangentVector localCoordinates(const PowerLieGroup& g, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const { + if (H1 || H2) { + throw std::runtime_error( + "PowerLieGroup::localCoordinates derivatives not implemented yet"); + } + TangentVector v; + for (size_t i = 0; i < N; ++i) { + const auto vi = traits::Local((*this)[i], g[i]); + v.template segment(i * baseDimension) = vi; + } + return v; + } + + /// @} + /// @name Lie Group Operations + /// @{ + + protected: + /// Jacobian types for internal use + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix BaseJacobian; + + public: + /// Compose with Jacobians + PowerLieGroup compose(const PowerLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = {}) const { + std::array jacobians; + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + result[i] = traits::Compose((*this)[i], other[i], + H1 ? &jacobians[i] : nullptr); + } + if (H1) { + H1->setZero(); + for (size_t i = 0; i < N; ++i) { + H1->template block( + i * baseDimension, i * baseDimension) = jacobians[i]; + } + } + if (H2) *H2 = Jacobian::Identity(); + return result; + } + + /// Between with Jacobians + PowerLieGroup between(const PowerLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = {}) const { + std::array jacobians; + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + result[i] = traits::Between((*this)[i], other[i], + H1 ? &jacobians[i] : nullptr); + } + if (H1) { + H1->setZero(); + for (size_t i = 0; i < N; ++i) { + H1->template block( + i * baseDimension, i * baseDimension) = jacobians[i]; + } + } + if (H2) *H2 = Jacobian::Identity(); + return result; + } + + /// Inverse with Jacobian + PowerLieGroup inverse(ChartJacobian D) const { + std::array jacobians; + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + result[i] = traits::Inverse((*this)[i], D ? &jacobians[i] : nullptr); + } + if (D) { + D->setZero(); + for (size_t i = 0; i < N; ++i) { + D->template block( + i * baseDimension, i * baseDimension) = jacobians[i]; + } + } + return result; + } + + /// Exponential map + static PowerLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) { + std::array jacobians; + PowerLieGroup result; + for (size_t i = 0; i < N; ++i) { + const auto vi = v.template segment(i * baseDimension); + result[i] = traits::Expmap(vi, Hv ? &jacobians[i] : nullptr); + } + if (Hv) { + Hv->setZero(); + for (size_t i = 0; i < N; ++i) { + Hv->template block( + i * baseDimension, i * baseDimension) = jacobians[i]; + } + } + return result; + } + + /// Logarithmic map + static TangentVector Logmap(const PowerLieGroup& p, ChartJacobian Hp = {}) { + std::array jacobians; + TangentVector v; + for (size_t i = 0; i < N; ++i) { + const auto vi = traits::Logmap(p[i], Hp ? &jacobians[i] : nullptr); + v.template segment(i * baseDimension) = vi; + } + if (Hp) { + Hp->setZero(); + for (size_t i = 0; i < N; ++i) { + Hp->template block( + i * baseDimension, i * baseDimension) = jacobians[i]; + } + } + return v; + } + + /// Local coordinates (same as Logmap) + static TangentVector LocalCoordinates(const PowerLieGroup& p, + ChartJacobian Hp = {}) { + return Logmap(p, Hp); + } + + /// Right multiplication by exponential map + PowerLieGroup expmap(const TangentVector& v) const { + return compose(PowerLieGroup::Expmap(v)); + } + + /// Logarithmic map for relative transformation + TangentVector logmap(const PowerLieGroup& g) const { + return PowerLieGroup::Logmap(between(g)); + } + + /// Adjoint map + Jacobian AdjointMap() const { + Jacobian adj = Jacobian::Zero(); + for (size_t i = 0; i < N; ++i) { + const auto adjGi = traits::AdjointMap((*this)[i]); + adj.template block( + i * baseDimension, i * baseDimension) = adjGi; + } + return adj; + } + /// @} + /// @name Testable interface + /// @{ + void print(const std::string& s = "") const { + std::cout << s << "PowerLieGroup" << std::endl; + for (size_t i = 0; i < N; ++i) { + traits::Print((*this)[i], " component[" + std::to_string(i) + "]"); + } + } + + bool equals(const PowerLieGroup& other, double tol = 1e-9) const { + for (size_t i = 0; i < N; ++i) { + if (!traits::Equals((*this)[i], other[i], tol)) { + return false; + } + } + return true; + } + /// @} }; -// Define any direct product group to be a model of the multiplicative Group concept -template -struct traits > : internal::LieGroupTraits > {}; +/// Traits specialization for ProductLieGroup +template +struct traits> + : internal::LieGroup> {}; -} // namespace gtsam +/// Traits specialization for PowerLieGroup +template +struct traits> : internal::LieGroup> {}; +} // namespace gtsam diff --git a/gtsam/navigation/EquivariantFilter.h b/gtsam/navigation/EquivariantFilter.h new file mode 100644 index 0000000000..af524f13ab --- /dev/null +++ b/gtsam/navigation/EquivariantFilter.h @@ -0,0 +1,161 @@ +/** + * @file EquivariantFilter.h + * @brief Equivariant Filter (EqF) implementation + * + * @author Darshan Rajasekaran + * @author Jennifer Oum + * @author Rohan Bansal + * @author Frank Dellaert + * @date 2025 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include // For std::accumulate +#include +#include + +// All implementations are wrapped in this namespace to avoid conflicts +namespace gtsam { + +using namespace std; +using namespace gtsam; + +/** + * Equivariant Filter (EqF) for state estimation on Lie groups with states on + * manifolds. + * @tparam M Manifold type for the physical state. + * @tparam StateAction Functor encoding the right group action on the state. + */ +template +class EqF : public LieGroupEKF { + private: + using G = typename StateAction::G; + using Base = LieGroupEKF; + using TangentVector = typename traits::TangentVector; + + M xi_ref_; // Origin (reference) state on the manifold + StateAction act_on_ref_; // Group action on the reference state + Matrix InnovationLift_; // Innovation lift matrix + + public: + static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G. + + /// Number of calibration states (sensors), expected to be provided by G + static constexpr int n_cal = G::numSensors; + + /** + * Initialize EqF + * @param X0 Initial Lie group state + * @param x0 Reference manifold state (origin of the lifted coordinates) + * @param Sigma Initial covariance (Dim x Dim) + * @param m Number of direction sensors (must be at least 2) + */ + EqF(const G& X0, const M& x0, const Matrix& Sigma, int m) + : Base(X0, Sigma), xi_ref_(x0), act_on_ref_(x0) { + if (Sigma.rows() != Dim || Sigma.cols() != Dim) { + throw std::invalid_argument( + "Initial covariance dimensions must match the degrees of freedom"); + } + + if (m <= 1) { + throw std::invalid_argument( + "Number of direction sensors must be at least 2"); + } + + // Compute differential of action phi at origin + + Matrix Dphi0 = act_on_ref_.jacobianAtIdentity(); + InnovationLift_ = Dphi0.completeOrthogonalDecomposition().pseudoInverse(); + } + + /** + * Return estimated physical state on manifold M. + * Applies the group action of the current group estimate on the origin state. + */ + M stateEstimate() const { + // Group action X * xi_ref (defined for ABC as Group * State). + return act_on_ref_(this->X_); + } + + /// Return the current group estimate. + const G& groupEstimate() const { return this->X_; } + + /** + * Propagate the filter state. + * @tparam Lift Computes the lifted tangent vector. + * @tparam InputAction Provides system matrices derived from the input. + * @tparam u the input vector. + * @param Q Process noise covariance in lifted coordinates + * @param dt Time step + */ + template + void predict(const Vector6& u, const Matrix& Q, double dt) { + // auto dynamics = [this](const G& X, const Vector6& u, + // OptionalJacobian Df) { + // M state_est = act_on_ref_(X); + // return state_est.lift(u); + // }; + + // Map current group estimate to physical state on the manifold + M state_est = stateEstimate(); + + // Compute lifted tangent vector from state and input + Lift lift_u(u); + TangentVector xi = lift_u(state_est); + + InputAction psi_u(u); + Matrix Phi = psi_u.stateTransitionMatrix(this->X_, dt); + Matrix Bt = psi_u.inputMatrixBt(this->X_); + + G X_next = traits::Compose(this->X_, traits::Expmap(xi * dt)); + Matrix Q_process = Bt * Q * Bt.transpose() * dt; + Base::predict(X_next, Phi, Q_process); + } + + /** + * Update the filter state with a direction measurement. + * @tparam OutputAction Functor encoding the action on the measurement. + * @tparam Measurement Measurement type carrying y, d, Sigma, and cal_idx. + * @param y Direction measurement + */ + template + void update(const OutputAction& phi_y, const Matrix& R) { + Matrix Ct = phi_y.measurementMatrixC(); + + // Kalman gain + Matrix Dt = phi_y.outputMatrixDt(this->X_); + Matrix S = Ct * this->P_ * Ct.transpose() + Dt * R * Dt.transpose(); + Matrix K = this->P_ * Ct.transpose() * S.inverse(); + + // Innovation lift + // TODO(Frank): Why inverse ???? + Vector3 innovation = phi_y.innovation(this->X_.inverse()); + TangentVector delta_xi = InnovationLift_ * (K * innovation); + + // Update state estimate (left-multiply by exp(delta_xi)) + // TODO(Frank): try X_ = traits::Retract(X_, delta_xi); + this->X_ = traits::Compose(traits::Expmap(delta_xi), this->X_); + + // Update covariance + Matrix I_n = Matrix::Identity(this->P_.rows(), this->P_.cols()); + this->P_ = (I_n - K * Ct) * this->P_; + } +}; + +} // namespace gtsam diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 007ed3ad09..edbf52cab0 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -64,6 +64,10 @@ namespace gtsam { static_assert(IsLieGroup::value, "Template parameter G must be a GTSAM Lie Group"); } + /// Expose base class predict method, + /// predict(const M& X_next, const Jacobian& F, const Covariance& Q) + using Base::predict; + /** * SFINAE check for correctly typed state-dependent dynamics function. * Signature: TangentVector f(const G& X, OptionalJacobian Df) @@ -109,11 +113,10 @@ namespace gtsam { // State transition Jacobian for left-invariant EKF: *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; } - return this->X_.compose(U); + return traits::Compose(this->X_, U); } } - /** * Predict step with state-dependent dynamics: * Uses predictMean to compute X_{k+1} and A, then updates covariance. @@ -131,8 +134,8 @@ namespace gtsam { if constexpr (Dim == Eigen::Dynamic) { A.resize(this->n_, this->n_); } - this->X_ = predictMean(std::forward(f), dt, A); - this->P_ = A * this->P_ * A.transpose() + Q; + G X_next = predictMean(std::forward(f), dt, A); + predict(X_next, A, Q); } /** diff --git a/gtsam_unstable/geometry/ABC.h b/gtsam_unstable/geometry/ABC.h new file mode 100644 index 0000000000..43f8d3e056 --- /dev/null +++ b/gtsam_unstable/geometry/ABC.h @@ -0,0 +1,499 @@ +/** + * @file ABC.h + * @brief Core components for Attitude-Bias-Calibration systems + * + * This file contains fundamental components and utilities for the ABC system + * based on the paper "Overcoming Bias: Equivariant Filter Design for Biased + * Attitude Estimation with Online Calibration" by Fornasier et al. + * + * @author Darshan Rajasekaran + * @author Jennifer Oum + * @author Rohan Bansal + * @author Frank Dellaert + * @date 2025 + */ + +#pragma once + +/** + * @file ABC.h + * @brief Core components for Attitude-Bias-Calibration systems + * + * This file contains fundamental components and utilities for the ABC system + * based on the paper "Overcoming Bias: Equivariant Filter Design for Biased + * Attitude Estimation with Online Calibration" by Fornasier et al. + * Authors: Darshan Rajasekaran & Jennifer Oum + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +namespace abc { + +/// Convert angular velocity vector to mathematical input (ω, 0) +inline Vector6 toInputVector(const Vector3& w) { + return (Vector6() << w, Z_3x1).finished(); +} + +/// Bundle of calibration rotations modeled as a Lie group +template +using Calibrations = PowerLieGroup; + +//======================================================================== +// State Manifold +//======================================================================== + +/// State class representing the state of the Biased Attitude System +template +struct State { + Rot3 R; // Attitude rotation matrix R + Vector3 b; // Gyroscope bias b + Calibrations S; // Sensor calibrations S + + static constexpr int dimension = 6 + 3 * N; + + /// Constructor + State(const Rot3& R = Rot3(), const Vector3& b = Z_3x1, + const Calibrations& S = Calibrations()) + : R(R), b(b), S(S) {} + + /// Identity function + static State identity() { return State(Rot3(), Z_3x1, Calibrations()); } + + /** + * Compute Local coordinates in the state relative to another state. + * @param other The other state + * @return Local coordinates in the tangent space + */ + Vector localCoordinates(const State& other) const { + Vector eps(dimension); + + // First 3 elements - attitude + eps.head<3>() = R.logmap(other.R); + // Next 3 elements - bias + eps.segment<3>(3) = other.b - b; + + // Remaining elements - calibrations + eps.template segment<3 * N>(6) = S.logmap(other.S); + + return eps; + } + + /** + * Retract from tangent space back to the manifold + * @param v Vector in the tangent space + * @return New state + */ + State retract(const Vector& v) const { + if (v.size() != static_cast(dimension)) { + throw std::invalid_argument( + "Vector size does not match state dimensions"); + } + Rot3 newR = R.expmap(v.head<3>()); + Vector3 newB = b + v.segment<3>(3); + typename Calibrations::TangentVector deltaS; + deltaS = v.template segment<3 * N>(6); + Calibrations 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& 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>::Equals(S, other.S, tol); + } +}; + +//======================================================================== +// Symmetry Group +//======================================================================== + +/** + * Symmetry group defined as the product Pose3 x Calibrations + * The Pose3 component models the (SO(3) ⋉ R^3) part acting on attitude/bias, + * while Calibrations captures the N sensor calibration rotations. + */ +template +struct Group : public ProductLieGroup> { + using Base = ProductLieGroup>; + using typename Base::ChartJacobian; + using typename Base::Jacobian; + using typename Base::TangentVector; + + static constexpr int dimension = Base::dimension; + static constexpr size_t numSensors = n; + + Group() : Base() {} + Group(const Pose3& pose, const Calibrations& calibrations) + : Base(pose, calibrations) {} + Group(const Base& base) : Base(base) {} + Group(const Rot3& A, const Matrix3& a, const Calibrations& B) + : Group(Pose3(A, Point3(Rot3::Vee(a))), B) {} + + static Group Identity() { return Group(); } + + Group operator*(const Group& other) const { + return Group(Base::operator*(other)); + } + + Group compose(const Group& other, ChartJacobian H1 = ChartJacobian(), + ChartJacobian H2 = ChartJacobian()) const { + return Group(Base::compose(other, H1, H2)); + } + + Group between(const Group& other, ChartJacobian H1 = ChartJacobian(), + ChartJacobian H2 = ChartJacobian()) const { + return Group(Base::between(other, H1, H2)); + } + + Group inverse(ChartJacobian D = ChartJacobian()) const { + return Group(Base::inverse(D)); + } + + Group retract(const TangentVector& v, ChartJacobian H1 = ChartJacobian(), + ChartJacobian H2 = ChartJacobian()) const { + return Group(Base::retract(v, H1, H2)); + } + + Group expmap(const TangentVector& v) const { return Group(Base::expmap(v)); } + + TangentVector logmap(const Group& g) const { return Base::logmap(g); } + + static Group Expmap(const TangentVector& v, + ChartJacobian Hv = ChartJacobian()) { + return Group(Base::Expmap(v, Hv)); + } + + static TangentVector Logmap(const Group& g, + ChartJacobian Hg = ChartJacobian()) { + return Base::Logmap(g, Hg); + } + + const Pose3& pose() const { return this->first; } + Rot3 A() const { return this->first.rotation(); } + Vector3 a() const { return this->first.translation(); } + const Calibrations& calibrations() const { return this->second; } + + void print(const std::string& s = "") const { + if (!s.empty()) std::cout << s << " "; + std::cout << "Group<" << n << ">" << std::endl; + pose().print(" Pose"); + for (size_t i = 0; i < n; ++i) { + const std::string label = " S[" + std::to_string(i) + "]"; + calibrations()[i].print(label); + } + } + + bool equals(const Group& other, double tol = 1e-9) const { + if (!pose().equals(other.pose(), tol)) return false; + return traits>::Equals(calibrations(), other.calibrations(), + tol); + } +}; + +//======================================================================== +// Group Actions on State, Input, and Output Manifolds +//======================================================================== + +/** + * The symmetry group G is defined as the product Pose3 x Calibrations. + * The Pose3 component models the (SO(3) ⋉ R^3) part acting on attitude/bias, + * while Calibrations captures the N sensor calibration rotations. + * + * The group action is defined as a function object below, + * applied to a given state x, specified in constructor. + */ +template +struct StateAction { + using M = State; + using G = Group; + + const M xi_; // Reference state + + /** + * Construct action functor with reference state + * @param xi State object + */ + StateAction(const M& xi) : xi_(xi) {} + + /** + * Implements group actions on the states + * @param g An element of the symmetry group G. + * @return Transformed state + */ + M operator()(const G& g) const { + const Rot3 new_R = xi_.R * g.A(); + const Rot3 At = g.A().inverse(); + const Vector3 new_b = At * (xi_.b - g.a()); + Calibrations new_S; + const Calibrations& B = g.calibrations(); + for (size_t i = 0; i < N; i++) new_S[i] = At * xi_.S[i] * B[i]; + return {new_R, new_b, new_S}; + } + + /** + * The Jacobian of the action at the identity of the symmetry group G + * @return A matrix representing the jacobian of the state action + */ + Matrix jacobianAtIdentity() const { + Matrix H = Matrix::Zero(M::dimension, G::dimension); + + // Rotation block: δθ maps directly to the state's rotational tangent. + H.block<3, 3>(0, 0) = I_3x3; + + // Bias block: δb = -δa + xi_.b × δθ. + H.block<3, 3>(3, 0) = Rot3::Hat(xi_.b); + H.block<3, 3>(3, 3) = -I_3x3; + + // Calibration blocks: δs_i = δσ_i - S_i^{-1} δθ. + for (size_t i = 0; i < N; ++i) { + const Matrix3 S_inv = xi_.S[i].inverse().matrix(); + const size_t row = 6 + 3 * i; + const size_t col = 6 + 3 * i; + H.block<3, 3>(row, 0) = -S_inv; // B[i] is identity for G::Identity + H.block<3, 3>(row, col) = I_3x3; + } + + return H; + } +}; + +/** + * Functor computing the lifted tangent vector from a state and fixed input. + */ +template +struct Lift { + using M = State; + using G = Group; + using Input = Vector6; + + explicit Lift(const Input& u) : u_(u) {} + + typename G::TangentVector operator()(const M& xi) const { + Vector3 w = u_.head<3>(); + Vector3 corrected_w = w - xi.b; + + typename G::TangentVector L; + L.template head<3>() = corrected_w; + L.template segment<3>(3) = -Rot3::Hat(w) * xi.b; + for (size_t i = 0; i < N; i++) { + L.template segment<3>(6 + 3 * i) = xi.S[i].unrotate(corrected_w); + } + + return L; + } + + private: + Input u_; +}; + +/** + * Functor encoding the right group action on the mathematical input u. + * For a fixed u = (ω, 0), applying X = (A, a, B) ∈ G yields + * φ_u(X) = (A^{-1}(ω - a), 0). + */ +template +struct InputAction { + using G = Group; + using Input = Vector6; + + static Matrix processNoise(const Matrix& Sigma) { + std::vector blocks{Sigma}; + blocks.insert(blocks.end(), N, 1e-9 * I_3x3); + return gtsam::diag(blocks); + } + + const Input u_; + + explicit InputAction(const Input& u) : u_(u) {} + + Input operator()(const G& X) const { + const Rot3 A = X.A(); + const Vector3 a = X.a(); + Input result; + result.head<3>() = A.unrotate(u_.head<3>() - a); + result.tail<3>() = Z_3x1; + return result; + } + + Matrix jacobianAtIdentity() const { + Matrix H = Matrix::Zero(Input::RowsAtCompileTime, G::dimension); + H.block<3, 3>(0, 0) = Rot3::Hat(u_.head<3>()); + H.block<3, 3>(0, 3) = -I_3x3; + // Remaining blocks stay zero: the virtual input is unaffected by + // calibrations. + return H; + } + + Matrix stateTransitionMatrix(const G& X_hat, double dt) const { + const Vector3 omega_tilde = + this->operator()(X_hat.inverse()).template head<3>(); + Matrix3 W0 = Rot3::Hat(omega_tilde); + Matrix Phi1 = Matrix::Zero(6, 6); + Matrix3 W0_sq = W0 * W0; + Matrix3 Phi12 = -dt * (I_3x3 + 0.5 * dt * W0 + (dt * dt / 6.0) * W0_sq); + Matrix3 Phi22 = I_3x3 + dt * W0 + 0.5 * dt * dt * W0_sq; + Phi1.block<3, 3>(0, 0) = I_3x3; + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + + std::vector blocks; + blocks.push_back(Phi1); + blocks.insert(blocks.end(), N, Phi22); + return gtsam::diag(blocks); + } + + Matrix stateMatrixA(const G& X_hat) const { + const Vector3 omega_tilde = + this->operator()(X_hat.inverse()).template head<3>(); + Matrix3 W0 = Rot3::Hat(omega_tilde); + + Matrix A1 = Matrix::Zero(6, 6); + A1.block<3, 3>(0, 3) = -I_3x3; + A1.block<3, 3>(3, 3) = W0; + + std::vector blocks{A1}; + blocks.insert(blocks.end(), N, W0); + return gtsam::diag(blocks); + } + + Matrix inputMatrix(const G& X_hat) const { + const Matrix3 A_matrix = X_hat.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) = X_hat.calibrations()[i].matrix(); + } + + return gtsam::diag({B1, B2}); + } + + Matrix inputMatrixBt(const G& X_hat) const { return inputMatrix(X_hat); } +}; + +/** + * Functor encoding the right group action on a direction measurement y. + * For a fixed y coming from sensor Index, applying X = (A, a, B) ∈ G yields + * φ_y(X) = B[Index]^{-1} y. + */ +template +struct OutputAction { + using G = Group; + using Output = Vector3; + + OutputAction(const Unit3& y, const Unit3& d, int index) + : y_(y), d_(d), index_(index) { + if (index_ >= static_cast(N)) { + throw std::out_of_range("OutputAction index out of range"); + } + } + + Output operator()(const G& X) const { + if (index_ == -1) { + const Rot3 A = X.A(); + return A.unrotate(y_.unitVector()); + } else { + const Calibrations& B = X.calibrations(); + return B[index_].unrotate(y_.unitVector()); + } + } + + Matrix jacobianAtIdentity() const { + Matrix H = Matrix::Zero(Output::RowsAtCompileTime, G::dimension); + if (index_ == -1) { + H.block<3, 3>(0, 0) = Rot3::Hat(y_.unitVector()); + } else { + H.block<3, 3>(0, 6 + 3 * index_) = Rot3::Hat(y_.unitVector()); + } + return H; + } + + Vector3 innovation(const G& X) const { + const Vector3 transformed_y = this->operator()(X); + return Rot3::Hat(d_.unitVector()) * transformed_y; + } + + /** + * Computes the linearized measurement matrix. The structure depends on + * whether the sensor has a calibration state + * @param d reference direction + * @return Measurement matrix + */ + Matrix measurementMatrixC() const { + 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; + } + + /** + * Computes the measurement uncertainty propagation matrix + * @return Returns B[idx] for calibrated sensors, A for uncalibrated + */ + Matrix outputMatrixDt(const G& X_hat) const { + if (index_ >= 0) { + return X_hat.calibrations()[index_].matrix(); + } else { + return X_hat.A().matrix(); + } + } + + private: + Unit3 y_; // measured direction + Unit3 d_; // reference direction + int index_; // sensor index +}; + +} // namespace abc + +template +struct traits> : public internal::Manifold> {}; + +template +struct traits> : public internal::Manifold> { +}; + +template +struct traits> : internal::LieGroup> {}; + +template +struct traits> : internal::LieGroup> {}; + +} // namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testABC.cpp b/gtsam_unstable/geometry/tests/testABC.cpp new file mode 100644 index 0000000000..13eda30a06 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testABC.cpp @@ -0,0 +1,581 @@ +/** + * @file testABC.cpp + * @brief Test file for ABC (Attitude-Bias-Calibration) system components + * + * @author Darshan Rajasekaran + * @author Jennifer Oum + * @author Rohan Bansal + * @author Frank Dellaert + * @date 2025 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +// Define N for testing purposes, e.g., 2 calibration states +using State = abc::State<2>; +using Group = abc::Group<2>; +using StateAction = abc::StateAction<2>; +using Lift = abc::Lift<2>; +using InputAction = abc::InputAction<2>; +using OutputAction = abc::OutputAction<2>; +using Calibrations = abc::Calibrations<2>; + +/* ************************************************************************* */ +namespace abc_examples { +const Rot3 A1 = Rot3::Rx(0.1); +const Vector3 t1(0.01, 0.02, 0.03); +const Calibrations B1{Rot3::Ry(0.05), Rot3::Rz(0.06)}; +const State xi1(A1, t1, B1); +const Group g1(Pose3(A1, t1), B1); + +const Rot3 A2 = Rot3::Ry(0.2); +const Vector3 t2(0.04, 0.05, 0.06); +const Calibrations B2{Rot3::Rz(0.07), Rot3::Rx(0.08)}; +const State xi2(A2, t2, B2); +const Group g2(Pose3(A2, t2), B2); + +const Vector3 omega(1, 2, 3); +const Vector6 u = abc::toInputVector(omega); + +const Vector3 omega2(0.01, -0.02, 0.015); +const Vector6 u2 = abc::toInputVector(omega2); + +const Unit3 y(1, 0, 0), d(0, 1, 0); +} // namespace abc_examples + +/* ************************************************************************* */ +TEST(ABC, State) { + State state1 = abc_examples::xi1; + + EXPECT(assert_equal(abc_examples::A1, state1.R)); + EXPECT(assert_equal(abc_examples::t1, state1.b)); + EXPECT(assert_equal(abc_examples::B1, state1.S)); + + // Test identity + State identityState = State::identity(); + EXPECT(assert_equal(identityState.R, Rot3())); + EXPECT(assert_equal(identityState.b, Z_3x1)); + Calibrations expectedS_id; + EXPECT(assert_equal(identityState.S, expectedS_id)); + + // Test localCoordinates and retract (manifold properties) + Rot3 R2 = Rot3::Rx(0.2); + Vector3 b2(0.05, 0.06, 0.07); + Calibrations S2; + S2[0] = Rot3::Ry(0.1); + S2[1] = Rot3::Rz(0.15); + State state2(R2, b2, S2); + + Vector actual_local = state1.localCoordinates(state2); + State retracted_state2 = state1.retract(actual_local); + EXPECT(assert_equal(retracted_state2.R, state2.R)); + EXPECT(assert_equal(retracted_state2.b, state2.b)); + EXPECT(assert_equal(retracted_state2.S, state2.S)); + + // Test localCoordinates at identity + Vector expected_identity_local = Vector::Zero(6 + 3 * 2); + EXPECT(assert_equal(identityState.localCoordinates(identityState), + expected_identity_local)); + + // Test retract at identity + Vector v_test = Vector::Zero(6 + 3 * 2); + v_test.head<3>() << 0.1, 0.2, 0.3; // R + v_test.segment<3>(3) << 0.01, 0.02, 0.03; // b + v_test.segment<3>(6) << 0.05, 0.06, 0.07; // S[0] + v_test.segment<3>(9) << 0.08, 0.09, 0.10; // S[1] + + State retracted_from_id = identityState.retract(v_test); + EXPECT(assert_equal(retracted_from_id.R, Rot3::Expmap(v_test.head<3>()))); + EXPECT(assert_equal(retracted_from_id.b, v_test.segment<3>(3).eval())); + EXPECT(assert_equal(retracted_from_id.S[0], + Rot3::Expmap(v_test.segment<3>(6).eval()))); + EXPECT(assert_equal(retracted_from_id.S[1], + Rot3::Expmap(v_test.segment<3>(9).eval()))); + + // Test retract invalid argument + CHECK_EXCEPTION(identityState.retract(Vector::Zero(1)), + std::invalid_argument); +} + +/* ************************************************************************* */ +TEST(ABC, GroupOperations) { + using namespace abc_examples; + + // Test group multiplication + Group g1_g2 = g1 * g2; + EXPECT(assert_equal(g1_g2.A(), A1 * A2)); + Vector3 expected_a = t1 + A1.matrix() * t2; + EXPECT(assert_equal(g1_g2.a(), expected_a)); + EXPECT(assert_equal(g1_g2.calibrations()[0], B1[0] * B2[0])); + EXPECT(assert_equal(g1_g2.calibrations()[1], B1[1] * B2[1])); + + // Test inverse + Group g1_inv = g1.inverse(); + EXPECT(assert_equal(g1_inv.A(), A1.inverse())); + Vector3 expected_a_inv = -A1.inverse().matrix() * t1; + EXPECT(assert_equal(g1_inv.a(), expected_a_inv)); + EXPECT(assert_equal(g1_inv.calibrations()[0], B1[0].inverse())); + EXPECT(assert_equal(g1_inv.calibrations()[1], B1[1].inverse())); + + // Test g * g.inv() == identity + Group identity_check = g1 * g1_inv; + Group expected_identity = Group::Identity(); + EXPECT(assert_equal(identity_check.A(), expected_identity.A())); + EXPECT(assert_equal(identity_check.a(), expected_identity.a())); + EXPECT(assert_equal(identity_check.calibrations(), + expected_identity.calibrations())); + + // Test Expmap and Logmap + Group::TangentVector v_tangent = Group::TangentVector::Zero(); + v_tangent.head<3>() << 0.1, 0.2, 0.3; // For A + v_tangent.segment<3>(3) << 0.01, 0.02, 0.03; // For 'a' part + v_tangent.segment<3>(6) << 0.04, 0.05, 0.06; // For B[0] + v_tangent.segment<3>(9) << 0.07, 0.08, 0.09; // For B[1] + + Group g_exp = Group::Expmap(v_tangent); + EXPECT(assert_equal(Group::Logmap(g_exp), v_tangent)); + + // Test retract on G + Group g_retracted = g1.retract(v_tangent); + + const Group composed = g1 * Group::Expmap(v_tangent); + EXPECT(assert_equal(g_retracted.A(), composed.A())); + EXPECT(assert_equal(g_retracted.a(), composed.a())); + EXPECT(assert_equal(g_retracted.calibrations(), composed.calibrations())); + + // Test traits for G + const Group identity = Group::Identity(); + EXPECT(assert_equal(traits::Identity().A(), identity.A())); + EXPECT(assert_equal(traits::Identity().a(), identity.a())); + EXPECT(assert_equal(traits::Identity().calibrations(), + identity.calibrations())); + // testLie(g1, g2); +} + +//****************************************************************************** +TEST(ABC, AdjointMap) { + using namespace abc_examples; + + Group::Jacobian adjoint = g1.AdjointMap(); + Group::Jacobian expected = Group::Jacobian::Zero(); + expected.block<6, 6>(0, 0) = g1.pose().AdjointMap(); + for (size_t i = 0; i < Group::numSensors; ++i) { + expected.block<3, 3>(6 + 3 * i, 6 + 3 * i) = + g1.calibrations()[i].AdjointMap(); + } + + EXPECT(assert_equal(adjoint, expected)); +} + +/* ************************************************************************* */ +TEST(ABC, StateAction) { + using namespace abc_examples; + + // Test State Action (G * State) + State transformed_xi = StateAction(xi2)(g1); + EXPECT(assert_equal(transformed_xi.R, xi2.R * g1.A())); + EXPECT(assert_equal(transformed_xi.b, + g1.A().inverse().matrix() * (xi2.b - g1.a()))); + EXPECT(assert_equal(transformed_xi.S[0], + g1.A().inverse() * xi2.S[0] * g1.calibrations()[0], + 1e-9)); + EXPECT(assert_equal(transformed_xi.S[1], + g1.A().inverse() * xi2.S[1] * g1.calibrations()[1], + 1e-9)); +} + +/* ************************************************************************* */ +// A right action satisfies φ(xi, g1 * g2) = φ(φ(xi, g1), g2), i.e. applying +// g1 then g2 equals applying their product, with the multiplication happening +// on the right. +TEST(ABC, StateActionIsRightAction) { + using namespace abc_examples; + + // Create action functors + StateAction phi_xi1(xi1); + + // Left side: apply composed group element (g1 * g2) to xi + const State left_side = phi_xi1(g1 * g2); + + // Right side: apply g1 first, then g2 to the result + const State xi1_g1 = phi_xi1(g1); + const State right_side = StateAction(xi1_g1)(g2); + + // For a right action, these should be equal + EXPECT(assert_equal(left_side, right_side)); + + // Additional test with g1 and g2 reversed + const State left_side_2 = phi_xi1(g2 * g1); + + const State xi1_g2 = phi_xi1(g2); + const State right_side_2 = StateAction(xi1_g2)(g1); + + EXPECT(assert_equal(left_side_2, right_side_2)); +} + +/* ************************************************************************* */ +TEST(ABC, StateActionJacobianAnalytic) { + using namespace abc_examples; + + StateAction action_xi1(xi1); + Matrix analytic1 = action_xi1.jacobianAtIdentity(); + Matrix numerical1 = gtsam::numericalDerivative11( + [&](const Group& g) { return action_xi1(g); }, Group::Identity()); + EXPECT(assert_equal(analytic1, numerical1)); + + StateAction action_xi2(xi2); + Matrix analytic2 = action_xi2.jacobianAtIdentity(); + Matrix numerical2 = gtsam::numericalDerivative11( + [&](const Group& g) { return action_xi2(g); }, Group::Identity()); + EXPECT(assert_equal(analytic2, numerical2)); +} + +/* ************************************************************************* */ +TEST(ABC, LiftFunctor) { + State xi = abc_examples::xi1; + + // Setup input + Vector3 omega(0.5, 0.6, 0.7); + Vector6 u = abc::toInputVector(omega); + typename Group::TangentVector L = Lift(u)(xi); + + // Expected values + Vector3 expected_L_head = omega - xi.b; + Vector3 expected_L_segment3 = -Rot3::Hat(omega) * xi.b; + Vector3 expected_L_segment6_0 = xi.S[0].inverse().matrix() * expected_L_head; + Vector3 expected_L_segment6_1 = xi.S[1].inverse().matrix() * expected_L_head; + + EXPECT(assert_equal(L.head<3>(), expected_L_head)); + EXPECT(assert_equal(L.segment<3>(3), expected_L_segment3)); + EXPECT(assert_equal(L.segment<3>(6), expected_L_segment6_0)); + EXPECT(assert_equal(L.segment<3>(9), expected_L_segment6_1)); +} + +/* ************************************************************************* */ +TEST(ABC, InputAction) { + using namespace abc_examples; + + InputAction psi_u(u); + + Vector6 transformed_u = psi_u(g1); + EXPECT(assert_equal(transformed_u.head<3>(), + g1.A().unrotate(omega - g1.a()))); + EXPECT(assert_equal(transformed_u.tail<3>(), Z_3x1, + 1e-9)); // Virtual input stays zero + + EXPECT(assert_equal(transformed_u, InputAction(u)(g1))); +} + +/* ************************************************************************* */ +// A right action satisfies φ(xi, g1 * g2) = φ(φ(xi, g1), g2), i.e. applying +// g1 then g2 equals applying their product, with the multiplication happening +// on the right. +TEST(ABC, InputActionIsRightAction) { + using namespace abc_examples; + + // Create action functors + InputAction psi_u(u); + + // Left side: apply composed group element (g1 * g2) to xi + const Vector6 left_side = psi_u(g1 * g2); + + // Right side: apply g1 first, then g2 to the result + const Vector6 u_g1 = psi_u(g1); + const Vector6 right_side = InputAction(u_g1)(g2); + // For a right action, these should be equal + EXPECT(assert_equal(left_side, right_side)); + + // Additional test with g1 and g2 reversed + const Vector6 left_side_2 = psi_u(g2 * g1); + + const Vector6 u_g2 = psi_u(g2); + const Vector6 right_side_2 = InputAction(u_g2)(g1); + + EXPECT(assert_equal(left_side_2, right_side_2)); +} + +/* ************************************************************************* */ +TEST(ABC, InputActionJacobianAnalytic) { + using namespace abc_examples; + + // Create action functors + InputAction psi_u(u); + Matrix analytic = psi_u.jacobianAtIdentity(); + Matrix numerical = gtsam::numericalDerivative11( + [&](const Group& g) { return psi_u(g); }, Group::Identity()); + + EXPECT(assert_equal(analytic, numerical)); +} + +/* ************************************************************************* */ +TEST(ABC, InputAction_stateMatrixA) { + using namespace abc_examples; + + // Setup input + InputAction psi_u(u); + Matrix A_matrix = psi_u.stateMatrixA(g1); + Matrix3 W0 = Rot3::Hat(psi_u(g1.inverse()).head<3>()); + + Matrix expected_A1 = Matrix::Zero(6, 6); + expected_A1.block<3, 3>(0, 3) = -I_3x3; + expected_A1.block<3, 3>(3, 3) = W0; + + Matrix expected_A2 = gtsam::diag({W0, W0}); + Matrix expected_A_matrix = gtsam::diag({expected_A1, expected_A2}); + + EXPECT(assert_equal(A_matrix, expected_A_matrix)); +} + +/* ************************************************************************* */ +TEST(ABC, InputAction_stateTransitionMatrix) { + Group X_hat = abc_examples::g1; + + // Setup input + Vector3 omega(0.5, 0.6, 0.7); + double dt = 0.1; + + Vector6 u = abc::toInputVector(omega); + InputAction psi_u(u); + Matrix Phi = psi_u.stateTransitionMatrix(X_hat, dt); + Matrix3 W0 = Rot3::Hat(psi_u(X_hat.inverse()).head<3>()); + Matrix Phi1 = Matrix::Zero(6, 6); + Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0); + Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0; + + Phi1.block<3, 3>(0, 0) = I_3x3; + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + Matrix Phi2 = gtsam::diag({Phi22, Phi22}); + Matrix expected_Phi = gtsam::diag({Phi1, Phi2}); + + EXPECT(assert_equal(Phi, expected_Phi)); +} + +/* ************************************************************************* */ +TEST(ABC, InputAction_inputMatrix) { + Group X_hat = abc_examples::g1; + InputAction psi_u(abc_examples::u); + + Matrix input_matrix = psi_u.inputMatrix(X_hat); + + const Matrix3 X_hat_rot = X_hat.A().matrix(); + Matrix expected_B1 = gtsam::diag({X_hat_rot, X_hat_rot}); + Matrix expected_B2(3 * 2, 3 * 2); + expected_B2.setZero(); + for (size_t i = 0; i < 2; ++i) { + expected_B2.block<3, 3>(3 * i, 3 * i) = X_hat.calibrations()[i].matrix(); + } + Matrix expected_input_matrix = gtsam::diag({expected_B1, expected_B2}); + + EXPECT(assert_equal(input_matrix, expected_input_matrix)); +} + +/* ************************************************************************* */ +TEST(ABC, InputAction_processNoise) { + Matrix Sigma = (Matrix(6, 6) << 1, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 3, + 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 6) + .finished(); + + Matrix Q = InputAction::processNoise(Sigma); + + Matrix expected_Q_cal_part = 1e-9 * I_6x6; + Matrix expected_Q = gtsam::diag({Sigma, expected_Q_cal_part}); + + EXPECT(assert_equal(Q, expected_Q)); +} + +/* ************************************************************************* */ +TEST(ABC, InputAction_inputMatrixBt) { + // This function is identical to inputMatrix, so we'll test its output matches + // inputMatrix. + Group X_hat = abc_examples::g1; + InputAction psi_u(abc_examples::u); + + Matrix input_matrix_Bt = psi_u.inputMatrixBt(X_hat); + Matrix input_matrix = psi_u.inputMatrix(X_hat); // Reference from other func + + EXPECT(assert_equal(input_matrix_Bt, input_matrix)); +} + +/* ************************************************************************* */ +TEST(ABC, OutputAction) { + using namespace abc_examples; + + // Test outputAction (calibrated sensor) + int cal_idx = 0; + OutputAction phi_y(y, d, cal_idx); + Vector3 transformed_y_calibrated = phi_y(g1); + EXPECT(assert_equal(transformed_y_calibrated, + g1.calibrations()[0].unrotate(y.unitVector()))); + + // Test outputAction (uncalibrated sensor) + int uncalibrated_idx = -1; + OutputAction uncalibrated_phi_y(y, d, uncalibrated_idx); + Vector3 transformed_y_uncalibrated = uncalibrated_phi_y(g1); + EXPECT(assert_equal(transformed_y_uncalibrated, + g1.A().unrotate(y.unitVector()))); + + CHECK_EXCEPTION(OutputAction(y, d, 2), std::out_of_range); +} + +/* ************************************************************************* */ +// A right action satisfies φ(y, g1 * g2) = φ(φ(y, g1), g2), mirroring the +// behavior tested for state and input actions. +TEST(ABC, OutputActionIsRightAction) { + using namespace abc_examples; + + OutputAction phi_y(y, d, 0); + + const Vector3 left_side = phi_y(g1 * g2); + + Unit3 y_g1(phi_y(g1)); + const Vector3 right_side = OutputAction(y_g1, d, 0)(g2); + EXPECT(assert_equal(left_side, right_side)); + + const Vector3 left_side_2 = phi_y(g2 * g1); + Unit3 y_g2(phi_y(g2)); + const Vector3 right_side_2 = OutputAction(y_g2, d, 0)(g1); + + EXPECT(assert_equal(left_side_2, right_side_2)); +} + +/* ************************************************************************* */ +TEST(ABC, OutputActionJacobianAnalytic) { + using namespace abc_examples; + + OutputAction phi_y(y, d, 0); + Matrix analytic = phi_y.jacobianAtIdentity(); + Matrix numerical = gtsam::numericalDerivative11( + [&](const Group& g) { return phi_y(g); }, Group::Identity()); + + EXPECT(assert_equal(analytic, numerical)); +} + +/* ************************************************************************* */ +TEST(ABC, OutputAction_measurementMatrixC) { + using namespace abc_examples; + Matrix3 wedge_d = Rot3::Hat(d.unitVector()); + + // Test with calibrated sensor (idx = 0) + int cal_idx = 0; + OutputAction phi_y(y, d, cal_idx); + Matrix C_cal = phi_y.measurementMatrixC(); + + Matrix expected_Cc_cal = Matrix::Zero(3, 3 * 2); + expected_Cc_cal.block<3, 3>(0, 3 * cal_idx) = wedge_d; + + Matrix expected_temp_cal(3, 6 + 3 * 2); + expected_temp_cal.block<3, 3>(0, 0) = wedge_d; + expected_temp_cal.block<3, 3>(0, 3) = Matrix3::Zero(); + expected_temp_cal.block(0, 6, 3, 3 * 2) = expected_Cc_cal; + Matrix expected_C_cal = wedge_d * expected_temp_cal; + + EXPECT(assert_equal(C_cal, expected_C_cal)); +} + +/* ************************************************************************* */ +TEST(ABC, EqFilter) { + using namespace abc_examples; + + using G = Group; + const State xi_ref = xi1; // Reference state (xi circle) + const int numSensors = 2; + + Matrix initialSigma = Matrix::Identity(G::dimension, G::dimension); + initialSigma.diagonal().head<3>() = + Vector3::Constant(0.1); // Attitude uncertainty + initialSigma.diagonal().segment<3>(3) = + Vector3::Constant(0.01); // Bias uncertainty + initialSigma.diagonal().tail<3>() = + Vector3::Constant(0.1); // Calibration uncertainty + + const G g_0; + EqF> filter(g_0, xi_ref, initialSigma, numSensors); + + // Check initial state + EXPECT(assert_equal(g_0, filter.state())); + EXPECT(assert_equal(g_0, filter.groupEstimate())); + + // Perform a prediction step + Matrix Sigma = I_6x6; + double dt = 0.01; + Matrix Q = InputAction::processNoise(Sigma); + filter.predict(u2, Q, dt); + + // Regression + Group expected({Rot3(1, 0.00015, -0.0004, // + -0.00015, 1, 3e-08, // + 0.0004, 3e-08, 1), + Point3(9.00091e-06, 1.49932e-06, -3.9982e-06)}, + {Rot3(1, 0.000149811, -0.000400001, // + -0.000149814, 1, -7.46691e-06, // + 0.000399999, 7.52684e-06, 1), + Rot3(1, 0.000150005, -0.000399278, // + -0.000149995, 1, 2.40155e-05, // + 0.000399282, -2.39557e-05, 1)}); + EXPECT(assert_equal(expected, filter.groupEstimate(), 1e-4)); + Matrix expected_P_after_predict = + (Matrix(12, 12) << 0.110001, -0, 0, -0.0001, 0, -0, 0, 0, 0, 0, 0, 0, // + -0, 0.110001, -0, 0, -0.0001, -0, 0, 0, 0, 0, 0, 0, // + 0, -0, 0.110001, 0, 0, -0.0001, 0, 0, 0, 0, 0, 0, // + -0.0001, 0, 0, 0.02, 0, -0, 0, 0, 0, 0, 0, 0, // + -0, -0.0001, 0, 0, 0.02, 0, 0, 0, 0, 0, 0, 0, // + -0, -0, -0.0001, -0, 0, 0.02, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 1, 0, -0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, -0, 0, 1, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, -0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, -0, 0, 0.1) + .finished(); + EXPECT(assert_equal(expected_P_after_predict, filter.covariance(), 1e-4)); + + // Perform an update step + const int cal_idx = 0; + const Matrix3 R = 0.01 * I_3x3; + OutputAction phi_y(y, d, cal_idx); + filter.update(phi_y, R); + + // Regression + Group expected_after_update({Rot3(0.995195, -0.097908, -0.000400003, // + 0.097908, 0.995195, 2.98008e-08, // + 0.000398078, -3.91931e-05, 1), + Point3(0.00201816, -0.000882995, 8.60911e-05)}, + {Rot3(0.548024, -0.836459, -0.00263326, // + 0.83646, 0.548012, 0.00413976, // + -0.00201968, -0.0044713, 0.999988), + Rot3(0.995195, -0.097908, -0.000399281, // + 0.097908, 0.995195, 2.40155e-05, // + 0.000395012, -6.2993e-05, 1)}); + EXPECT(assert_equal(expected_after_update, filter.groupEstimate(), 1e-4)); + Matrix expected_P_after_update = + (Matrix(12, 12) << // + 0.0991972, + -0, 0, -9.01785e-05, 0, -0, -0.0982151, 0, 0, 0, 0, 0, // + -0, 0.110001, -0, 0, -0.0001, -0, 0, 0, 0, 0, 0, 0, // + 0, -0, 0.0991972, 0, 0, -0.0001, 0, 0, -0.0982151, 0, 0, 0, // + -0.0001, 0, 0, 0.02, 0, -0, 0, 0, 0, 0, 0, 0, // + -0, -0.0001, 0, 0, 0.02, 0, 0, 0, 0, 0, 0, 0, // + -0, -0, -0.0001, -0, 0, 0.02, 0, 0, 0, 0, 0, 0, // + -0.0982151, 0, 0, 0, 0, 0, 0.107144, 0, -0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, // + 0, 0, -0.0982151, 0, 0, 0, -0, 0, 0.107144, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, -0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, -0, 0, 0.1) + .finished(); + EXPECT(assert_equal(expected_P_after_update, filter.covariance(), 1e-4)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/tests/testLie.cpp b/tests/testLie.cpp deleted file mode 100644 index f4d5cde606..0000000000 --- a/tests/testLie.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------1------------------------------------------- */ - -/** - * @file testLie.cpp - * @date May, 2015 - * @author Frank Dellaert - * @brief unit tests for Lie group type machinery - */ - -#include - -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; - -static const double tol = 1e-9; - -//****************************************************************************** -typedef ProductLieGroup Product; - -// Define any direct product group to be a model of the multiplicative Group concept -namespace gtsam { -template<> struct traits : internal::LieGroupTraits { - static void Print(const Product& m, const string& s = "") { - cout << s << "(" << m.first << "," << m.second.translation() << "/" - << m.second.theta() << ")" << endl; - } - static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { - return traits::Equals(m1.first, m2.first, tol) && m1.second.equals(m2.second, tol); - } -}; -} - -//****************************************************************************** -TEST(Lie, ProductLieGroup) { - GTSAM_CONCEPT_ASSERT(IsGroup); - GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); - Product pair1; - Vector5 d; - d << 1, 2, 0.1, 0.2, 0.3; - Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); - Product pair2 = pair1.expmap(d); - EXPECT(assert_equal(expected, pair2, 1e-9)); - EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9)); - const auto adj = pair1.AdjointMap(); - EXPECT_LONGS_EQUAL(5, adj.rows()); - EXPECT_LONGS_EQUAL(5, adj.cols()); -} - -/* ************************************************************************* */ -Product compose_proxy(const Product& A, const Product& B) { - return A.compose(B); -} -TEST( testProduct, compose ) { - Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; - - Matrix actH1, actH2; - state1.compose(state2, actH1, actH2); - Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); - Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); - EXPECT(assert_equal(numericH1, actH1, tol)); - EXPECT(assert_equal(numericH2, actH2, tol)); -} - -/* ************************************************************************* */ -Product between_proxy(const Product& A, const Product& B) { - return A.between(B); -} -TEST( testProduct, between ) { - Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; - - Matrix actH1, actH2; - state1.between(state2, actH1, actH2); - Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); - Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); - EXPECT(assert_equal(numericH1, actH1, tol)); - EXPECT(assert_equal(numericH2, actH2, tol)); -} - -/* ************************************************************************* */ -Product inverse_proxy(const Product& A) { - return A.inverse(); -} -TEST( testProduct, inverse ) { - Product state1(Point2(1, 2), Pose2(3, 4, 5)); - - Matrix actH1; - state1.inverse(actH1); - Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); - EXPECT(assert_equal(numericH1, actH1, tol)); -} - -/* ************************************************************************* */ -Product expmap_proxy(const Vector5& vec) { - return Product::Expmap(vec); -} -TEST( testProduct, Expmap ) { - Vector5 vec; - vec << 1, 2, 0.1, 0.2, 0.3; - - Matrix actH; - Product::Expmap(vec, actH); - Matrix numericH = numericalDerivative11(expmap_proxy, vec); - EXPECT(assert_equal(numericH, actH, tol)); -} - -/* ************************************************************************* */ -Vector5 logmap_proxy(const Product& p) { - return Product::Logmap(p); -} -TEST( testProduct, Logmap ) { - Product state(Point2(1, 2), Pose2(3, 4, 5)); - - Matrix actH; - Product::Logmap(state, actH); - Matrix numericH = numericalDerivative11(logmap_proxy, state); - EXPECT(assert_equal(numericH, actH, tol)); -} - -/* ************************************************************************* */ -Product interpolate_proxy(const Product& x, const Product& y, double t) { - return interpolate(x, y, t); -} - -TEST(Lie, Interpolate) { - Product x(Point2(1, 2), Pose2(3, 4, 5)); - Product y(Point2(6, 7), Pose2(8, 9, 0)); - - double t; - Matrix actH1, numericH1, actH2, numericH2, actH3, numericH3; - - t = 0.0; - interpolate(x, y, t, actH1, actH2, actH3); - numericH1 = numericalDerivative31( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH1, actH1, tol)); - numericH2 = numericalDerivative32( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH2, actH2, tol)); - numericH3 = numericalDerivative33( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH3, actH3, tol)); - - t = 0.5; - interpolate(x, y, t, actH1, actH2); - numericH1 = numericalDerivative31( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH1, actH1, tol)); - numericH2 = numericalDerivative32( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH2, actH2, tol)); - numericH3 = numericalDerivative33( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH3, actH3, tol)); - - t = 1.0; - interpolate(x, y, t, actH1, actH2); - numericH1 = numericalDerivative31( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH1, actH1, tol)); - numericH2 = numericalDerivative32( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH2, actH2, tol)); - numericH3 = numericalDerivative33( - interpolate_proxy, x, y, t); - EXPECT(assert_equal(numericH3, actH3, tol)); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - diff --git a/tests/testProductLieGroup.cpp b/tests/testProductLieGroup.cpp new file mode 100644 index 0000000000..63a3e762df --- /dev/null +++ b/tests/testProductLieGroup.cpp @@ -0,0 +1,321 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- + */ + +/** + * @file testLie.cpp + * @date May, 2015 + * @author Frank Dellaert + * @brief unit tests for Lie group type machinery + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; + +constexpr double kTol = 1e-9; + +using Product = ProductLieGroup; +constexpr size_t kPowerComponents = 2; +using Power = PowerLieGroup; +using PowerTangent = Power::TangentVector; + +namespace gtsam { +template <> +struct traits : internal::LieGroupTraits { + static void Print(const Product& m, const std::string& s = "") { + std::cout << s << "(" << m.first << "," << m.second.translation() << "/" + << m.second.theta() << ")" << std::endl; + } + static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { + return traits::Equals(m1.first, m2.first, tol) && + m1.second.equals(m2.second, tol); + } +}; + +template <> +struct traits : internal::LieGroupTraits { + static void Print(const Power& m, const std::string& s = "") { + std::cout << s << "["; + for (size_t i = 0; i < kPowerComponents; ++i) { + if (i > 0) std::cout << ", "; + std::cout << "Pose2(" << m[i].x() << "," << m[i].y() << "," + << m[i].theta() << ")"; + } + std::cout << "]" << std::endl; + } + static bool Equals(const Power& m1, const Power& m2, double tol = 1e-8) { + for (size_t i = 0; i < kPowerComponents; ++i) { + if (!m1[i].equals(m2[i], tol)) return false; + } + return true; + } +}; +} // namespace gtsam + +namespace { +Product composeProductProxy(const Product& A, const Product& B) { + return A.compose(B); +} + +Product betweenProductProxy(const Product& A, const Product& B) { + return A.between(B); +} + +Product inverseProductProxy(const Product& A) { return A.inverse(); } + +Product expmapProductProxy(const Vector5& vec) { return Product::Expmap(vec); } + +Vector5 logmapProductProxy(const Product& p) { return Product::Logmap(p); } + +Power composePowerProxy(const Power& A, const Power& B) { return A.compose(B); } + +Power betweenPowerProxy(const Power& A, const Power& B) { return A.between(B); } + +Power inversePowerProxy(const Power& A) { return A.inverse(); } + +Power powerExpmapProxy(const PowerTangent& vec) { return Power::Expmap(vec); } + +PowerTangent powerLogmapProxy(const Power& p) { return Power::Logmap(p); } +} // namespace + +/* ************************************************************************* */ +TEST(Lie, ProductLieGroup) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + Product pair1; + Vector5 d; + d << 1, 2, 0.1, 0.2, 0.3; + Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); + Product pair2 = pair1.expmap(d); + EXPECT(assert_equal(expected, pair2, kTol)); + EXPECT(assert_equal(d, pair1.logmap(pair2), kTol)); + const auto adj = pair1.AdjointMap(); + EXPECT_LONGS_EQUAL(5, adj.rows()); + EXPECT_LONGS_EQUAL(5, adj.cols()); +} + +/* ************************************************************************* */ +TEST(testProduct, compose) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(composeProductProxy, state1, state2); + Matrix numericH2 = numericalDerivative22(composeProductProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProduct, between) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(betweenProductProxy, state1, state2); + Matrix numericH2 = numericalDerivative22(betweenProductProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testProduct, inverse) { + Product state1(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverseProductProxy, state1); + EXPECT(assert_equal(numericH1, actH1, kTol)); +} + +/* ************************************************************************* */ +TEST(testProduct, Expmap) { + Vector5 vec; + vec << 1, 2, 0.1, 0.2, 0.3; + + Matrix actH; + Product::Expmap(vec, actH); + Matrix numericH = numericalDerivative11(expmapProductProxy, vec); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProduct, Logmap) { + Product state(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH; + Product::Logmap(state, actH); + Matrix numericH = numericalDerivative11(logmapProductProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testProduct, AdjointMap) { + Product state(Point2(1, 2), Pose2(3, 4, 5)); + const Matrix actual = state.AdjointMap(); + + Matrix expected = Matrix::Zero(5, 5); + expected.topLeftCorner<2, 2>() = Matrix2::Identity(); + expected.bottomRightCorner<3, 3>() = state.second.AdjointMap(); + + EXPECT(assert_equal(expected, actual, kTol)); +} + +/* ************************************************************************* */ +Product interpolate_proxy(const Product& x, const Product& y, double t) { + return interpolate(x, y, t); +} + +TEST(Lie, Interpolate) { + Product x(Point2(1, 2), Pose2(3, 4, 5)); + Product y(Point2(6, 7), Pose2(8, 9, 0)); + + double t; + Matrix actH1, numericH1, actH2, numericH2, actH3, numericH3; + + t = 0.0; + interpolate(x, y, t, actH1, actH2, actH3); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, kTol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, kTol)); + numericH3 = numericalDerivative33( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH3, actH3, kTol)); + + t = 0.5; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, kTol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, kTol)); + numericH3 = numericalDerivative33( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH3, actH3, kTol)); + + t = 1.0; + interpolate(x, y, t, actH1, actH2); + numericH1 = numericalDerivative31( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH1, actH1, kTol)); + numericH2 = numericalDerivative32( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH2, actH2, kTol)); + numericH3 = numericalDerivative33( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH3, actH3, kTol)); +} + +/* ************************************************************************* */ +TEST(Lie, PowerLieGroup) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + + Power identity; + PowerTangent xi; + xi << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + Power expected({Pose2::Expmap(xi.head<3>()), Pose2::Expmap(xi.tail<3>())}); + + Power actual = identity.expmap(xi); + EXPECT(assert_equal(expected, actual, kTol)); + EXPECT(assert_equal(xi, identity.logmap(actual), kTol)); + const auto adj = identity.AdjointMap(); + EXPECT_LONGS_EQUAL(6, adj.rows()); + EXPECT_LONGS_EQUAL(6, adj.cols()); +} + +/* ************************************************************************* */ +TEST(testPower, compose) { + Power state1({Pose2(1, 2, 3), Pose2(4, 5, 6)}), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(composePowerProxy, state1, state2); + Matrix numericH2 = numericalDerivative22(composePowerProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testPower, between) { + Power state1({Pose2(1, 2, 3), Pose2(4, 5, 6)}), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(betweenPowerProxy, state1, state2); + Matrix numericH2 = numericalDerivative22(betweenPowerProxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, kTol)); + EXPECT(assert_equal(numericH2, actH2, kTol)); +} + +/* ************************************************************************* */ +TEST(testPower, inverse) { + Power state1({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inversePowerProxy, state1); + EXPECT(assert_equal(numericH1, actH1, kTol)); +} + +/* ************************************************************************* */ +TEST(testPower, Expmap) { + PowerTangent vec; + vec << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + + Matrix actH; + Power::Expmap(vec, actH); + Matrix numericH = numericalDerivative11(powerExpmapProxy, vec); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testPower, Logmap) { + Power state({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + + Matrix actH; + Power::Logmap(state, actH); + Matrix numericH = numericalDerivative11(powerLogmapProxy, state); + EXPECT(assert_equal(numericH, actH, kTol)); +} + +/* ************************************************************************* */ +TEST(testPower, AdjointMap) { + Power state({Pose2(1, 2, 3), Pose2(4, 5, 6)}); + const Matrix actual = state.AdjointMap(); + + Matrix expected = Matrix::Zero(6, 6); + expected.block<3, 3>(0, 0) = state[0].AdjointMap(); + expected.block<3, 3>(3, 3) = state[1].AdjointMap(); + + EXPECT(assert_equal(expected, actual, kTol)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//******************************************************************************