From 673336545bb212f516f1c50d1baa7980e3e8512c Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Wed, 12 Mar 2025 21:32:31 -0700 Subject: [PATCH 01/38] Implement ABC_EQF in cpp - Inputs are simulated- Use with caution --- examples/ABC_EQF/CMakeLists.txt | 13 ++ examples/ABC_EQF/Data.h | 41 +++++ examples/ABC_EQF/Direction.cpp | 13 ++ examples/ABC_EQF/Direction.h | 27 ++++ examples/ABC_EQF/EqF.cpp | 258 ++++++++++++++++++++++++++++++ examples/ABC_EQF/EqF.h | 153 ++++++++++++++++++ examples/ABC_EQF/G.cpp | 61 +++++++ examples/ABC_EQF/G.h | 63 ++++++++ examples/ABC_EQF/Input.cpp | 29 ++++ examples/ABC_EQF/Input.h | 41 +++++ examples/ABC_EQF/Measurements.cpp | 17 ++ examples/ABC_EQF/Measurements.h | 36 +++++ examples/ABC_EQF/State.cpp | 12 ++ examples/ABC_EQF/State.h | 29 ++++ examples/ABC_EQF/main.cpp | 195 ++++++++++++++++++++++ examples/ABC_EQF/utilities.cpp | 87 ++++++++++ examples/ABC_EQF/utilities.h | 30 ++++ examples/CMakeLists.txt | 1 + 18 files changed, 1106 insertions(+) create mode 100644 examples/ABC_EQF/CMakeLists.txt create mode 100644 examples/ABC_EQF/Data.h create mode 100644 examples/ABC_EQF/Direction.cpp create mode 100644 examples/ABC_EQF/Direction.h create mode 100644 examples/ABC_EQF/EqF.cpp create mode 100644 examples/ABC_EQF/EqF.h create mode 100644 examples/ABC_EQF/G.cpp create mode 100644 examples/ABC_EQF/G.h create mode 100644 examples/ABC_EQF/Input.cpp create mode 100644 examples/ABC_EQF/Input.h create mode 100644 examples/ABC_EQF/Measurements.cpp create mode 100644 examples/ABC_EQF/Measurements.h create mode 100644 examples/ABC_EQF/State.cpp create mode 100644 examples/ABC_EQF/State.h create mode 100644 examples/ABC_EQF/main.cpp create mode 100644 examples/ABC_EQF/utilities.cpp create mode 100644 examples/ABC_EQF/utilities.h diff --git a/examples/ABC_EQF/CMakeLists.txt b/examples/ABC_EQF/CMakeLists.txt new file mode 100644 index 000000000..f1be48997 --- /dev/null +++ b/examples/ABC_EQF/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(ABC_EqF + main.cpp + EqF.cpp + State.cpp + Input.cpp + G.cpp + Direction.cpp + Measurements.cpp + utilities.cpp +) + +target_link_libraries(ABC_EqF gtsam) + diff --git a/examples/ABC_EQF/Data.h b/examples/ABC_EQF/Data.h new file mode 100644 index 000000000..2e6005572 --- /dev/null +++ b/examples/ABC_EQF/Data.h @@ -0,0 +1,41 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef DATA_H +#define DATA_H +//#pragma once + +#include "State.h" +#include "Input.h" +#include "Measurements.h" +#include + +/** + * Data structure for ground-truth, input and output data + */ +struct Data { + State xi; // Ground-truth state + int n_cal; // Number of calibration states + Input u; // Input measurements + std::vector y; // Output measurements + int n_meas; // Number of measurements + double t; // Time + double dt; // Time step + + /** + * Initialize Data + * @param xi Ground-truth state + * @param n_cal Number of calibration states + * @param u Input measurements + * @param y Output measurements + * @param n_meas Number of measurements + * @param t Time + * @param dt Time step + */ + Data(const State& xi, int n_cal, const Input& u, + const std::vector& y, int n_meas, + double t, double dt) + : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} +}; +#endif //DATA_H diff --git a/examples/ABC_EQF/Direction.cpp b/examples/ABC_EQF/Direction.cpp new file mode 100644 index 000000000..751324b0b --- /dev/null +++ b/examples/ABC_EQF/Direction.cpp @@ -0,0 +1,13 @@ +// +// Created by darshan on 3/11/25. +// + +#include "Direction.h" +#include "utilities.h" +#include + +Direction::Direction(const Vector3& d_vec) : d(d_vec) { + if (!checkNorm(d_vec)) { + throw std::invalid_argument("Direction must be a unit vector"); + } +} \ No newline at end of file diff --git a/examples/ABC_EQF/Direction.h b/examples/ABC_EQF/Direction.h new file mode 100644 index 000000000..633dd122d --- /dev/null +++ b/examples/ABC_EQF/Direction.h @@ -0,0 +1,27 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef DIRECTION_H +#define DIRECTION_H +//#pragma once + +#include +#include + +using namespace gtsam; + +/** + * Direction class as a S2 element + */ +class Direction { +public: + Unit3 d; // Direction (unit vector on S2) + + /** + * Initialize direction + * @param d_vec Direction vector (must be unit norm) + */ + Direction(const Vector3& d_vec); +}; +#endif //DIRECTION_H diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp new file mode 100644 index 000000000..b53eedf32 --- /dev/null +++ b/examples/ABC_EQF/EqF.cpp @@ -0,0 +1,258 @@ +// +// Created by darshan on 3/11/25. +// +#include "EqF.h" +#include "utilities.h" +#include +#include +#include + +// Implementation of helper functions + +Vector lift(const State& xi, const Input& u) { + int n = xi.S.size(); + 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 (int i = 0; i < n; i++) { + L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>(); + } + + return L; +} + +State stateAction(const G& X, const State& xi) { + if (xi.S.size() != X.B.size()) { + throw std::invalid_argument("Number of calibration states and B elements must match"); + } + + std::vector new_S; + for (size_t i = 0; i < X.B.size(); i++) { + new_S.push_back(X.A.inverse() * xi.S[i] * X.B[i]); + } + + return State(xi.R * X.A, + X.A.inverse().matrix() * (xi.b - vee(X.a)), + new_S); +} + +Input velocityAction(const G& X, const Input& u) { + return Input(X.A.inverse().matrix() * (u.w - vee(X.a)), u.Sigma); +} + +Vector3 outputAction(const G& X, const Direction& y, int idx) { + if (idx == -1) { + return X.A.inverse().matrix() * y.d.unitVector(); + } else { + if (idx >= static_cast(X.B.size())) { + throw std::out_of_range("Calibration index out of range"); + } + return X.B[idx].inverse().matrix() * y.d.unitVector(); + } +} + +Vector local_coords(const State& e) { + if (COORDINATE == "EXPONENTIAL") { + Vector eps(6 + 3 * e.S.size()); + + // First 3 elements + eps.head<3>() = Rot3::Logmap(e.R); + + // Next 3 elements + eps.segment<3>(3) = e.b; + + // Remaining elements + for (size_t i = 0; i < e.S.size(); i++) { + eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); + } + + return eps; + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +State local_coords_inv(const Vector& eps) { + G X = G::exp(eps); + + if (COORDINATE == "EXPONENTIAL") { + std::vector S = X.B; + return State(X.A, eps.segment<3>(3), S); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +Matrix stateActionDiff(const State& xi) { + std::function coordsAction = + [&xi](const Vector& U) { + return local_coords(stateAction(G::exp(U), xi)); + }; + + Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); + Matrix differential = numericalDifferential(coordsAction, zeros); + return differential; +} + +// EqF class implementation + +EqF::EqF(const Matrix& Sigma, int n, int m) + : __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)), + __Sigma(Sigma), __xi_0(State::identity(n)) { + + 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(); +} + +State EqF::stateEstimate() const { + return stateAction(__X_hat, __xi_0); +} + +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 * Matrix3::Identity(), __n_cal)); + 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; +} + +void EqF::update(const Measurement& y) { + if (y.cal_idx > __n_cal) { + throw std::invalid_argument("Calibration index out of range"); + } + + Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); + + Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); + Vector3 delta_vec = wedge(y.d.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; +} + +Matrix EqF::__stateMatrixA(const Input& u) const { + Matrix3 W0 = velocityAction(__X_hat.inv(), u).W(); + Matrix A1 = Matrix::Zero(6, 6); + + if (COORDINATE == "EXPONENTIAL") { + A1.block<3, 3>(0, 3) = -Matrix3::Identity(); + A1.block<3, 3>(3, 3) = W0; + Matrix A2 = repBlock(W0, __n_cal); + return blockDiag(A1, A2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +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 * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); + Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; + + if (COORDINATE == "EXPONENTIAL") { + Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + Matrix Phi2 = repBlock(Phi22, __n_cal); + return blockDiag(Phi1, Phi2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +Matrix EqF::__inputMatrixBt() const { + if (COORDINATE == "EXPONENTIAL") { + Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix()); + Matrix B2; + + for (const auto& B : __X_hat.B) { + if (B2.size() == 0) { + B2 = B.matrix(); + } else { + B2 = blockDiag(B2, B.matrix()); + } + } + + return blockDiag(B1, B2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { + Matrix Cc = Matrix::Zero(3, 3 * __n_cal); + + // If the measurement is related to a sensor that has a calibration state + if (idx >= 0) { + Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); + } + + Matrix3 wedge_d = wedge(d.d.unitVector()); + Matrix result(3, __dof); + result.block<3, 3>(0, 0) = wedge_d; + result.block<3, 3>(0, 3) = Matrix3::Zero(); + result.block(0, 6, 3, 3 * __n_cal) = Cc; + + return result; +} + +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(__X_hat.B.size())) { + throw std::out_of_range("Calibration index out of range"); + } + return __X_hat.B[idx].matrix(); + } else { + return __X_hat.A.matrix(); + } +} \ No newline at end of file diff --git a/examples/ABC_EQF/EqF.h b/examples/ABC_EQF/EqF.h new file mode 100644 index 000000000..d09099ac1 --- /dev/null +++ b/examples/ABC_EQF/EqF.h @@ -0,0 +1,153 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef EQF_H +#define EQF_H +#pragma once + +#include "State.h" +#include "Input.h" +#include "G.h" +#include "Direction.h" +#include "Measurements.h" +#include + +using namespace gtsam; + +/** + * Equivariant Filter (EqF) implementation + */ +class EqF { +private: + int __dof; // Degrees of freedom + int __n_cal; // Number of calibration states + int __n_sensor; // Number of sensors + 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 + +public: + /** + * Initialize EqF + * @param Sigma Initial covariance + * @param n Number of calibration states + * @param m Number of sensors + */ + EqF(const Matrix& Sigma, int n, 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); + +private: + /** + * 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 Direction& d, int idx) const; + + /** + * Return the measurement output matrix Dt + * @param idx Calibration index + * @return Measurement output matrix Dt + */ + Matrix __outputMatrixDt(int idx) const; +}; + +// Function declarations for helper functions used by EqF + +/** + * Compute the lift of the system (Theorem 3.8, Equation 7) + * @param xi State + * @param u Input + * @return Lift vector + */ +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 + */ +State stateAction(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 + */ +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 + */ +Vector3 outputAction(const G& X, const Direction& y, int idx = -1); + +/** + * Local coordinates assuming xi_0 = identity (Equation 9) + * @param e State representing equivariant error + * @return Local coordinates + */ +Vector local_coords(const State& e); + +/** + * Local coordinates inverse assuming xi_0 = identity + * @param eps Local coordinates + * @return Corresponding state + */ +State local_coords_inv(const Vector& eps); + +/** + * Differential of the phi action at E = Id in local coordinates + * @param xi State + * @return Differential matrix + */ +Matrix stateActionDiff(const State& xi); +#endif //EQF_H diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp new file mode 100644 index 000000000..1fb765c4f --- /dev/null +++ b/examples/ABC_EQF/G.cpp @@ -0,0 +1,61 @@ +// +// Created by darshan on 3/11/25. +// +#include "G.h" +#include "utilities.h" +#include + +G::G(const Rot3& A, const Matrix3& a, const std::vector& B) + : A(A), a(a), B(B) {} + +G G::operator*(const G& other) const { + if (B.size() != other.B.size()) { + throw std::invalid_argument("Group elements must have the same number of calibration elements"); + } + + std::vector new_B; + for (size_t i = 0; i < B.size(); i++) { + new_B.push_back(B[i] * other.B[i]); + } + + return G(A * other.A, + a + wedge(A.matrix() * vee(other.a)), + new_B); +} + +G G::inv() const { + Matrix3 A_inv = A.inverse().matrix(); + + std::vector B_inv; + for (const auto& b : B) { + B_inv.push_back(b.inverse()); + } + + return G(A.inverse(), + -wedge(A_inv * vee(a)), + B_inv); +} + +G G::identity(int n) { + std::vector B(n, Rot3::Identity()); + return G(Rot3::Identity(), Matrix3::Zero(), B); +} + +G G::exp(const Vector& x) { + if (x.size() < 6 || x.size() % 3 != 0) { + throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); + } + + int n = (x.size() - 6) / 3; + Rot3 A = Rot3::Expmap(x.head<3>()); + + Vector3 a_vee = Rot3LeftJacobian(x.head<3>()) * x.segment<3>(3); + Matrix3 a = wedge(a_vee); + + std::vector B; + for (int i = 0; i < n; i++) { + B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); + } + + return G(A, a, B); +} \ No newline at end of file diff --git a/examples/ABC_EQF/G.h b/examples/ABC_EQF/G.h new file mode 100644 index 000000000..397303ac4 --- /dev/null +++ b/examples/ABC_EQF/G.h @@ -0,0 +1,63 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef G_H +#define G_H + + +#include +#include +#include +#include + +using namespace gtsam; + +/** + * 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 + */ +class G { +public: + Rot3 A; // First SO(3) element + Matrix3 a; // so(3) element (skew-symmetric matrix) + std::vector B; // List of SO(3) elements for calibration + + /** + * Initialize the symmetry group G + * @param A SO3 element + * @param a so(3) element (skew symmetric matrix) + * @param B list of SO3 elements + */ + G(const Rot3& A = Rot3::Identity(), + const Matrix3& a = Matrix3::Zero(), + const std::vector& B = std::vector()); + + /** + * Define the group operation (multiplication) + * @param other Another group element + * @return The product of this and other + */ + G operator*(const G& other) const; + + /** + * Return the inverse element of the symmetry group + * @return The inverse of this group element + */ + G inv() const; + + /** + * Return the identity of the symmetry group + * @param n Number of calibration elements + * @return The identity element with n calibration components + */ + static G identity(int n); + + /** + * Return a group element X given by X = exp(x) + * @param x Vector representation of Lie algebra element + * @return Group element given by the exponential of x + */ + static G exp(const Vector& x); +}; +#endif //G_H diff --git a/examples/ABC_EQF/Input.cpp b/examples/ABC_EQF/Input.cpp new file mode 100644 index 000000000..4e55a4ed0 --- /dev/null +++ b/examples/ABC_EQF/Input.cpp @@ -0,0 +1,29 @@ +// +// Created by darshan on 3/11/25. +// +#include "Input.h" +#include "utilities.h" +#include +#include + +Input::Input(const Vector3& w, const Matrix& Sigma) + : w(w), Sigma(Sigma) { + if (Sigma.rows() != 6 || Sigma.cols() != 6) { + throw std::invalid_argument("Input measurement noise covariance must be 6x6"); + } + + // 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"); + } +} + +Matrix3 Input::W() const { + return wedge(w); +} + +Input Input::random() { + Vector3 w = Vector3::Random(); + return Input(w, Matrix::Identity(6, 6)); +} \ No newline at end of file diff --git a/examples/ABC_EQF/Input.h b/examples/ABC_EQF/Input.h new file mode 100644 index 000000000..437d63b14 --- /dev/null +++ b/examples/ABC_EQF/Input.h @@ -0,0 +1,41 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef INPUT_H +#define INPUT_H + +#include +#include + +using namespace gtsam; + +/** + * Input class for the Biased Attitude System + */ +class Input { +public: + Vector3 w; // Angular velocity + Matrix Sigma; // Noise covariance + + /** + * Initialize Input + * @param w Angular velocity (3-vector) + * @param Sigma Noise covariance (6x6 matrix) + */ + Input(const Vector3& w, const Matrix& Sigma); + + /** + * Return the Input as a skew-symmetric matrix + * @return w as a skew-symmetric matrix + */ + Matrix3 W() const; + + /** + * Return a random angular velocity + * @return A random angular velocity as Input element + */ + static Input random(); +}; + +#endif //INPUT_H diff --git a/examples/ABC_EQF/Measurements.cpp b/examples/ABC_EQF/Measurements.cpp new file mode 100644 index 000000000..f1bd987ab --- /dev/null +++ b/examples/ABC_EQF/Measurements.cpp @@ -0,0 +1,17 @@ +// +// Created by darshan on 3/11/25. +// +#include "Measurements.h" +#include +#include + +Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i) + : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { + + // 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"); + } +} \ No newline at end of file diff --git a/examples/ABC_EQF/Measurements.h b/examples/ABC_EQF/Measurements.h new file mode 100644 index 000000000..b74cc538c --- /dev/null +++ b/examples/ABC_EQF/Measurements.h @@ -0,0 +1,36 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef MEASUREMENTS_H +#define MEASUREMENTS_H + + +#include "Direction.h" +#include + +using namespace gtsam; + +/** + * Measurement class + * cal_idx is an index corresponding to the calibration related to the measurement + * cal_idx = -1 indicates the measurement is from a calibrated sensor + */ +class Measurement { +public: + Direction y; // Measurement direction in sensor frame + Direction d; // Known direction in global frame + Matrix3 Sigma; // Covariance matrix of the measurement + int cal_idx = -1; // Calibration index (-1 for calibrated sensor) + + /** + * Initialize measurement + * @param y_vec Direction measurement in sensor frame + * @param d_vec Known direction in global frame + * @param Sigma Measurement noise covariance + * @param i Calibration index (-1 for calibrated sensor) + */ + Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i = -1); +}; +#endif //MEASUREMENTS_H diff --git a/examples/ABC_EQF/State.cpp b/examples/ABC_EQF/State.cpp new file mode 100644 index 000000000..90dabfbda --- /dev/null +++ b/examples/ABC_EQF/State.cpp @@ -0,0 +1,12 @@ +// +// Created by darshan on 3/11/25. +// +#include "State.h" + +State::State(const Rot3& R, const Vector3& b, const std::vector& S) + : R(R), b(b), S(S) {} + +State State::identity(int n) { + std::vector calibrations(n, Rot3::Identity()); + return State(Rot3::Identity(), Vector3::Zero(), calibrations); +} \ No newline at end of file diff --git a/examples/ABC_EQF/State.h b/examples/ABC_EQF/State.h new file mode 100644 index 000000000..6c8b9c5a6 --- /dev/null +++ b/examples/ABC_EQF/State.h @@ -0,0 +1,29 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef STATE_H +#define STATE_H + +#include +#include +#include + +using namespace gtsam; + +/** + * State class representing the state of the Biased Attitude System + */ +class State { +public: + Rot3 R; // Attitude rotation matrix R + Vector3 b; // Gyroscope bias b + std::vector S; // Sensor calibrations S + + State(const Rot3& R = Rot3::Identity(), + const Vector3& b = Vector3::Zero(), + const std::vector& S = std::vector()); + + static State identity(int n); +}; +#endif //STATE_H diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp new file mode 100644 index 000000000..540bbfc01 --- /dev/null +++ b/examples/ABC_EQF/main.cpp @@ -0,0 +1,195 @@ +// +// Created by darshan on 3/11/25. +// +#include "EqF.h" +#include "State.h" +#include "Input.h" +#include "Direction.h" +#include "Measurements.h" +#include "Data.h" +#include "utilities.h" +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Simplified data loading function - in a real application, implement proper CSV parsing +std::vector loadSimulatedData() { + std::vector data_list; + + double t = 0.0; + double dt = 0.01; + + // Number of data points + int num_points = 100; + + // Set up one calibration state + int n_cal = 1; + + for (int i = 0; i < num_points; i++) { + t += dt; + + // Create a simple sinusoidal trajectory + double angle = 0.1 * sin(t); + Rot3 R = Rot3::Rz(angle); + + // Create a bias + Vector3 b(0.01, 0.02, 0.03); + + // Create a calibration + std::vector S; + S.push_back(Rot3::Ry(0.05)); + + // State + State xi(R, b, S); + + // Input (angular velocity) + Vector3 w(0.1 * cos(t), 0.05 * sin(t), 0.02); + Matrix Sigma_u = Matrix::Identity(6, 6) * 0.01; + Input u(w, Sigma_u); + + // Measurements + std::vector measurements; + + // Measurement 1 - from uncalibrated sensor + Vector3 d1_vec = Vector3(1, 0, 0).normalized(); // Known direction in global frame + Vector3 y1_vec = S[0].inverse().matrix() * R.inverse().matrix() * d1_vec; // Direction in sensor frame + Matrix3 Sigma1 = Matrix3::Identity() * 0.01; + measurements.push_back(Measurement(y1_vec, d1_vec, Sigma1, 0)); // cal_idx = 0 + + // Measurement 2 - from calibrated sensor + Vector3 d2_vec = Vector3(0, 1, 0).normalized(); // Known direction in global frame + Vector3 y2_vec = R.inverse().matrix() * d2_vec; // Direction in sensor frame + Matrix3 Sigma2 = Matrix3::Identity() * 0.01; + measurements.push_back(Measurement(y2_vec, d2_vec, Sigma2, -1)); // cal_idx = -1 + + // Add to data list + data_list.push_back(Data(xi, n_cal, u, measurements, 2, t, dt)); + } + + return data_list; +} + +void runSimulation(EqF& filter, const std::vector& data) { + std::cout << "Starting simulation with " << data.size() << " data points..." << std::endl; + + // Track time for performance measurement + auto start = std::chrono::high_resolution_clock::now(); + + // Store results for analysis + std::vector times; + std::vector attitude_errors; + std::vector bias_errors; + std::vector calibration_errors; + + for (const auto& d : data) { + // Propagation + try { + filter.propagation(d.u, d.dt); + } catch (const std::exception& e) { + std::cerr << "Propagation error at t=" << d.t << ": " << e.what() << std::endl; + continue; + } + + // Update with measurements + for (const auto& y : d.y) { + try { + if (!std::isnan(y.y.d.unitVector().norm()) && !std::isnan(y.d.d.unitVector().norm())) { + filter.update(y); + } + } catch (const std::exception& e) { + std::cerr << "Update error at t=" << d.t << ": " << e.what() << std::endl; + } + } + + // Get state estimate + State estimate = filter.stateEstimate(); + + // Compute errors + Vector3 att_error = Rot3::Logmap(d.xi.R.between(estimate.R)); + Vector3 bias_error = estimate.b - d.xi.b; + Vector3 cal_error = Vector3::Zero(); + if (!d.xi.S.empty() && !estimate.S.empty()) { + cal_error = Rot3::Logmap(d.xi.S[0].between(estimate.S[0])); + } + + // Store results + times.push_back(d.t); + attitude_errors.push_back(att_error); + bias_errors.push_back(bias_error); + calibration_errors.push_back(cal_error); + + // Print some info + if (d.t < 0.1 || fmod(d.t, 1.0) < d.dt) { + std::cout << "Time: " << d.t + << ", Attitude error (deg): " << (att_error.norm() * 180.0/M_PI) + << ", Bias error: " << bias_error.norm() + << ", Calibration error (deg): " << (cal_error.norm() * 180.0/M_PI) + << std::endl; + } + } + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = end - start; + std::cout << "Simulation completed in " << elapsed.count() << " seconds" << std::endl; + + // Print summary statistics + double avg_att_error = 0.0; + double avg_bias_error = 0.0; + double avg_cal_error = 0.0; + + for (size_t i = 0; i < times.size(); i++) { + avg_att_error += attitude_errors[i].norm(); + avg_bias_error += bias_errors[i].norm(); + avg_cal_error += calibration_errors[i].norm(); + } + + avg_att_error /= times.size(); + avg_bias_error /= times.size(); + avg_cal_error /= times.size(); + + std::cout << "Average attitude error (deg): " << (avg_att_error * 180.0/M_PI) << std::endl; + std::cout << "Average bias error: " << avg_bias_error << std::endl; + std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl; +} + +int main(int argc, char** argv) { + std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; + std::cout << "========================================================" << std::endl; + + // Initialize filter + int n_cal = 1; // Number of calibration states + int n_sensors = 2; // Number of sensors + + // Initial covariance - larger values for higher uncertainty + Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); + initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty + initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty + initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty + + std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl; + + try { + // Create filter + EqF filter(initialSigma, n_cal, n_sensors); + + // Generate simulated data + std::cout << "Generating simulated data..." << std::endl; + std::vector data = loadSimulatedData(); + + // Run simulation + runSimulation(filter, data); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return 1; + } + + std::cout << "Done." << std::endl; + return 0; +} \ No newline at end of file diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp new file mode 100644 index 000000000..74f4a97c9 --- /dev/null +++ b/examples/ABC_EQF/utilities.cpp @@ -0,0 +1,87 @@ +// +// Created by darshan on 3/11/25. +// +#include "utilities.h" +#include + +// Global configuration +const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" + +Matrix3 wedge(const Vector3& vec) { + Matrix3 mat; + mat << 0.0, -vec(2), vec(1), + vec(2), 0.0, -vec(0), + -vec(1), vec(0), 0.0; + return mat; +} + +Vector3 vee(const Matrix3& mat) { + Vector3 vec; + vec << mat(2, 1), mat(0, 2), mat(1, 0); + return vec; +} + +Matrix3 Rot3LeftJacobian(const Vector3& arr) { + double angle = arr.norm(); + + // Near |phi|==0, use first order Taylor expansion + if (angle < 1e-10) { + return Matrix3::Identity() + 0.5 * wedge(arr); + } + + Vector3 axis = arr / angle; + double s = sin(angle); + double c = cos(angle); + + return (s / angle) * Matrix3::Identity() + + (1 - (s / angle)) * (axis * axis.transpose()) + + ((1 - c) / angle) * wedge(axis); +} + +bool checkNorm(const Vector3& x, double tol) { + return abs(x.norm() - 1) < tol || std::isnan(x.norm()); +} + +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; + } +} + +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; +} + +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; +} \ No newline at end of file diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h new file mode 100644 index 000000000..f19b06c07 --- /dev/null +++ b/examples/ABC_EQF/utilities.h @@ -0,0 +1,30 @@ +// +// Created by darshan on 3/11/25. +// + +#ifndef UTILITIES_H +#define UTILITIES_H + +#pragma once + +#include +#include +#include +#include + +using namespace gtsam; + +// Global configuration +extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL" + +/** + * Utility functions + */ +Matrix3 wedge(const Vector3& vec); +Vector3 vee(const Matrix3& mat); +Matrix3 Rot3LeftJacobian(const Vector3& arr); +bool checkNorm(const Vector3& x, double tol = 1e-3); +Matrix blockDiag(const Matrix& A, const Matrix& B); +Matrix repBlock(const Matrix& A, int n); +Matrix numericalDifferential(std::function f, const Vector& x); +#endif //UTILITIES_H diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8da95722c..581535c9f 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,3 +17,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES) endif() gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) +add_subdirectory(ABC_EQF) \ No newline at end of file From d9cd90589c476cad53ee36aad02514d075282d00 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Wed, 19 Mar 2025 21:04:32 -0700 Subject: [PATCH 02/38] c++ implementation - use with caution --- examples/ABC_EQF/CMakeLists.txt | 1 + examples/ABC_EQF/Direction.h | 16 +++++-- examples/ABC_EQF/EqF.cpp | 73 +++++++++++++++++++++---------- examples/ABC_EQF/main.cpp | 76 ++++++++++++++++++++++----------- 4 files changed, 115 insertions(+), 51 deletions(-) diff --git a/examples/ABC_EQF/CMakeLists.txt b/examples/ABC_EQF/CMakeLists.txt index f1be48997..23eb309d2 100644 --- a/examples/ABC_EQF/CMakeLists.txt +++ b/examples/ABC_EQF/CMakeLists.txt @@ -7,6 +7,7 @@ add_executable(ABC_EqF Direction.cpp Measurements.cpp utilities.cpp + runEQF_withcsv.h ) target_link_libraries(ABC_EqF gtsam) diff --git a/examples/ABC_EQF/Direction.h b/examples/ABC_EQF/Direction.h index 633dd122d..041b90bb6 100644 --- a/examples/ABC_EQF/Direction.h +++ b/examples/ABC_EQF/Direction.h @@ -5,23 +5,31 @@ #ifndef DIRECTION_H #define DIRECTION_H //#pragma once - #include #include - using namespace gtsam; - /** * Direction class as a S2 element */ class Direction { public: Unit3 d; // Direction (unit vector on S2) - + /** * Initialize direction * @param d_vec Direction vector (must be unit norm) */ Direction(const Vector3& d_vec); + + // Accessor methods for vector components + double x() const { return d.unitVector()[0]; } + double y() const { return d.unitVector()[1]; } + double z() const { return d.unitVector()[2]; } + + // Check if the direction contains NaN values + bool hasNaN() const { + Vector3 vec = d.unitVector(); + return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); + } }; #endif //DIRECTION_H diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp index b53eedf32..418d9ac08 100644 --- a/examples/ABC_EQF/EqF.cpp +++ b/examples/ABC_EQF/EqF.cpp @@ -152,23 +152,43 @@ void EqF::propagation(const Input& u, double dt) { __Sigma = Phi_DT * __Sigma * Phi_DT.transpose() + M_DT; } +bool hasNaN(const Vector3& vec) { + return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); +} + void EqF::update(const Measurement& y) { - if (y.cal_idx > __n_cal) { - throw std::invalid_argument("Calibration index out of range"); + if (y.cal_idx > __n_cal) { + throw std::invalid_argument("Calibration index out of range"); + } + + // Get vector representations for checking + Vector3 y_vec = y.y.d.unitVector(); + Vector3 d_vec = y.d.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 } + static int update_count = 0; + if (update_count < 5) { + std::cout << "Update " << update_count << ":\n"; + std::cout << "y_vec: " << y_vec.transpose() << "\n"; + std::cout << "d_vec: " << d_vec.transpose() << "\n"; + update_count++; + } - Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); - Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); - Vector3 delta_vec = wedge(y.d.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; + Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); + Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); + Vector3 delta_vec = wedge(y.d.d.unitVector()) * action_result; // Ensure this is the right operation + 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; } Matrix EqF::__stateMatrixA(const Input& u) const { @@ -229,20 +249,27 @@ Matrix EqF::__inputMatrixBt() const { } Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * __n_cal); + Matrix Cc = Matrix::Zero(3, 3 * __n_cal); - // If the measurement is related to a sensor that has a calibration state - if (idx >= 0) { - Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); - } + // If the measurement is related to a sensor that has a calibration state + if (idx >= 0) { + // Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); // WRONG + // Set the correct 3x3 block in Cc + Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); + } - Matrix3 wedge_d = wedge(d.d.unitVector()); - Matrix result(3, __dof); - result.block<3, 3>(0, 0) = wedge_d; - result.block<3, 3>(0, 3) = Matrix3::Zero(); - result.block(0, 6, 3, 3 * __n_cal) = Cc; + Matrix3 wedge_d = wedge(d.d.unitVector()); - return result; + // This Matrix concatenation was different from the Python version + // Create the equivalent of: + // Rot3.Hat(d.d.unitVector()) @ np.hstack((Rot3.Hat(d.d.unitVector()), np.zeros((3, 3)), Cc)) + + Matrix temp(3, 6 + 3 * __n_cal); + temp.block<3, 3>(0, 0) = wedge_d; + temp.block<3, 3>(0, 3) = Matrix3::Zero(); + temp.block(0, 6, 3, 3 * __n_cal) = Cc; + + return wedge_d * temp; } Matrix EqF::__outputMatrixDt(int idx) const { diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp index 540bbfc01..c11d3a179 100644 --- a/examples/ABC_EQF/main.cpp +++ b/examples/ABC_EQF/main.cpp @@ -7,6 +7,7 @@ #include "Direction.h" #include "Measurements.h" #include "Data.h" +#include "runEQF_withcsv.h" #include "utilities.h" #include #include @@ -158,38 +159,65 @@ void runSimulation(EqF& filter, const std::vector& data) { std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl; } +// int main(int argc, char** argv) { +// std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; +// std::cout << "========================================================" << std::endl; +// +// // Initialize filter +// int n_cal = 1; // Number of calibration states +// int n_sensors = 2; // Number of sensors +// +// // Initial covariance - larger values for higher uncertainty +// Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); +// initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty +// initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty +// initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty +// +// std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl; +// +// try { +// // Create filter +// EqF filter(initialSigma, n_cal, n_sensors); +// +// // Generate simulated data +// std::cout << "Generating simulated data..." << std::endl; +// std::vector data = loadSimulatedData(); +// +// // Run simulation +// runSimulation(filter, data); +// +// } catch (const std::exception& e) { +// std::cerr << "Error: " << e.what() << std::endl; +// return 1; +// } +// +// std::cout << "Done." << std::endl; +// return 0; int main(int argc, char** argv) { std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; std::cout << "========================================================" << std::endl; - - // Initialize filter - int n_cal = 1; // Number of calibration states - int n_sensors = 2; // Number of sensors - - // Initial covariance - larger values for higher uncertainty - Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); - initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty - initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty - initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty - - std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl; - + + std::string csvFilePath; + + // Check if CSV file path is provided as command line argument + if (argc > 1) { + csvFilePath = argv[1]; + } else { + std::cout << "Please enter the path to your CSV data file: "; + std::cin >> csvFilePath; + } + + std::cout << "Using CSV data from: " << csvFilePath << std::endl; + try { - // Create filter - EqF filter(initialSigma, n_cal, n_sensors); - - // Generate simulated data - std::cout << "Generating simulated data..." << std::endl; - std::vector data = loadSimulatedData(); - - // Run simulation - runSimulation(filter, data); - + // Run with CSV data + runEqFWithCSVData(csvFilePath); } catch (const std::exception& e) { std::cerr << "Error: " << e.what() << std::endl; return 1; } - + std::cout << "Done." << std::endl; return 0; + } \ No newline at end of file From 9aefd92998ee0bc529ec818ad0e15815989800e6 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Mon, 24 Mar 2025 20:47:34 -0700 Subject: [PATCH 03/38] Adding a function to translate output data into csv and store in local directory --- examples/ABC_EQF/runEQF_withcsv.h | 683 ++++++++++++++++++++++++++++++ 1 file changed, 683 insertions(+) create mode 100644 examples/ABC_EQF/runEQF_withcsv.h diff --git a/examples/ABC_EQF/runEQF_withcsv.h b/examples/ABC_EQF/runEQF_withcsv.h new file mode 100644 index 000000000..d31baffe0 --- /dev/null +++ b/examples/ABC_EQF/runEQF_withcsv.h @@ -0,0 +1,683 @@ +// +// Created by darshan on 3/17/25. +// + +#ifndef RUNEQF_WITHCSV_H +#define RUNEQF_WITHCSV_H + +// +// Created by darshan on 3/17/25. +// +#include "Data.h" +#include "State.h" +#include "Input.h" +#include "Direction.h" +#include "Measurements.h" +#include "utilities.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Load data from CSV file into a vector of Data objects for the EqF + * + * CSV format: + * - t: Time + * - q_w, q_x, q_y, q_z: True attitude quaternion + * - b_x, b_y, b_z: True bias + * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion + * - w_x, w_y, w_z: Angular velocity measurements + * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations + * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations + * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements + * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations + * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions + * + * @param filename Path to the CSV file + * @param startRow First row to load (default: 0) + * @param maxRows Maximum number of rows to load (default: all) + * @param downsample Downsample factor (default: 1, which means no downsampling) + * @return Vector of Data objects + */ +inline std::vector loadDataFromCSV(const std::string& filename, + int startRow = 0, + int maxRows = -1, + int downsample = 1) { + std::vector data_list; + std::ifstream file(filename); + + if (!file.is_open()) { + throw std::runtime_error("Failed to open file: " + filename); + } + + std::string line; + int lineNumber = 0; + int rowCount = 0; + double prevTime = 0.0; + + // Skip header + std::getline(file, line); + lineNumber++; + + // Skip to startRow + while (lineNumber < startRow && std::getline(file, line)) { + lineNumber++; + } + + // Read data + while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { + lineNumber++; + + // Apply downsampling + if ((lineNumber - startRow - 1) % downsample != 0) { + continue; + } + + std::istringstream ss(line); + std::string token; + std::vector values; + + // Parse line into values + while (std::getline(ss, token, ',')) { + try { + values.push_back(std::stod(token)); + } catch (const std::exception& e) { + std::cerr << "Error parsing value at line " << lineNumber << ": " << token << std::endl; + values.push_back(0.0); // Use default value + } + } + + // Check if we have enough values + if (values.size() < 39) { + std::cerr << "Warning: Line " << lineNumber << " has only " << values.size() + << " values, expected 39. Skipping." << std::endl; + continue; + } + + // Extract values + double t = values[0]; + double dt = (rowCount == 0) ? 0.0 : t - prevTime; + prevTime = t; + + // Create ground truth state + Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z + Rot3 R = Rot3(quat); + + Vector3 b(values[5], values[6], values[7]); + + Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z + std::vector S = {Rot3(calQuat)}; + + State xi(R, b, S); + + // Create input + Vector3 w(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); + + // Create measurements + std::vector measurements; + + // First measurement (calibrated sensor, cal_idx = 0) + Vector3 y0(values[21], values[22], values[23]); + Vector3 d0(values[33], values[34], values[35]); + + // Normalize vectors if needed + if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); + if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); + + // Measurement covariance + Matrix3 covY0 = Matrix3::Zero(); + covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 + covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 + covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 + + // Create measurement + measurements.push_back(Measurement(y0, d0, covY0, 0)); + + // Second measurement (calibrated sensor, cal_idx = -1) + Vector3 y1(values[24], values[25], values[26]); + Vector3 d1(values[36], values[37], values[38]); + + // Normalize vectors if needed + if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); + if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); + + // Measurement covariance + Matrix3 covY1 = Matrix3::Zero(); + covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 + covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 + covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 + + // Create measurement + measurements.push_back(Measurement(y1, d1, covY1, -1)); + + // Create Data object and add to list + data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); + + rowCount++; + } + + std::cout << "Loaded " << data_list.size() << " data points from CSV file." << std::endl; + + return data_list; +} + +/** + * Process Data objects with the EqF filter + * + * @param filter EqF filter to use + * @param data_list Vector of Data objects + * @param saveResults Whether to save results to a file + * @param resultFilename Filename to save results to + */ +inline void printDataPoint(const Data& data, int index) { + std::cout << "Data[" << index << "] @ t=" << data.t << ", dt=" << data.dt << std::endl; + + // Print angular velocity + std::cout << " ω = [" << data.u.w[0] << ", " << data.u.w[1] << ", " << data.u.w[2] << "]" << std::endl; + + // Print measurements + for (size_t i = 0; i < data.y.size(); i++) { + const Measurement& meas = data.y[i]; + // Use the unitVector() method to get a Vector3 from a Unit3 object + Vector3 y_vec = meas.y.d.unitVector(); + Vector3 d_vec = meas.d.d.unitVector(); + std::cout << " y" << i << " = [" << y_vec[0] << ", " << y_vec[1] << ", " << y_vec[2] << "]" << std::endl; + std::cout << " d" << i << " = [" << d_vec[0] << ", " << d_vec[1] << ", " << d_vec[2] << "]" << std::endl; + } + + std::cout << std::endl; +} + + +// Function to print sample data points +inline void printDataSamples(const std::vector& data_list, int count = 3) { + int total = data_list.size(); + + std::cout << "\n=== First " << count << " Data Points ===" << std::endl; + for (int i = 0; i < std::min(count, total); i++) { + printDataPoint(data_list[i], i); + } + + if (total > 2*count) { + std::cout << "\n... (" << (total - 2*count) << " points omitted) ...\n" << std::endl; + + std::cout << "=== Last " << count << " Data Points ===" << std::endl; + for (int i = std::max(count, total - count); i < total; i++) { + printDataPoint(data_list[i], i); + } + } +} + +// Function to validate data +inline bool validateData(const std::vector& data_list) { + if (data_list.empty()) { + std::cerr << "ERROR: No data loaded from CSV" << std::endl; + return false; + } + + std::cout << "Validating " << data_list.size() << " data points..." << std::endl; + + // Track statistics + int invalid_count = 0; + + // Open a log file to record detailed issues + std::ofstream logFile("data_validation.log"); + logFile << "Data Validation Report" << std::endl; + logFile << "--------------------" << std::endl; + + for (size_t i = 0; i < data_list.size(); ++i) { + const Data& data = data_list[i]; + bool point_valid = true; + + // Check time and dt + if (std::isnan(data.t) || std::isnan(data.dt)) { + logFile << "Point " << i << ": Invalid time values (t=" << data.t + << ", dt=" << data.dt << ")" << std::endl; + point_valid = false; + } + + // Check ground truth state for NaN - using isnan directly on components + const auto& R_matrix = data.xi.R.matrix(); + bool R_has_nan = false; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + if (std::isnan(R_matrix(r, c))) { + R_has_nan = true; + break; + } + } + } + + if (R_has_nan) { + logFile << "Point " << i << ": NaN in ground truth attitude matrix" << std::endl; + point_valid = false; + } + + // Check bias vector for NaN + bool b_has_nan = false; + for (int j = 0; j < 3; j++) { + if (std::isnan(data.xi.b[j])) { + b_has_nan = true; + break; + } + } + + if (b_has_nan) { + logFile << "Point " << i << ": NaN in ground truth bias vector" << std::endl; + point_valid = false; + } + + // Check calibration matrices for NaN + for (size_t j = 0; j < data.xi.S.size(); ++j) { + const auto& S_matrix = data.xi.S[j].matrix(); + bool S_has_nan = false; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + if (std::isnan(S_matrix(r, c))) { + S_has_nan = true; + break; + } + } + } + + if (S_has_nan) { + logFile << "Point " << i << ": NaN in ground truth calibration matrix " + << j << std::endl; + point_valid = false; + } + } + + // Check input for NaN + bool w_has_nan = false; + for (int j = 0; j < 3; j++) { + if (std::isnan(data.u.w[j])) { + w_has_nan = true; + break; + } + } + + if (w_has_nan) { + logFile << "Point " << i << ": NaN in angular velocity" << std::endl; + point_valid = false; + } + + // Check measurements + for (size_t j = 0; j < data.y.size(); ++j) { + const Measurement& meas = data.y[j]; + + // Get the Vector3 representations to check them + Vector3 y_vec = meas.y.d.unitVector(); + Vector3 d_vec = meas.d.d.unitVector(); + + // Check measurement vector for NaN + bool y_has_nan = false; + bool d_has_nan = false; + + for (int k = 0; k < 3; k++) { + if (std::isnan(y_vec[k])) { + y_has_nan = true; + break; + } + if (std::isnan(d_vec[k])) { + d_has_nan = true; + break; + } + } + + if (y_has_nan) { + logFile << "Point " << i << ", Meas " << j << ": NaN in measurement vector" << std::endl; + point_valid = false; + } + + if (d_has_nan) { + logFile << "Point " << i << ", Meas " << j << ": NaN in reference direction" << std::endl; + point_valid = false; + } + + // Calculate norm using Vector3 norms + double y_norm = y_vec.norm(); + double d_norm = d_vec.norm(); + + if (std::abs(y_norm - 1.0) > 1e-5) { + logFile << "Point " << i << ", Meas " << j + << ": Measurement vector not normalized. Norm = " << y_norm << std::endl; + point_valid = false; + } + + if (std::abs(d_norm - 1.0) > 1e-5) { + logFile << "Point " << i << ", Meas " << j + << ": Reference direction not normalized. Norm = " << d_norm << std::endl; + point_valid = false; + } + } + + if (!point_valid) { + invalid_count++; + + // Print first few invalid points to console + if (invalid_count <= 5) { + std::cerr << "Invalid data at point " << i << " (t=" << data.t << ")" << std::endl; + } + } + } + + // Close the log + logFile << std::endl << "Summary: " << invalid_count << " invalid data points out of " + << data_list.size() << std::endl; + logFile.close(); + + + + + + + + + + // Print summary + std::cout << "Data validation complete. " << invalid_count << " invalid points found." << std::endl; + if (invalid_count > 0) { + std::cout << "See data_validation.log for details." << std::endl; + } + + return (invalid_count == 0); +} + +inline void processDataWithEqF(EqF& filter, + const std::vector& data_list, + bool saveResults = false, + const std::string& resultFilename = "eqf_results.csv") { + std::ofstream resultFile; + if (saveResults) { + resultFile.open(resultFilename); + if (!resultFile.is_open()) { + throw std::runtime_error("Failed to open result file: " + resultFilename); + } + + // Write header - now adding roll, pitch, yaw columns for estimated and true values + resultFile << "t,"; + // Estimated state quaternion + resultFile << "est_qw,est_qx,est_qy,est_qz,"; + // Estimated bias + resultFile << "est_bx,est_by,est_bz,"; + // Estimated calibration quaternion + resultFile << "est_cqw,est_cqx,est_cqy,est_cqz,"; + // True state quaternion + resultFile << "true_qw,true_qx,true_qy,true_qz,"; + // True bias + resultFile << "true_bx,true_by,true_bz,"; + // True calibration quaternion + resultFile << "true_cqw,true_cqx,true_cqy,true_cqz,"; + // Add Euler angles for estimated state + resultFile << "est_roll,est_pitch,est_yaw,"; + // Add Euler angles for true state + resultFile << "true_roll,true_pitch,true_yaw,"; + // Add Euler angles for estimated calibration + resultFile << "est_cal_roll,est_cal_pitch,est_cal_yaw,"; + // Add Euler angles for true calibration + resultFile << "true_cal_roll,true_cal_pitch,true_cal_yaw"; + resultFile << std::endl; + } + + std::cout << "Processing data with EqF..." << std::endl; + + // Track time for performance measurement + auto start = std::chrono::high_resolution_clock::now(); + + // Store error metrics + std::vector att_errors; + std::vector bias_errors; + std::vector cal_errors; + + int total_measurements = 0; + int valid_measurements = 0; + int invalid_measurements = 0; + + for (size_t i = 0; i < data_list.size(); i++) { + const Data& data = data_list[i]; + + // Propagation + filter.propagation(data.u, data.dt); + + // Update with measurements + for (const auto& y : data.y) { + total_measurements++; + + // Check for NaN values in measurement vectors + bool has_nan = false; + Vector3 y_vec = y.y.d.unitVector(); + Vector3 d_vec = y.d.d.unitVector(); + + for (int j = 0; j < 3; j++) { + if (std::isnan(y_vec[j]) || std::isnan(d_vec[j])) { + has_nan = true; + break; + } + } + + if (!has_nan) { + try { + filter.update(y); + valid_measurements++; + } catch (const std::exception& e) { + std::cerr << "Error updating at t=" << data.t << ": " << e.what() << std::endl; + invalid_measurements++; + } + } else { + invalid_measurements++; + } + } + + // Get state estimate + State estimate = filter.stateEstimate(); + + // Compute 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(); + if (!data.xi.S.empty() && !estimate.S.empty()) { + cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); + } + + // Store errors + att_errors.push_back(att_error.norm()); + bias_errors.push_back(bias_error.norm()); + cal_errors.push_back(cal_error.norm()); + + // Save results + if (saveResults) { + // Extract quaternions + Quaternion est_q = estimate.R.toQuaternion(); + Quaternion true_q = data.xi.R.toQuaternion(); + + // Extract Euler angles (roll, pitch, yaw) from estimated rotation + Vector3 est_rpy = estimate.R.rpy(); + // Convert to degrees for easier comparison + Vector3 est_rpy_deg = est_rpy * 180.0 / M_PI; + + // Extract Euler angles from true rotation + Vector3 true_rpy = data.xi.R.rpy(); + // Convert to degrees + Vector3 true_rpy_deg = true_rpy * 180.0 / M_PI; + + // Get calibration quaternions and Euler angles + Quaternion est_cal_q, true_cal_q; + Vector3 est_cal_rpy_deg = Vector3::Zero(); + Vector3 true_cal_rpy_deg = Vector3::Zero(); + + if (!estimate.S.empty() && !data.xi.S.empty()) { + est_cal_q = estimate.S[0].toQuaternion(); + true_cal_q = data.xi.S[0].toQuaternion(); + + // Get Euler angles for calibrations + Vector3 est_cal_rpy = estimate.S[0].rpy(); + est_cal_rpy_deg = est_cal_rpy * 180.0 / M_PI; + + Vector3 true_cal_rpy = data.xi.S[0].rpy(); + true_cal_rpy_deg = true_cal_rpy * 180.0 / M_PI; + } else { + est_cal_q = Quaternion(1, 0, 0, 0); // Identity quaternion + true_cal_q = Quaternion(1, 0, 0, 0); + } + + // Write to file + resultFile << data.t << ","; + // Estimated quaternion + resultFile << est_q.w() << "," << est_q.x() << "," << est_q.y() << "," << est_q.z() << ","; + // Estimated bias + resultFile << estimate.b[0] << "," << estimate.b[1] << "," << estimate.b[2] << ","; + // Estimated calibration quaternion + resultFile << est_cal_q.w() << "," << est_cal_q.x() << "," << est_cal_q.y() << "," << est_cal_q.z() << ","; + // True quaternion + resultFile << true_q.w() << "," << true_q.x() << "," << true_q.y() << "," << true_q.z() << ","; + // True bias + resultFile << data.xi.b[0] << "," << data.xi.b[1] << "," << data.xi.b[2] << ","; + // True calibration quaternion + resultFile << true_cal_q.w() << "," << true_cal_q.x() << "," << true_cal_q.y() << "," << true_cal_q.z() << ","; + + // Add Euler angles (in degrees) for estimated state + resultFile << est_rpy_deg[0] << "," << est_rpy_deg[1] << "," << est_rpy_deg[2] << ","; + // Add Euler angles (in degrees) for true state + resultFile << true_rpy_deg[0] << "," << true_rpy_deg[1] << "," << true_rpy_deg[2] << ","; + // Add Euler angles (in degrees) for estimated calibration + resultFile << est_cal_rpy_deg[0] << "," << est_cal_rpy_deg[1] << "," << est_cal_rpy_deg[2] << ","; + // Add Euler angles (in degrees) for true calibration + resultFile << true_cal_rpy_deg[0] << "," << true_cal_rpy_deg[1] << "," << true_cal_rpy_deg[2]; + + resultFile << std::endl; + } + + // Print progress + if (i % 1000 == 0 || i == data_list.size() - 1) { + std::cout << "Processed " << i+1 << "/" << data_list.size() + << " (" << (100.0 * (i+1) / data_list.size()) << "%) "; + std::cout << "Attitude error: " << (att_error.norm() * 180.0/M_PI) << " deg, "; + std::cout << "Bias error: " << bias_error.norm() << ", "; + std::cout << "Calibration error: " << (cal_error.norm() * 180.0/M_PI) << " deg" << std::endl; + } + } + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = end - start; + + // Calculate average errors + double avg_att_error = 0.0; + double avg_bias_error = 0.0; + double avg_cal_error = 0.0; + + if (!att_errors.empty()) { + avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); + avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); + avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); + } + + std::cout << std::endl; + std::cout << "EqF Processing completed in " << elapsed.count() << " seconds" << std::endl; + std::cout << "Average attitude error: " << (avg_att_error * 180.0/M_PI) << " deg" << std::endl; + std::cout << "Average bias error: " << avg_bias_error << std::endl; + std::cout << "Average calibration error: " << (avg_cal_error * 180.0/M_PI) << " deg" << std::endl; + std::cout << "Total measurements: " << total_measurements << std::endl; + std::cout << "Valid measurements processed: " << valid_measurements << std::endl; + std::cout << "Invalid measurements skipped: " << invalid_measurements << std::endl; + + if (saveResults) { + resultFile.close(); + std::cout << "Results saved to " << resultFilename << std::endl; + } +} + + + + + +inline void runEqFWithCSVData(const std::string& filename) { + try { + // Load data from CSV file with optional parameters + int startRow = 0; + int maxRows = -1; + int downsample = 1; + + std::vector data = loadDataFromCSV(filename, startRow, maxRows, downsample); + + if (data.empty()) { + std::cerr << "No data loaded from CSV file." << std::endl; + return; + } + + // Print sample data points to inspect the loaded data + std::cout << "Data loaded, displaying samples..." << std::endl; + printDataSamples(data); + + // Validate the data to check for issues + std::cout << "Validating data integrity..." << std::endl; + bool dataValid = validateData(data); + + if (!dataValid) { + std::cout << "Warning: Data validation found issues." << std::endl; + std::string proceed; + std::cout << "Do you want to proceed anyway? (y/n): "; + std::cin >> proceed; + if (proceed != "y" && proceed != "Y") { + std::cout << "Operation cancelled by user." << std::endl; + return; + } + } + + // Initialize EqF filter + int n_cal = 1; // Number of calibration states (from the data) + int n_sensors = 2; // Number of sensors (from the data) + + // Initial covariance + Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); + initialSigma.diagonal().head<3>() = Vector3::Constant(0.1); // Reduced attitude uncertainty + initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.01); // Reduced bias uncertainty + initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Reduced calibration uncertainty + + // Create filter + EqF filter(initialSigma, n_cal, n_sensors); + + // Initialize filter state from the first ground truth if possible + if (!data.empty()) { + // You'll need to add a method to your EqF class to set the initial state + // Something like: + // filter.setInitialState(data[0].xi); + + // If you don't have such a method, you can print the first ground truth + // to see if it makes sense + std::cout << "First ground truth state:" << std::endl; + std::cout << "Attitude: " << data[0].xi.R.matrix() << std::endl; + std::cout << "Bias: " << data[0].xi.b.transpose() << std::endl; + std::cout << "Calibration: " << data[0].xi.S[0].matrix() << std::endl; + } + + // Process data with the filter and save results + processDataWithEqF(filter, data, true, "eqf_results.csv"); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + } +} +/** + * Example usage function to demonstrate how to use the data loader with the EqF + */ + +#endif //RUNEQF_WITHCSV_H From d1673b7df93fc0b6b40b17331f882aa91a87e3d3 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Wed, 26 Mar 2025 21:24:26 -0700 Subject: [PATCH 04/38] Used GTSAM ExpmapDerivative to calculate Left Jacobian(-1) --- examples/ABC_EQF/G.cpp | 2 +- examples/ABC_EQF/main.cpp | 57 +++++++++------------------------- examples/ABC_EQF/utilities.cpp | 22 +++---------- examples/ABC_EQF/utilities.h | 2 +- 4 files changed, 21 insertions(+), 62 deletions(-) diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp index 1fb765c4f..6cbdd1e3e 100644 --- a/examples/ABC_EQF/G.cpp +++ b/examples/ABC_EQF/G.cpp @@ -49,7 +49,7 @@ G G::exp(const Vector& x) { int n = (x.size() - 6) / 3; Rot3 A = Rot3::Expmap(x.head<3>()); - Vector3 a_vee = Rot3LeftJacobian(x.head<3>()) * x.segment<3>(3); + Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3); Matrix3 a = wedge(a_vee); std::vector B; diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp index c11d3a179..b21eae44a 100644 --- a/examples/ABC_EQF/main.cpp +++ b/examples/ABC_EQF/main.cpp @@ -15,6 +15,8 @@ #include #include #include +#include + using namespace std; using namespace gtsam; @@ -159,56 +161,26 @@ void runSimulation(EqF& filter, const std::vector& data) { std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl; } -// int main(int argc, char** argv) { -// std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; -// std::cout << "========================================================" << std::endl; -// -// // Initialize filter -// int n_cal = 1; // Number of calibration states -// int n_sensors = 2; // Number of sensors -// -// // Initial covariance - larger values for higher uncertainty -// Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); -// initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty -// initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty -// initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty -// -// std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl; -// -// try { -// // Create filter -// EqF filter(initialSigma, n_cal, n_sensors); -// -// // Generate simulated data -// std::cout << "Generating simulated data..." << std::endl; -// std::vector data = loadSimulatedData(); -// -// // Run simulation -// runSimulation(filter, data); -// -// } catch (const std::exception& e) { -// std::cerr << "Error: " << e.what() << std::endl; -// return 1; -// } -// -// std::cout << "Done." << std::endl; -// return 0; + + + int main(int argc, char** argv) { std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; std::cout << "========================================================" << std::endl; std::string csvFilePath; - // Check if CSV file path is provided as command line argument - if (argc > 1) { - csvFilePath = argv[1]; - } else { - std::cout << "Please enter the path to your CSV data file: "; - std::cin >> csvFilePath; + // Try to find the EQFdata file in the GTSAM examples directory + try { + // Look specifically for EQFdata.csv in GTSAM examples + csvFilePath = findExampleDataFile("EqFdata.csv"); + std::cout << "Using GTSAM example data file: " << csvFilePath << std::endl; + } catch (const std::exception& e) { + std::cerr << "Error: Could not find EqFdata.csv in GTSAM examples directory" << std::endl; + std::cerr << e.what() << std::endl; + return 1; } - std::cout << "Using CSV data from: " << csvFilePath << std::endl; - try { // Run with CSV data runEqFWithCSVData(csvFilePath); @@ -219,5 +191,4 @@ int main(int argc, char** argv) { std::cout << "Done." << std::endl; return 0; - } \ No newline at end of file diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp index 74f4a97c9..e35481ad3 100644 --- a/examples/ABC_EQF/utilities.cpp +++ b/examples/ABC_EQF/utilities.cpp @@ -5,7 +5,7 @@ #include // Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" +const std::string COORDINATE = "EXPONENTIAL"; Matrix3 wedge(const Vector3& vec) { Matrix3 mat; @@ -21,22 +21,10 @@ Vector3 vee(const Matrix3& mat) { return vec; } -Matrix3 Rot3LeftJacobian(const Vector3& arr) { - double angle = arr.norm(); - - // Near |phi|==0, use first order Taylor expansion - if (angle < 1e-10) { - return Matrix3::Identity() + 0.5 * wedge(arr); - } - - Vector3 axis = arr / angle; - double s = sin(angle); - double c = cos(angle); - - return (s / angle) * Matrix3::Identity() + - (1 - (s / angle)) * (axis * axis.transpose()) + - ((1 - c) / angle) * wedge(axis); -} +// Matrix3 Rot3LeftJacobian(const Vector3& arr) { +// return Rot3::ExpmapDerivative(-arr); +// +// } bool checkNorm(const Vector3& x, double tol) { return abs(x.norm() - 1) < tol || std::isnan(x.norm()); diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h index f19b06c07..b3837ad22 100644 --- a/examples/ABC_EQF/utilities.h +++ b/examples/ABC_EQF/utilities.h @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -22,7 +23,6 @@ extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL" */ Matrix3 wedge(const Vector3& vec); Vector3 vee(const Matrix3& mat); -Matrix3 Rot3LeftJacobian(const Vector3& arr); bool checkNorm(const Vector3& x, double tol = 1e-3); Matrix blockDiag(const Matrix& A, const Matrix& B); Matrix repBlock(const Matrix& A, int n); From 39a7a5b62794a02b2590734ee27398c931bb096f Mon Sep 17 00:00:00 2001 From: jenniferoum Date: Wed, 26 Mar 2025 21:41:07 -0700 Subject: [PATCH 05/38] Update wedge and vee to Rot3 Hat and Vee for EqF --- examples/ABC_EQF/EqF.cpp | 10 +++++----- examples/ABC_EQF/G.cpp | 6 +++--- examples/ABC_EQF/Input.cpp | 3 ++- examples/ABC_EQF/utilities.cpp | 20 +------------------- examples/ABC_EQF/utilities.h | 2 -- 5 files changed, 11 insertions(+), 30 deletions(-) diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp index 418d9ac08..32414bb74 100644 --- a/examples/ABC_EQF/EqF.cpp +++ b/examples/ABC_EQF/EqF.cpp @@ -38,12 +38,12 @@ State stateAction(const G& X, const State& xi) { } return State(xi.R * X.A, - X.A.inverse().matrix() * (xi.b - vee(X.a)), + X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)), new_S); } Input velocityAction(const G& X, const Input& u) { - return Input(X.A.inverse().matrix() * (u.w - vee(X.a)), u.Sigma); + return Input(X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma); } Vector3 outputAction(const G& X, const Direction& y, int idx) { @@ -182,7 +182,7 @@ void EqF::update(const Measurement& y) { Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); - Vector3 delta_vec = wedge(y.d.d.unitVector()) * action_result; // Ensure this is the right operation + Vector3 delta_vec = Rot3::Hat(y.d.d.unitVector()) * action_result; // Ensure this is the right operation Matrix Dt = __outputMatrixDt(y.cal_idx); Matrix S = Ct * __Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose(); Matrix K = __Sigma * Ct.transpose() * S.inverse(); @@ -255,10 +255,10 @@ Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { if (idx >= 0) { // Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); // WRONG // Set the correct 3x3 block in Cc - Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); + Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.d.unitVector()); } - Matrix3 wedge_d = wedge(d.d.unitVector()); + Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); // This Matrix concatenation was different from the Python version // Create the equivalent of: diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp index 6cbdd1e3e..1c9684e90 100644 --- a/examples/ABC_EQF/G.cpp +++ b/examples/ABC_EQF/G.cpp @@ -19,7 +19,7 @@ G G::operator*(const G& other) const { } return G(A * other.A, - a + wedge(A.matrix() * vee(other.a)), + a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), new_B); } @@ -32,7 +32,7 @@ G G::inv() const { } return G(A.inverse(), - -wedge(A_inv * vee(a)), + -Rot3::Hat(A_inv * Rot3::Vee(a)), B_inv); } @@ -50,7 +50,7 @@ G G::exp(const Vector& x) { Rot3 A = Rot3::Expmap(x.head<3>()); Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3); - Matrix3 a = wedge(a_vee); + Matrix3 a = Rot3::Hat(a_vee); std::vector B; for (int i = 0; i < n; i++) { diff --git a/examples/ABC_EQF/Input.cpp b/examples/ABC_EQF/Input.cpp index 4e55a4ed0..488102cdb 100644 --- a/examples/ABC_EQF/Input.cpp +++ b/examples/ABC_EQF/Input.cpp @@ -5,6 +5,7 @@ #include "utilities.h" #include #include +#include "gtsam/geometry/Rot3.h" Input::Input(const Vector3& w, const Matrix& Sigma) : w(w), Sigma(Sigma) { @@ -20,7 +21,7 @@ Input::Input(const Vector3& w, const Matrix& Sigma) } Matrix3 Input::W() const { - return wedge(w); + return Rot3::Hat(w); } Input Input::random() { diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp index e35481ad3..5166cb0d6 100644 --- a/examples/ABC_EQF/utilities.cpp +++ b/examples/ABC_EQF/utilities.cpp @@ -5,26 +5,8 @@ #include // Global configuration -const std::string COORDINATE = "EXPONENTIAL"; +const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" -Matrix3 wedge(const Vector3& vec) { - Matrix3 mat; - mat << 0.0, -vec(2), vec(1), - vec(2), 0.0, -vec(0), - -vec(1), vec(0), 0.0; - return mat; -} - -Vector3 vee(const Matrix3& mat) { - Vector3 vec; - vec << mat(2, 1), mat(0, 2), mat(1, 0); - return vec; -} - -// Matrix3 Rot3LeftJacobian(const Vector3& arr) { -// return Rot3::ExpmapDerivative(-arr); -// -// } bool checkNorm(const Vector3& x, double tol) { return abs(x.norm() - 1) < tol || std::isnan(x.norm()); diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h index b3837ad22..50bbf88aa 100644 --- a/examples/ABC_EQF/utilities.h +++ b/examples/ABC_EQF/utilities.h @@ -9,7 +9,6 @@ #include #include -#include #include #include @@ -22,7 +21,6 @@ extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL" * Utility functions */ Matrix3 wedge(const Vector3& vec); -Vector3 vee(const Matrix3& mat); bool checkNorm(const Vector3& x, double tol = 1e-3); Matrix blockDiag(const Matrix& A, const Matrix& B); Matrix repBlock(const Matrix& A, int n); From cd1782e5d49b0ba83cd35c39425ecde535fa9552 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Tue, 15 Apr 2025 21:06:25 -0700 Subject: [PATCH 06/38] C++ implementation of the EqF to estimate bias. --- examples/ABC_EQF.cpp | 1009 +++++++++++++++++++++++++++++ examples/ABC_EQF.h | 427 ++++++++++++ examples/ABC_EQF/CMakeLists.txt | 14 - examples/ABC_EQF/Data.h | 41 -- examples/ABC_EQF/Direction.cpp | 13 - examples/ABC_EQF/Direction.h | 35 - examples/ABC_EQF/EqF.cpp | 285 -------- examples/ABC_EQF/EqF.h | 153 ----- examples/ABC_EQF/G.cpp | 61 -- examples/ABC_EQF/G.h | 63 -- examples/ABC_EQF/Input.cpp | 30 - examples/ABC_EQF/Input.h | 41 -- examples/ABC_EQF/Measurements.cpp | 17 - examples/ABC_EQF/Measurements.h | 36 - examples/ABC_EQF/State.cpp | 12 - examples/ABC_EQF/State.h | 29 - examples/ABC_EQF/main.cpp | 194 ------ examples/ABC_EQF/runEQF_withcsv.h | 683 ------------------- examples/ABC_EQF/utilities.cpp | 57 -- examples/ABC_EQF/utilities.h | 28 - examples/ABC_EQF_Demo.cpp | 89 +++ examples/CMakeLists.txt | 1 - gtsam/CMakeLists.txt | 5 +- 23 files changed, 1529 insertions(+), 1794 deletions(-) create mode 100644 examples/ABC_EQF.cpp create mode 100644 examples/ABC_EQF.h delete mode 100644 examples/ABC_EQF/CMakeLists.txt delete mode 100644 examples/ABC_EQF/Data.h delete mode 100644 examples/ABC_EQF/Direction.cpp delete mode 100644 examples/ABC_EQF/Direction.h delete mode 100644 examples/ABC_EQF/EqF.cpp delete mode 100644 examples/ABC_EQF/EqF.h delete mode 100644 examples/ABC_EQF/G.cpp delete mode 100644 examples/ABC_EQF/G.h delete mode 100644 examples/ABC_EQF/Input.cpp delete mode 100644 examples/ABC_EQF/Input.h delete mode 100644 examples/ABC_EQF/Measurements.cpp delete mode 100644 examples/ABC_EQF/Measurements.h delete mode 100644 examples/ABC_EQF/State.cpp delete mode 100644 examples/ABC_EQF/State.h delete mode 100644 examples/ABC_EQF/main.cpp delete mode 100644 examples/ABC_EQF/runEQF_withcsv.h delete mode 100644 examples/ABC_EQF/utilities.cpp delete mode 100644 examples/ABC_EQF/utilities.h create mode 100644 examples/ABC_EQF_Demo.cpp diff --git a/examples/ABC_EQF.cpp b/examples/ABC_EQF.cpp new file mode 100644 index 000000000..8209e7b29 --- /dev/null +++ b/examples/ABC_EQF.cpp @@ -0,0 +1,1009 @@ +/** + * @file ABC_EQF.cpp + * @brief Implementation of the Attitude-Bias-Calibration Equivariant Filter + * + * This file contains the implementation 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 + */ + +#include "ABC_EQF.h" + +namespace abc_eqf_lib { + +// Global configuration +const std::string COORDINATE = "EXPONENTIAL"; // Denotes how the states are mapped to the vector representations + +//======================================================================== +// Utility Functions +//======================================================================== +/** + * @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 + * Uses Vector3 norm() method to calculate vector magnitude + */ +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 + * Uses Matrix's rows(), cols(), setZero(), and block() methods + */ + +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 + * Recursively uses blockDiag() function + */ +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; +} + +/** + * @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; +} + +//======================================================================== +// Direction Class Implementation +//======================================================================== + +/** + * @brief Initializes a direction object vector from a provided 3D vector input + * @param d_vec A 3-D vector that should have a unit norm(This represents a + * direction in 3D space) Uses Unit3's constructor which normalizes vectors + */ + + +Direction::Direction(const Vector3& d_vec) : d(d_vec) { + if (!checkNorm(d_vec)) { + throw std::invalid_argument("Direction must be a unit vector"); + } +} + /** Access the individual components of the direction vector defined above using this methods below + * Uses the Unit3 object from GTSAM to compute the components + */ + +double Direction::x() const { + return d.unitVector()[0]; +} + +double Direction::y() const { + return d.unitVector()[1]; +} + +double Direction::z() const { + return d.unitVector()[2]; +} + +bool Direction::hasNaN() const { + Vector3 vec = d.unitVector(); + return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); +} + +//======================================================================== +// Input Class Implementation +//======================================================================== +/** + * @brief Constructs an input object using the Angular velocity vector and the + * covariance matrix + * @param w Angular vector + * @param Sigma 6X6 covariance matrix + * Uses Matrix's rows(), cols() and Eigen's SelfAdjointEigenSolver + */ +Input::Input(const Vector3& w, const Matrix& Sigma) : w(w), Sigma(Sigma) { + if (Sigma.rows() != 6 || Sigma.cols() != 6) { + throw std::invalid_argument("Input measurement noise covariance must be 6x6"); + } + + // 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"); + } +} +/** + * + * @return 3X3 skey symmetric matrix when called + * Uses Rot3's Hat() to create skew-symmetric matrix + */ +Matrix3 Input::W() const { + return Rot3::Hat(w); +} + + +//======================================================================== +// Measurement Class Implementation +//======================================================================== +/** + * @brief Constructs measurement with directions and covariance. + * @param y_vec A 3D vector representing the measured direction in the sensor frame + * @param d_vec A 3D vector representing the known reference direction in the global frame aka ground truth direction + * @param Sigma 3x3 positive definite covariance vector representing the uncertainty in the measurements + * @param i Calibration index - A non-negative integer specifies the element in the calibration vector + * that corresponds to the sensor of interest. A value of -1 indicates that all the sensors have been calibrated + * + * Creates a measurement object that stores the measured direction(y), reference direction(d), measurement noise covariance(Sigma) + * and Calibration Index cal_idx + * + * Uses Eigen's SelfAdjointEigenSolver + * + */ + +Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i) + : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { + + // 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"); + } +} + +//======================================================================== +// State Class Implementation +//======================================================================== +/** + * + * @param R Rot3 (Attitude) + * @param b Vector (Bias) + * @param S Vector (Rot 3 calibration states) + * Combines the navigation and the calibration states together and provides a + * mechanism to represent the complete system + * + */ +State::State(const Rot3& R, const Vector3& b, const std::vector& S) + : R(R), b(b), S(S) {} + +/** + * + * @param n Number of Calibration states + * @return State object intitialized to identity + * Creates a default/ initial state + * Uses GTSAM's Rot3::identity and Vector3 zero function + */ +State State::identity(int n) { + std::vector calibrations(n, Rot3::Identity()); + return State(Rot3::Identity(), Vector3::Zero(), calibrations); +} + +//======================================================================== +// Data Struct Implementation +//======================================================================== + +/** + * + * @param xi Ground Truth state + * @param n_cal Number of calibration states + * @param u Input measurements + * @param y Vector of y measurements + * @param n_meas number of measurements + * @param t timestamp + * @param dt time step + * Used to organize the state, input and measurement data with timestamps for + * testing Uses Rot3, Vector 3 and Unit3 classes + */ +Data::Data(const State& xi, int n_cal, const Input& u, + const std::vector& y, int n_meas, + double t, double dt) + : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} + +//======================================================================== +// Symmetry Group Implementation - Group Elements and actions +//======================================================================== +/** + * + * @param A Attitude element of Rot3 type + * @param a Matrix3 bias element + * @param B Rot3 vector containing calibration elements + * Ouptuts a G object using Rot3 for rotation representation + */ +G::G(const Rot3& A, const Matrix3& a, const std::vector& B) + : A(A), a(a), B(B) {} + +/** + * Defines the group operation (multiplication) + * @param other Another Group element + * @return G a product of two group elements + * Uses Rot3 Hat, Rot3 Vee for multiplication + * + */ +G G::operator*(const G& other) const { + if (B.size() != other.B.size()) { + throw std::invalid_argument("Group elements must have the same number of calibration elements"); + } + + std::vector new_B; + for (size_t i = 0; i < B.size(); i++) { + new_B.push_back(B[i] * other.B[i]); + } + + return G(A * other.A, + a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), + new_B); +} + +/** + * Used to compute the Group inverse + * @return The inverse of group element + * Uses Rot3 inverse, Rot3 matrix, hat and vee functions + * The left invariant property of the semi-direct product group structure is implemented here by using the -ve sign + */ +G G::inv() const { + Matrix3 A_inv = A.inverse().matrix(); + + std::vector B_inv; + for (const auto& b : B) { + B_inv.push_back(b.inverse()); + } + + return G(A.inverse(), + -Rot3::Hat(A_inv * Rot3::Vee(a)), + B_inv); +} + +/** + * Creates the identity element of the group + * @param n Number of calibration elements + * @return the identity element + * Uses Rot3 Identity and Matrix zero + */ +G G::identity(int n) { + std::vector B(n, Rot3::Identity()); + return G(Rot3::Identity(), Matrix3::Zero(), B); +} +/** + * Maps the tangent space elements to the group + * @param x Vector in lie algebra + * @return the group element G + * Uses Rot3 expmap and Expmapderivative function + */ +G G::exp(const Vector& x) { + if (x.size() < 6 || x.size() % 3 != 0) { + throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); + } + + int n = (x.size() - 6) / 3; + 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::vector B; + for (int i = 0; i < n; i++) { + B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); + } + + return G(A, a, B); +} + +//======================================================================== +// 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 + */ +Vector lift(const State& xi, const Input& u) { + int n = xi.S.size(); + 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 (int 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 + */ +State stateAction(const G& X, const State& xi) { + if (xi.S.size() != X.B.size()) { + throw std::invalid_argument("Number of calibration states and B elements must match"); + } + + std::vector new_S; + for (size_t i = 0; i < X.B.size(); i++) { + new_S.push_back(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 + */ +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 + */ +Vector3 outputAction(const G& X, const Direction& y, int idx) { + if (idx == -1) { + return X.A.inverse().matrix() * y.d.unitVector(); + } else { + if (idx >= static_cast(X.B.size())) { + throw std::out_of_range("Calibration index out of range"); + } + return X.B[idx].inverse().matrix() * y.d.unitVector(); + } +} + +/** + * Maps the error states to vector representations through exponential + * coordinates + * @param e error state + * @return Vector with local coordinates + * Uses Rot3 logamo for mapping rotations to the tangent space + */ +Vector local_coords(const State& e) { + if (COORDINATE == "EXPONENTIAL") { + Vector eps(6 + 3 * e.S.size()); + + // First 3 elements + eps.head<3>() = Rot3::Logmap(e.R); + + // Next 3 elements + eps.segment<3>(3) = e.b; + + // Remaining elements + for (size_t i = 0; i < e.S.size(); i++) { + eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); + } + + return eps; + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * Used to convert the vectorized errors back to state space + * @param eps Local coordinates in the exponential parameterization + * @return State object corresponding to these coordinates + * Uses Rot3 expmap through the G::exp() function + */ +State local_coords_inv(const Vector& eps) { + G X = G::exp(eps); + + if (COORDINATE == "EXPONENTIAL") { + std::vector S = X.B; + return State(X.A, eps.segment<3>(3), S); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * 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 + */ +Matrix stateActionDiff(const State& xi) { + std::function coordsAction = + [&xi](const Vector& U) { + return local_coords(stateAction(G::exp(U), xi)); + }; + + Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); + 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() + */ +EqF::EqF(const Matrix& Sigma, int n, int m) + : __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)), + __Sigma(Sigma), __xi_0(State::identity(n)) { + + 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 + */ +State EqF::stateEstimate() const { + return stateAction(__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 + */ +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 * Matrix3::Identity(), __n_cal)); + 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 + */ +void EqF::update(const Measurement& y) { + if (y.cal_idx > __n_cal) { + throw std::invalid_argument("Calibration index out of range"); + } + + // Get vector representations for checking + Vector3 y_vec = y.y.d.unitVector(); + Vector3 d_vec = y.d.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.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 + */ +Matrix EqF::__stateMatrixA(const Input& u) const { + Matrix3 W0 = velocityAction(__X_hat.inv(), u).W(); + Matrix A1 = Matrix::Zero(6, 6); + + if (COORDINATE == "EXPONENTIAL") { + A1.block<3, 3>(0, 3) = -Matrix3::Identity(); + A1.block<3, 3>(3, 3) = W0; + Matrix A2 = repBlock(W0, __n_cal); + return blockDiag(A1, A2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +/** + * Computes the discrete time state transition matrix + * @param u Angular velocity + * @param dt time step + * @return State transition matrix in discrete time + */ +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 * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); + Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; + + if (COORDINATE == "EXPONENTIAL") { + Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + Matrix Phi2 = repBlock(Phi22, __n_cal); + return blockDiag(Phi1, Phi2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * Computes the input uncertainty propagation matrix + * @return + * Uses the blockdiag matrix + */ +Matrix EqF::__inputMatrixBt() const { + if (COORDINATE == "EXPONENTIAL") { + Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix()); + Matrix B2; + + for (const auto& B : __X_hat.B) { + if (B2.size() == 0) { + B2 = B.matrix(); + } else { + B2 = blockDiag(B2, B.matrix()); + } + } + + return blockDiag(B1, B2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * 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 + */ +Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { + Matrix Cc = Matrix::Zero(3, 3 * __n_cal); + + // 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.d.unitVector()); + } + + Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); + + // Create the combined matrix + Matrix temp(3, 6 + 3 * __n_cal); + temp.block<3, 3>(0, 0) = wedge_d; + temp.block<3, 3>(0, 3) = Matrix3::Zero(); + temp.block(0, 6, 3, 3 * __n_cal) = 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 + */ +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(__X_hat.B.size())) { + throw std::out_of_range("Calibration index out of range"); + } + return __X_hat.B[idx].matrix(); + } else { + return __X_hat.A.matrix(); + } +} + +//======================================================================== +// Data Processing Functions Implementation +//======================================================================== + +/** + * @brief Loads the test data from the csv file + * @param filename path to the csv file is specified + * @param startRow First row to load based on csv, 0 by default + * @param maxRows maximum rows to load, defaults to all rows + * @param downsample Downsample factor, default 1 + * @return A list of data objects +*/ + + + +std::vector loadDataFromCSV(const std::string& filename, + int startRow, + int maxRows, + int downsample) { + std::vector data_list; + std::ifstream file(filename); + + if (!file.is_open()) { + throw std::runtime_error("Failed to open file: " + filename); + } + + std::cout << "Loading data from " << filename << "..." << std::flush; + + std::string line; + int lineNumber = 0; + int rowCount = 0; + int errorCount = 0; + double prevTime = 0.0; + + // Skip header + std::getline(file, line); + lineNumber++; + + // Skip to startRow + while (lineNumber < startRow && std::getline(file, line)) { + lineNumber++; + } + + // Read data + while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { + lineNumber++; + + // Apply downsampling + if ((lineNumber - startRow - 1) % downsample != 0) { + continue; + } + + std::istringstream ss(line); + std::string token; + std::vector values; + + // Parse line into values + while (std::getline(ss, token, ',')) { + try { + values.push_back(std::stod(token)); + } catch (const std::exception& e) { + errorCount++; + values.push_back(0.0); // Use default value + } + } + + // Check if we have enough values + if (values.size() < 39) { + errorCount++; + continue; + } + + // Extract values + double t = values[0]; + double dt = (rowCount == 0) ? 0.0 : t - prevTime; + prevTime = t; + + // Create ground truth state + Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z + Rot3 R = Rot3(quat); + + Vector3 b(values[5], values[6], values[7]); + + Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z + std::vector S = {Rot3(calQuat)}; + + State xi(R, b, S); + + // Create input + Vector3 w(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); + + // Create measurements + std::vector measurements; + + // First measurement (calibrated sensor, cal_idx = 0) + Vector3 y0(values[21], values[22], values[23]); + Vector3 d0(values[33], values[34], values[35]); + + // Normalize vectors if needed + if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); + if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); + + // Measurement covariance + Matrix3 covY0 = Matrix3::Zero(); + covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 + covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 + covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 + + // Create measurement + measurements.push_back(Measurement(y0, d0, covY0, 0)); + + // Second measurement (calibrated sensor, cal_idx = -1) + Vector3 y1(values[24], values[25], values[26]); + Vector3 d1(values[36], values[37], values[38]); + + // Normalize vectors if needed + if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); + if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); + + // Measurement covariance + Matrix3 covY1 = Matrix3::Zero(); + covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 + covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 + covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 + + // Create measurement + measurements.push_back(Measurement(y1, d1, covY1, -1)); + + // Create Data object and add to list + data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); + + rowCount++; + + // Show loading progress every 1000 rows + if (rowCount % 1000 == 0) { + std::cout << "." << std::flush; + } + } + + std::cout << " Done!" << std::endl; + std::cout << "Loaded " << data_list.size() << " data points"; + + if (errorCount > 0) { + std::cout << " (" << errorCount << " errors encountered)"; + } + + std::cout << std::endl; + + return data_list; +} +/** + * @brief Takes in the data and runs an EqF on it and reports the results + * @param filter Initialized EqF filter + * @param data_list std::vector + * @param printInterval Progress indicator + * Prints the performance statstics like average error etc + * Uses Rot3 between, logmap and rpy functions + */ +void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval) { + if (data_list.empty()) { + std::cerr << "No data to process" << std::endl; + return; + } + + std::cout << "Processing " << data_list.size() << " data points with EqF..." << std::endl; + + // Track performance metrics + std::vector att_errors; + std::vector bias_errors; + std::vector cal_errors; + + // Track time for performance measurement + auto start = std::chrono::high_resolution_clock::now(); + + int totalMeasurements = 0; + int validMeasurements = 0; + + // Define constant for converting radians to degrees + const double RAD_TO_DEG = 180.0 / M_PI; + + // Print a progress indicator + int progressStep = data_list.size() / 10; // 10 progress updates + if (progressStep < 1) progressStep = 1; + + std::cout << "Progress: "; + + for (size_t i = 0; i < data_list.size(); i++) { + const Data& data = data_list[i]; + + // Propagate filter with current input and time step + filter.propagation(data.u, data.dt); + + // Process all measurements + for (const auto& y : data.y) { + totalMeasurements++; + + // Skip invalid measurements + if (y.y.hasNaN() || y.d.hasNaN()) { + continue; + } + + try { + filter.update(y); + validMeasurements++; + } catch (const std::exception& e) { + std::cerr << "Error updating at t=" << data.t + << ": " << e.what() << std::endl; + } + } + + // Get current state estimate + State estimate = filter.stateEstimate(); + + // 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(); + if (!data.xi.S.empty() && !estimate.S.empty()) { + cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); + } + + // Store errors + att_errors.push_back(att_error.norm()); + bias_errors.push_back(bias_error.norm()); + cal_errors.push_back(cal_error.norm()); + + // Show progress dots + if (i % progressStep == 0) { + std::cout << "." << std::flush; + } + } + + std::cout << " Done!" << std::endl; + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = end - start; + + // Calculate average errors + double avg_att_error = 0.0; + double avg_bias_error = 0.0; + double avg_cal_error = 0.0; + + if (!att_errors.empty()) { + avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); + avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); + avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); + } + + // Calculate final errors from last data point + const Data& final_data = data_list.back(); + State final_estimate = filter.stateEstimate(); + 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(); + 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])); + } + + // Print summary statistics + std::cout << "\n=== Filter Performance Summary ===" << std::endl; + std::cout << "Processing time: " << elapsed.count() << " seconds" << std::endl; + std::cout << "Processed measurements: " << totalMeasurements << " (valid: " << validMeasurements << ")" << std::endl; + + // Average errors + std::cout << "\n-- Average Errors --" << std::endl; + std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl; + std::cout << "Bias: " << avg_bias_error << std::endl; + std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°" << std::endl; + + // Final errors + std::cout << "\n-- Final Errors --" << std::endl; + std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°" << std::endl; + std::cout << "Bias: " << final_bias_error.norm() << std::endl; + std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°" << std::endl; + + // Print a brief comparison of final estimate vs ground truth + std::cout << "\n-- Final State vs Ground Truth --" << std::endl; + std::cout << "Attitude (RPY) - Estimate: " + << (final_estimate.R.rpy() * RAD_TO_DEG).transpose() << "° | Truth: " + << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; + std::cout << "Bias - Estimate: " << final_estimate.b.transpose() + << " | Truth: " << final_data.xi.b.transpose() << std::endl; + + if (!final_estimate.S.empty() && !final_data.xi.S.empty()) { + std::cout << "Calibration (RPY) - Estimate: " + << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose() << "° | Truth: " + << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; + } +} + +} // namespace abc_eqf_lib \ No newline at end of file diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h new file mode 100644 index 000000000..6d381b1ee --- /dev/null +++ b/examples/ABC_EQF.h @@ -0,0 +1,427 @@ +/** + * @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 +#include +#include // For std::accumulate + +// All implementations are wrapped in this namespace to avoid conflicts +namespace abc_eqf_lib { + +using namespace std; +using namespace gtsam; + +// Global configuration +// Define coordinate type: "EXPONENTIAL" or "NORMAL" +extern const std::string COORDINATE; + +//======================================================================== +// 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); + +/** + * Calculate numerical differential + */ +Matrix numericalDifferential(std::function f, const Vector& x); + +//======================================================================== +// Core Data Types +//======================================================================== + +/** + * Direction class as a S2 element + */ +class Direction { +public: + Unit3 d; // Direction (unit vector on S2) + + /** + * Initialize direction + * @param d_vec Direction vector (must be unit norm) + */ + Direction(const Vector3& d_vec); + + // Accessor methods for vector components + double x() const; + double y() const; + double z() const; + + // Check if the direction contains NaN values + bool hasNaN() const; +}; + +/** + * Input class for the Biased Attitude System + */ +class Input { +public: + Vector3 w; // Angular velocity + Matrix Sigma; // Noise covariance + + /** + * Initialize Input + * @param w Angular velocity (3-vector) + * @param Sigma Noise covariance (6x6 matrix) + */ + Input(const Vector3& w, const Matrix& Sigma); + + /** + * Return the Input as a skew-symmetric matrix + * @return w as a skew-symmetric matrix + */ + Matrix3 W() const; + + /** + * Return a random angular velocity + * @return A random angular velocity as Input element + */ + static Input random(); +}; + +/** + * Measurement class + * cal_idx is an index corresponding to the calibration related to the measurement + * cal_idx = -1 indicates the measurement is from a calibrated sensor + */ +class Measurement { +public: + Direction y; // Measurement direction in sensor frame + Direction d; // Known direction in global frame + Matrix3 Sigma; // Covariance matrix of the measurement + int cal_idx = -1; // Calibration index (-1 for calibrated sensor) + + /** + * Initialize measurement + * @param y_vec Direction measurement in sensor frame + * @param d_vec Known direction in global frame + * @param Sigma Measurement noise covariance + * @param i Calibration index (-1 for calibrated sensor) + */ + Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i = -1); +}; + +/** + * State class representing the state of the Biased Attitude System + */ +class State { +public: + Rot3 R; // Attitude rotation matrix R + Vector3 b; // Gyroscope bias b + std::vector S; // Sensor calibrations S + + State(const Rot3& R = Rot3::Identity(), + const Vector3& b = Vector3::Zero(), + const std::vector& S = std::vector()); + + static State identity(int n); +}; + +/** + * Data structure for ground-truth, input and output data + */ +struct Data { + State xi; // Ground-truth state + int n_cal; // Number of calibration states + Input u; // Input measurements + std::vector y; // Output measurements + int n_meas; // Number of measurements + double t; // Time + double dt; // Time step + + /** + * Initialize Data + * @param xi Ground-truth state + * @param n_cal Number of calibration states + * @param u Input measurements + * @param y Output measurements + * @param n_meas Number of measurements + * @param t Time + * @param dt Time step + */ + Data(const State& xi, int n_cal, const Input& u, + const std::vector& y, int n_meas, + double t, double dt); +}; + +//======================================================================== +// 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 + */ +class G { +public: + Rot3 A; // First SO(3) element + Matrix3 a; // so(3) element (skew-symmetric matrix) + std::vector B; // List of SO(3) elements for calibration + + /** + * Initialize the symmetry group G + * @param A SO3 element + * @param a so(3) element (skew symmetric matrix) + * @param B list of SO3 elements + */ + G(const Rot3& A = Rot3::Identity(), + const Matrix3& a = Matrix3::Zero(), + const std::vector& B = std::vector()); + + /** + * Define the group operation (multiplication) + * @param other Another group element + * @return The product of this and other + */ + G operator*(const G& other) const; + + /** + * Return the inverse element of the symmetry group + * @return The inverse of this group element + */ + G inv() const; + + /** + * Return the identity of the symmetry group + * @param n Number of calibration elements + * @return The identity element with n calibration components + */ + static G identity(int n); + + /** + * Return a group element X given by X = exp(x) + * @param x Vector representation of Lie algebra element + * @return Group element given by the exponential of x + */ + static G exp(const Vector& x); +}; + +//======================================================================== +// Helper Functions for EqF +//======================================================================== + +/** + * Compute the lift of the system (Theorem 3.8, Equation 7) + * @param xi State + * @param u Input + * @return Lift vector + */ +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 + */ +State stateAction(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 + */ +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 + */ +Vector3 outputAction(const G& X, const Direction& y, int idx); + +/** + * Local coordinates assuming xi_0 = identity (Equation 9) + * @param e State representing equivariant error + * @return Local coordinates + */ +Vector local_coords(const State& e); + +/** + * Local coordinates inverse assuming xi_0 = identity + * @param eps Local coordinates + * @return Corresponding state + */ +State local_coords_inv(const Vector& eps); + +/** + * Differential of the phi action at E = Id in local coordinates + * @param xi State + * @return Differential matrix + */ +Matrix stateActionDiff(const State& xi); + +//======================================================================== +// Equivariant Filter (EqF) +//======================================================================== + +/** + * Equivariant Filter (EqF) implementation + */ +class EqF { +private: + int __dof; // Degrees of freedom + int __n_cal; // Number of calibration states + int __n_sensor; // Number of sensors + 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 Direction& 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 n Number of calibration states + * @param m Number of sensors + */ + EqF(const Matrix& Sigma, int n, 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); +}; + +//======================================================================== +// Data Processing Functions +//======================================================================== + +/** + * Load data from CSV file into a vector of Data objects for the EqF + * + * CSV format: + * - t: Time + * - q_w, q_x, q_y, q_z: True attitude quaternion + * - b_x, b_y, b_z: True bias + * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion + * - w_x, w_y, w_z: Angular velocity measurements + * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations + * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations + * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements + * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations + * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions + * + * @param filename Path to the CSV file + * @param startRow First row to load (default: 0) + * @param maxRows Maximum number of rows to load (default: all) + * @param downsample Downsample factor (default: 1, which means no downsampling) + * @return Vector of Data objects + */ +std::vector loadDataFromCSV(const std::string& filename, + int startRow = 0, + int maxRows = -1, + int downsample = 1); + +/** + * Process data with EqF and print summary results + * @param filter Initialized EqF filter + * @param data_list Vector of Data objects to process + * @param printInterval Progress indicator interval (used internally) + */ +void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval = 10); + +} // namespace abc_eqf_lib + +#endif // ABC_EQF_H \ No newline at end of file diff --git a/examples/ABC_EQF/CMakeLists.txt b/examples/ABC_EQF/CMakeLists.txt deleted file mode 100644 index 23eb309d2..000000000 --- a/examples/ABC_EQF/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -add_executable(ABC_EqF - main.cpp - EqF.cpp - State.cpp - Input.cpp - G.cpp - Direction.cpp - Measurements.cpp - utilities.cpp - runEQF_withcsv.h -) - -target_link_libraries(ABC_EqF gtsam) - diff --git a/examples/ABC_EQF/Data.h b/examples/ABC_EQF/Data.h deleted file mode 100644 index 2e6005572..000000000 --- a/examples/ABC_EQF/Data.h +++ /dev/null @@ -1,41 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef DATA_H -#define DATA_H -//#pragma once - -#include "State.h" -#include "Input.h" -#include "Measurements.h" -#include - -/** - * Data structure for ground-truth, input and output data - */ -struct Data { - State xi; // Ground-truth state - int n_cal; // Number of calibration states - Input u; // Input measurements - std::vector y; // Output measurements - int n_meas; // Number of measurements - double t; // Time - double dt; // Time step - - /** - * Initialize Data - * @param xi Ground-truth state - * @param n_cal Number of calibration states - * @param u Input measurements - * @param y Output measurements - * @param n_meas Number of measurements - * @param t Time - * @param dt Time step - */ - Data(const State& xi, int n_cal, const Input& u, - const std::vector& y, int n_meas, - double t, double dt) - : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} -}; -#endif //DATA_H diff --git a/examples/ABC_EQF/Direction.cpp b/examples/ABC_EQF/Direction.cpp deleted file mode 100644 index 751324b0b..000000000 --- a/examples/ABC_EQF/Direction.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#include "Direction.h" -#include "utilities.h" -#include - -Direction::Direction(const Vector3& d_vec) : d(d_vec) { - if (!checkNorm(d_vec)) { - throw std::invalid_argument("Direction must be a unit vector"); - } -} \ No newline at end of file diff --git a/examples/ABC_EQF/Direction.h b/examples/ABC_EQF/Direction.h deleted file mode 100644 index 041b90bb6..000000000 --- a/examples/ABC_EQF/Direction.h +++ /dev/null @@ -1,35 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef DIRECTION_H -#define DIRECTION_H -//#pragma once -#include -#include -using namespace gtsam; -/** - * Direction class as a S2 element - */ -class Direction { -public: - Unit3 d; // Direction (unit vector on S2) - - /** - * Initialize direction - * @param d_vec Direction vector (must be unit norm) - */ - Direction(const Vector3& d_vec); - - // Accessor methods for vector components - double x() const { return d.unitVector()[0]; } - double y() const { return d.unitVector()[1]; } - double z() const { return d.unitVector()[2]; } - - // Check if the direction contains NaN values - bool hasNaN() const { - Vector3 vec = d.unitVector(); - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); - } -}; -#endif //DIRECTION_H diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp deleted file mode 100644 index 32414bb74..000000000 --- a/examples/ABC_EQF/EqF.cpp +++ /dev/null @@ -1,285 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "EqF.h" -#include "utilities.h" -#include -#include -#include - -// Implementation of helper functions - -Vector lift(const State& xi, const Input& u) { - int n = xi.S.size(); - 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 (int i = 0; i < n; i++) { - L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>(); - } - - return L; -} - -State stateAction(const G& X, const State& xi) { - if (xi.S.size() != X.B.size()) { - throw std::invalid_argument("Number of calibration states and B elements must match"); - } - - std::vector new_S; - for (size_t i = 0; i < X.B.size(); i++) { - new_S.push_back(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); -} - -Input velocityAction(const G& X, const Input& u) { - return Input(X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma); -} - -Vector3 outputAction(const G& X, const Direction& y, int idx) { - if (idx == -1) { - return X.A.inverse().matrix() * y.d.unitVector(); - } else { - if (idx >= static_cast(X.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return X.B[idx].inverse().matrix() * y.d.unitVector(); - } -} - -Vector local_coords(const State& e) { - if (COORDINATE == "EXPONENTIAL") { - Vector eps(6 + 3 * e.S.size()); - - // First 3 elements - eps.head<3>() = Rot3::Logmap(e.R); - - // Next 3 elements - eps.segment<3>(3) = e.b; - - // Remaining elements - for (size_t i = 0; i < e.S.size(); i++) { - eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); - } - - return eps; - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -State local_coords_inv(const Vector& eps) { - G X = G::exp(eps); - - if (COORDINATE == "EXPONENTIAL") { - std::vector S = X.B; - return State(X.A, eps.segment<3>(3), S); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix stateActionDiff(const State& xi) { - std::function coordsAction = - [&xi](const Vector& U) { - return local_coords(stateAction(G::exp(U), xi)); - }; - - Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); - Matrix differential = numericalDifferential(coordsAction, zeros); - return differential; -} - -// EqF class implementation - -EqF::EqF(const Matrix& Sigma, int n, int m) - : __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)), - __Sigma(Sigma), __xi_0(State::identity(n)) { - - 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(); -} - -State EqF::stateEstimate() const { - return stateAction(__X_hat, __xi_0); -} - -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 * Matrix3::Identity(), __n_cal)); - 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; -} - -bool hasNaN(const Vector3& vec) { - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); -} - -void EqF::update(const Measurement& y) { - if (y.cal_idx > __n_cal) { - throw std::invalid_argument("Calibration index out of range"); - } - - // Get vector representations for checking - Vector3 y_vec = y.y.d.unitVector(); - Vector3 d_vec = y.d.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 - } - static int update_count = 0; - if (update_count < 5) { - std::cout << "Update " << update_count << ":\n"; - std::cout << "y_vec: " << y_vec.transpose() << "\n"; - std::cout << "d_vec: " << d_vec.transpose() << "\n"; - update_count++; - } - - - - 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.d.unitVector()) * action_result; // Ensure this is the right operation - 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; -} - -Matrix EqF::__stateMatrixA(const Input& u) const { - Matrix3 W0 = velocityAction(__X_hat.inv(), u).W(); - Matrix A1 = Matrix::Zero(6, 6); - - if (COORDINATE == "EXPONENTIAL") { - A1.block<3, 3>(0, 3) = -Matrix3::Identity(); - A1.block<3, 3>(3, 3) = W0; - Matrix A2 = repBlock(W0, __n_cal); - return blockDiag(A1, A2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -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 * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); - Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; - - if (COORDINATE == "EXPONENTIAL") { - Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); - Phi1.block<3, 3>(0, 3) = Phi12; - Phi1.block<3, 3>(3, 3) = Phi22; - Matrix Phi2 = repBlock(Phi22, __n_cal); - return blockDiag(Phi1, Phi2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix EqF::__inputMatrixBt() const { - if (COORDINATE == "EXPONENTIAL") { - Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix()); - Matrix B2; - - for (const auto& B : __X_hat.B) { - if (B2.size() == 0) { - B2 = B.matrix(); - } else { - B2 = blockDiag(B2, B.matrix()); - } - } - - return blockDiag(B1, B2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * __n_cal); - - // If the measurement is related to a sensor that has a calibration state - if (idx >= 0) { - // Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); // WRONG - // Set the correct 3x3 block in Cc - Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.d.unitVector()); - } - - Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); - - // This Matrix concatenation was different from the Python version - // Create the equivalent of: - // Rot3.Hat(d.d.unitVector()) @ np.hstack((Rot3.Hat(d.d.unitVector()), np.zeros((3, 3)), Cc)) - - Matrix temp(3, 6 + 3 * __n_cal); - temp.block<3, 3>(0, 0) = wedge_d; - temp.block<3, 3>(0, 3) = Matrix3::Zero(); - temp.block(0, 6, 3, 3 * __n_cal) = Cc; - - return wedge_d * temp; -} - -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(__X_hat.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return __X_hat.B[idx].matrix(); - } else { - return __X_hat.A.matrix(); - } -} \ No newline at end of file diff --git a/examples/ABC_EQF/EqF.h b/examples/ABC_EQF/EqF.h deleted file mode 100644 index d09099ac1..000000000 --- a/examples/ABC_EQF/EqF.h +++ /dev/null @@ -1,153 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef EQF_H -#define EQF_H -#pragma once - -#include "State.h" -#include "Input.h" -#include "G.h" -#include "Direction.h" -#include "Measurements.h" -#include - -using namespace gtsam; - -/** - * Equivariant Filter (EqF) implementation - */ -class EqF { -private: - int __dof; // Degrees of freedom - int __n_cal; // Number of calibration states - int __n_sensor; // Number of sensors - 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 - -public: - /** - * Initialize EqF - * @param Sigma Initial covariance - * @param n Number of calibration states - * @param m Number of sensors - */ - EqF(const Matrix& Sigma, int n, 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); - -private: - /** - * 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 Direction& d, int idx) const; - - /** - * Return the measurement output matrix Dt - * @param idx Calibration index - * @return Measurement output matrix Dt - */ - Matrix __outputMatrixDt(int idx) const; -}; - -// Function declarations for helper functions used by EqF - -/** - * Compute the lift of the system (Theorem 3.8, Equation 7) - * @param xi State - * @param u Input - * @return Lift vector - */ -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 - */ -State stateAction(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 - */ -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 - */ -Vector3 outputAction(const G& X, const Direction& y, int idx = -1); - -/** - * Local coordinates assuming xi_0 = identity (Equation 9) - * @param e State representing equivariant error - * @return Local coordinates - */ -Vector local_coords(const State& e); - -/** - * Local coordinates inverse assuming xi_0 = identity - * @param eps Local coordinates - * @return Corresponding state - */ -State local_coords_inv(const Vector& eps); - -/** - * Differential of the phi action at E = Id in local coordinates - * @param xi State - * @return Differential matrix - */ -Matrix stateActionDiff(const State& xi); -#endif //EQF_H diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp deleted file mode 100644 index 1c9684e90..000000000 --- a/examples/ABC_EQF/G.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "G.h" -#include "utilities.h" -#include - -G::G(const Rot3& A, const Matrix3& a, const std::vector& B) - : A(A), a(a), B(B) {} - -G G::operator*(const G& other) const { - if (B.size() != other.B.size()) { - throw std::invalid_argument("Group elements must have the same number of calibration elements"); - } - - std::vector new_B; - for (size_t i = 0; i < B.size(); i++) { - new_B.push_back(B[i] * other.B[i]); - } - - return G(A * other.A, - a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), - new_B); -} - -G G::inv() const { - Matrix3 A_inv = A.inverse().matrix(); - - std::vector B_inv; - for (const auto& b : B) { - B_inv.push_back(b.inverse()); - } - - return G(A.inverse(), - -Rot3::Hat(A_inv * Rot3::Vee(a)), - B_inv); -} - -G G::identity(int n) { - std::vector B(n, Rot3::Identity()); - return G(Rot3::Identity(), Matrix3::Zero(), B); -} - -G G::exp(const Vector& x) { - if (x.size() < 6 || x.size() % 3 != 0) { - throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); - } - - int n = (x.size() - 6) / 3; - 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::vector B; - for (int i = 0; i < n; i++) { - B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); - } - - return G(A, a, B); -} \ No newline at end of file diff --git a/examples/ABC_EQF/G.h b/examples/ABC_EQF/G.h deleted file mode 100644 index 397303ac4..000000000 --- a/examples/ABC_EQF/G.h +++ /dev/null @@ -1,63 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef G_H -#define G_H - - -#include -#include -#include -#include - -using namespace gtsam; - -/** - * 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 - */ -class G { -public: - Rot3 A; // First SO(3) element - Matrix3 a; // so(3) element (skew-symmetric matrix) - std::vector B; // List of SO(3) elements for calibration - - /** - * Initialize the symmetry group G - * @param A SO3 element - * @param a so(3) element (skew symmetric matrix) - * @param B list of SO3 elements - */ - G(const Rot3& A = Rot3::Identity(), - const Matrix3& a = Matrix3::Zero(), - const std::vector& B = std::vector()); - - /** - * Define the group operation (multiplication) - * @param other Another group element - * @return The product of this and other - */ - G operator*(const G& other) const; - - /** - * Return the inverse element of the symmetry group - * @return The inverse of this group element - */ - G inv() const; - - /** - * Return the identity of the symmetry group - * @param n Number of calibration elements - * @return The identity element with n calibration components - */ - static G identity(int n); - - /** - * Return a group element X given by X = exp(x) - * @param x Vector representation of Lie algebra element - * @return Group element given by the exponential of x - */ - static G exp(const Vector& x); -}; -#endif //G_H diff --git a/examples/ABC_EQF/Input.cpp b/examples/ABC_EQF/Input.cpp deleted file mode 100644 index 488102cdb..000000000 --- a/examples/ABC_EQF/Input.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "Input.h" -#include "utilities.h" -#include -#include -#include "gtsam/geometry/Rot3.h" - -Input::Input(const Vector3& w, const Matrix& Sigma) - : w(w), Sigma(Sigma) { - if (Sigma.rows() != 6 || Sigma.cols() != 6) { - throw std::invalid_argument("Input measurement noise covariance must be 6x6"); - } - - // 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"); - } -} - -Matrix3 Input::W() const { - return Rot3::Hat(w); -} - -Input Input::random() { - Vector3 w = Vector3::Random(); - return Input(w, Matrix::Identity(6, 6)); -} \ No newline at end of file diff --git a/examples/ABC_EQF/Input.h b/examples/ABC_EQF/Input.h deleted file mode 100644 index 437d63b14..000000000 --- a/examples/ABC_EQF/Input.h +++ /dev/null @@ -1,41 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef INPUT_H -#define INPUT_H - -#include -#include - -using namespace gtsam; - -/** - * Input class for the Biased Attitude System - */ -class Input { -public: - Vector3 w; // Angular velocity - Matrix Sigma; // Noise covariance - - /** - * Initialize Input - * @param w Angular velocity (3-vector) - * @param Sigma Noise covariance (6x6 matrix) - */ - Input(const Vector3& w, const Matrix& Sigma); - - /** - * Return the Input as a skew-symmetric matrix - * @return w as a skew-symmetric matrix - */ - Matrix3 W() const; - - /** - * Return a random angular velocity - * @return A random angular velocity as Input element - */ - static Input random(); -}; - -#endif //INPUT_H diff --git a/examples/ABC_EQF/Measurements.cpp b/examples/ABC_EQF/Measurements.cpp deleted file mode 100644 index f1bd987ab..000000000 --- a/examples/ABC_EQF/Measurements.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "Measurements.h" -#include -#include - -Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i) - : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { - - // 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"); - } -} \ No newline at end of file diff --git a/examples/ABC_EQF/Measurements.h b/examples/ABC_EQF/Measurements.h deleted file mode 100644 index b74cc538c..000000000 --- a/examples/ABC_EQF/Measurements.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef MEASUREMENTS_H -#define MEASUREMENTS_H - - -#include "Direction.h" -#include - -using namespace gtsam; - -/** - * Measurement class - * cal_idx is an index corresponding to the calibration related to the measurement - * cal_idx = -1 indicates the measurement is from a calibrated sensor - */ -class Measurement { -public: - Direction y; // Measurement direction in sensor frame - Direction d; // Known direction in global frame - Matrix3 Sigma; // Covariance matrix of the measurement - int cal_idx = -1; // Calibration index (-1 for calibrated sensor) - - /** - * Initialize measurement - * @param y_vec Direction measurement in sensor frame - * @param d_vec Known direction in global frame - * @param Sigma Measurement noise covariance - * @param i Calibration index (-1 for calibrated sensor) - */ - Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i = -1); -}; -#endif //MEASUREMENTS_H diff --git a/examples/ABC_EQF/State.cpp b/examples/ABC_EQF/State.cpp deleted file mode 100644 index 90dabfbda..000000000 --- a/examples/ABC_EQF/State.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "State.h" - -State::State(const Rot3& R, const Vector3& b, const std::vector& S) - : R(R), b(b), S(S) {} - -State State::identity(int n) { - std::vector calibrations(n, Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), calibrations); -} \ No newline at end of file diff --git a/examples/ABC_EQF/State.h b/examples/ABC_EQF/State.h deleted file mode 100644 index 6c8b9c5a6..000000000 --- a/examples/ABC_EQF/State.h +++ /dev/null @@ -1,29 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef STATE_H -#define STATE_H - -#include -#include -#include - -using namespace gtsam; - -/** - * State class representing the state of the Biased Attitude System - */ -class State { -public: - Rot3 R; // Attitude rotation matrix R - Vector3 b; // Gyroscope bias b - std::vector S; // Sensor calibrations S - - State(const Rot3& R = Rot3::Identity(), - const Vector3& b = Vector3::Zero(), - const std::vector& S = std::vector()); - - static State identity(int n); -}; -#endif //STATE_H diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp deleted file mode 100644 index b21eae44a..000000000 --- a/examples/ABC_EQF/main.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "EqF.h" -#include "State.h" -#include "Input.h" -#include "Direction.h" -#include "Measurements.h" -#include "Data.h" -#include "runEQF_withcsv.h" -#include "utilities.h" -#include -#include -#include -#include -#include -#include -#include - - -using namespace std; -using namespace gtsam; - -// Simplified data loading function - in a real application, implement proper CSV parsing -std::vector loadSimulatedData() { - std::vector data_list; - - double t = 0.0; - double dt = 0.01; - - // Number of data points - int num_points = 100; - - // Set up one calibration state - int n_cal = 1; - - for (int i = 0; i < num_points; i++) { - t += dt; - - // Create a simple sinusoidal trajectory - double angle = 0.1 * sin(t); - Rot3 R = Rot3::Rz(angle); - - // Create a bias - Vector3 b(0.01, 0.02, 0.03); - - // Create a calibration - std::vector S; - S.push_back(Rot3::Ry(0.05)); - - // State - State xi(R, b, S); - - // Input (angular velocity) - Vector3 w(0.1 * cos(t), 0.05 * sin(t), 0.02); - Matrix Sigma_u = Matrix::Identity(6, 6) * 0.01; - Input u(w, Sigma_u); - - // Measurements - std::vector measurements; - - // Measurement 1 - from uncalibrated sensor - Vector3 d1_vec = Vector3(1, 0, 0).normalized(); // Known direction in global frame - Vector3 y1_vec = S[0].inverse().matrix() * R.inverse().matrix() * d1_vec; // Direction in sensor frame - Matrix3 Sigma1 = Matrix3::Identity() * 0.01; - measurements.push_back(Measurement(y1_vec, d1_vec, Sigma1, 0)); // cal_idx = 0 - - // Measurement 2 - from calibrated sensor - Vector3 d2_vec = Vector3(0, 1, 0).normalized(); // Known direction in global frame - Vector3 y2_vec = R.inverse().matrix() * d2_vec; // Direction in sensor frame - Matrix3 Sigma2 = Matrix3::Identity() * 0.01; - measurements.push_back(Measurement(y2_vec, d2_vec, Sigma2, -1)); // cal_idx = -1 - - // Add to data list - data_list.push_back(Data(xi, n_cal, u, measurements, 2, t, dt)); - } - - return data_list; -} - -void runSimulation(EqF& filter, const std::vector& data) { - std::cout << "Starting simulation with " << data.size() << " data points..." << std::endl; - - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); - - // Store results for analysis - std::vector times; - std::vector attitude_errors; - std::vector bias_errors; - std::vector calibration_errors; - - for (const auto& d : data) { - // Propagation - try { - filter.propagation(d.u, d.dt); - } catch (const std::exception& e) { - std::cerr << "Propagation error at t=" << d.t << ": " << e.what() << std::endl; - continue; - } - - // Update with measurements - for (const auto& y : d.y) { - try { - if (!std::isnan(y.y.d.unitVector().norm()) && !std::isnan(y.d.d.unitVector().norm())) { - filter.update(y); - } - } catch (const std::exception& e) { - std::cerr << "Update error at t=" << d.t << ": " << e.what() << std::endl; - } - } - - // Get state estimate - State estimate = filter.stateEstimate(); - - // Compute errors - Vector3 att_error = Rot3::Logmap(d.xi.R.between(estimate.R)); - Vector3 bias_error = estimate.b - d.xi.b; - Vector3 cal_error = Vector3::Zero(); - if (!d.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(d.xi.S[0].between(estimate.S[0])); - } - - // Store results - times.push_back(d.t); - attitude_errors.push_back(att_error); - bias_errors.push_back(bias_error); - calibration_errors.push_back(cal_error); - - // Print some info - if (d.t < 0.1 || fmod(d.t, 1.0) < d.dt) { - std::cout << "Time: " << d.t - << ", Attitude error (deg): " << (att_error.norm() * 180.0/M_PI) - << ", Bias error: " << bias_error.norm() - << ", Calibration error (deg): " << (cal_error.norm() * 180.0/M_PI) - << std::endl; - } - } - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end - start; - std::cout << "Simulation completed in " << elapsed.count() << " seconds" << std::endl; - - // Print summary statistics - double avg_att_error = 0.0; - double avg_bias_error = 0.0; - double avg_cal_error = 0.0; - - for (size_t i = 0; i < times.size(); i++) { - avg_att_error += attitude_errors[i].norm(); - avg_bias_error += bias_errors[i].norm(); - avg_cal_error += calibration_errors[i].norm(); - } - - avg_att_error /= times.size(); - avg_bias_error /= times.size(); - avg_cal_error /= times.size(); - - std::cout << "Average attitude error (deg): " << (avg_att_error * 180.0/M_PI) << std::endl; - std::cout << "Average bias error: " << avg_bias_error << std::endl; - std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl; -} - - - - -int main(int argc, char** argv) { - std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; - std::cout << "========================================================" << std::endl; - - std::string csvFilePath; - - // Try to find the EQFdata file in the GTSAM examples directory - try { - // Look specifically for EQFdata.csv in GTSAM examples - csvFilePath = findExampleDataFile("EqFdata.csv"); - std::cout << "Using GTSAM example data file: " << csvFilePath << std::endl; - } catch (const std::exception& e) { - std::cerr << "Error: Could not find EqFdata.csv in GTSAM examples directory" << std::endl; - std::cerr << e.what() << std::endl; - return 1; - } - - try { - // Run with CSV data - runEqFWithCSVData(csvFilePath); - } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; - return 1; - } - - std::cout << "Done." << std::endl; - return 0; -} \ No newline at end of file diff --git a/examples/ABC_EQF/runEQF_withcsv.h b/examples/ABC_EQF/runEQF_withcsv.h deleted file mode 100644 index d31baffe0..000000000 --- a/examples/ABC_EQF/runEQF_withcsv.h +++ /dev/null @@ -1,683 +0,0 @@ -// -// Created by darshan on 3/17/25. -// - -#ifndef RUNEQF_WITHCSV_H -#define RUNEQF_WITHCSV_H - -// -// Created by darshan on 3/17/25. -// -#include "Data.h" -#include "State.h" -#include "Input.h" -#include "Direction.h" -#include "Measurements.h" -#include "utilities.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * Load data from CSV file into a vector of Data objects for the EqF - * - * CSV format: - * - t: Time - * - q_w, q_x, q_y, q_z: True attitude quaternion - * - b_x, b_y, b_z: True bias - * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion - * - w_x, w_y, w_z: Angular velocity measurements - * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations - * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations - * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements - * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations - * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions - * - * @param filename Path to the CSV file - * @param startRow First row to load (default: 0) - * @param maxRows Maximum number of rows to load (default: all) - * @param downsample Downsample factor (default: 1, which means no downsampling) - * @return Vector of Data objects - */ -inline std::vector loadDataFromCSV(const std::string& filename, - int startRow = 0, - int maxRows = -1, - int downsample = 1) { - std::vector data_list; - std::ifstream file(filename); - - if (!file.is_open()) { - throw std::runtime_error("Failed to open file: " + filename); - } - - std::string line; - int lineNumber = 0; - int rowCount = 0; - double prevTime = 0.0; - - // Skip header - std::getline(file, line); - lineNumber++; - - // Skip to startRow - while (lineNumber < startRow && std::getline(file, line)) { - lineNumber++; - } - - // Read data - while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { - lineNumber++; - - // Apply downsampling - if ((lineNumber - startRow - 1) % downsample != 0) { - continue; - } - - std::istringstream ss(line); - std::string token; - std::vector values; - - // Parse line into values - while (std::getline(ss, token, ',')) { - try { - values.push_back(std::stod(token)); - } catch (const std::exception& e) { - std::cerr << "Error parsing value at line " << lineNumber << ": " << token << std::endl; - values.push_back(0.0); // Use default value - } - } - - // Check if we have enough values - if (values.size() < 39) { - std::cerr << "Warning: Line " << lineNumber << " has only " << values.size() - << " values, expected 39. Skipping." << std::endl; - continue; - } - - // Extract values - double t = values[0]; - double dt = (rowCount == 0) ? 0.0 : t - prevTime; - prevTime = t; - - // Create ground truth state - Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z - Rot3 R = Rot3(quat); - - Vector3 b(values[5], values[6], values[7]); - - Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z - std::vector S = {Rot3(calQuat)}; - - State xi(R, b, S); - - // Create input - Vector3 w(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); - - // Create measurements - std::vector measurements; - - // First measurement (calibrated sensor, cal_idx = 0) - Vector3 y0(values[21], values[22], values[23]); - Vector3 d0(values[33], values[34], values[35]); - - // Normalize vectors if needed - if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); - if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); - - // Measurement covariance - Matrix3 covY0 = Matrix3::Zero(); - covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 - covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 - covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 - - // Create measurement - measurements.push_back(Measurement(y0, d0, covY0, 0)); - - // Second measurement (calibrated sensor, cal_idx = -1) - Vector3 y1(values[24], values[25], values[26]); - Vector3 d1(values[36], values[37], values[38]); - - // Normalize vectors if needed - if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); - if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); - - // Measurement covariance - Matrix3 covY1 = Matrix3::Zero(); - covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 - covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 - covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 - - // Create measurement - measurements.push_back(Measurement(y1, d1, covY1, -1)); - - // Create Data object and add to list - data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); - - rowCount++; - } - - std::cout << "Loaded " << data_list.size() << " data points from CSV file." << std::endl; - - return data_list; -} - -/** - * Process Data objects with the EqF filter - * - * @param filter EqF filter to use - * @param data_list Vector of Data objects - * @param saveResults Whether to save results to a file - * @param resultFilename Filename to save results to - */ -inline void printDataPoint(const Data& data, int index) { - std::cout << "Data[" << index << "] @ t=" << data.t << ", dt=" << data.dt << std::endl; - - // Print angular velocity - std::cout << " ω = [" << data.u.w[0] << ", " << data.u.w[1] << ", " << data.u.w[2] << "]" << std::endl; - - // Print measurements - for (size_t i = 0; i < data.y.size(); i++) { - const Measurement& meas = data.y[i]; - // Use the unitVector() method to get a Vector3 from a Unit3 object - Vector3 y_vec = meas.y.d.unitVector(); - Vector3 d_vec = meas.d.d.unitVector(); - std::cout << " y" << i << " = [" << y_vec[0] << ", " << y_vec[1] << ", " << y_vec[2] << "]" << std::endl; - std::cout << " d" << i << " = [" << d_vec[0] << ", " << d_vec[1] << ", " << d_vec[2] << "]" << std::endl; - } - - std::cout << std::endl; -} - - -// Function to print sample data points -inline void printDataSamples(const std::vector& data_list, int count = 3) { - int total = data_list.size(); - - std::cout << "\n=== First " << count << " Data Points ===" << std::endl; - for (int i = 0; i < std::min(count, total); i++) { - printDataPoint(data_list[i], i); - } - - if (total > 2*count) { - std::cout << "\n... (" << (total - 2*count) << " points omitted) ...\n" << std::endl; - - std::cout << "=== Last " << count << " Data Points ===" << std::endl; - for (int i = std::max(count, total - count); i < total; i++) { - printDataPoint(data_list[i], i); - } - } -} - -// Function to validate data -inline bool validateData(const std::vector& data_list) { - if (data_list.empty()) { - std::cerr << "ERROR: No data loaded from CSV" << std::endl; - return false; - } - - std::cout << "Validating " << data_list.size() << " data points..." << std::endl; - - // Track statistics - int invalid_count = 0; - - // Open a log file to record detailed issues - std::ofstream logFile("data_validation.log"); - logFile << "Data Validation Report" << std::endl; - logFile << "--------------------" << std::endl; - - for (size_t i = 0; i < data_list.size(); ++i) { - const Data& data = data_list[i]; - bool point_valid = true; - - // Check time and dt - if (std::isnan(data.t) || std::isnan(data.dt)) { - logFile << "Point " << i << ": Invalid time values (t=" << data.t - << ", dt=" << data.dt << ")" << std::endl; - point_valid = false; - } - - // Check ground truth state for NaN - using isnan directly on components - const auto& R_matrix = data.xi.R.matrix(); - bool R_has_nan = false; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - if (std::isnan(R_matrix(r, c))) { - R_has_nan = true; - break; - } - } - } - - if (R_has_nan) { - logFile << "Point " << i << ": NaN in ground truth attitude matrix" << std::endl; - point_valid = false; - } - - // Check bias vector for NaN - bool b_has_nan = false; - for (int j = 0; j < 3; j++) { - if (std::isnan(data.xi.b[j])) { - b_has_nan = true; - break; - } - } - - if (b_has_nan) { - logFile << "Point " << i << ": NaN in ground truth bias vector" << std::endl; - point_valid = false; - } - - // Check calibration matrices for NaN - for (size_t j = 0; j < data.xi.S.size(); ++j) { - const auto& S_matrix = data.xi.S[j].matrix(); - bool S_has_nan = false; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - if (std::isnan(S_matrix(r, c))) { - S_has_nan = true; - break; - } - } - } - - if (S_has_nan) { - logFile << "Point " << i << ": NaN in ground truth calibration matrix " - << j << std::endl; - point_valid = false; - } - } - - // Check input for NaN - bool w_has_nan = false; - for (int j = 0; j < 3; j++) { - if (std::isnan(data.u.w[j])) { - w_has_nan = true; - break; - } - } - - if (w_has_nan) { - logFile << "Point " << i << ": NaN in angular velocity" << std::endl; - point_valid = false; - } - - // Check measurements - for (size_t j = 0; j < data.y.size(); ++j) { - const Measurement& meas = data.y[j]; - - // Get the Vector3 representations to check them - Vector3 y_vec = meas.y.d.unitVector(); - Vector3 d_vec = meas.d.d.unitVector(); - - // Check measurement vector for NaN - bool y_has_nan = false; - bool d_has_nan = false; - - for (int k = 0; k < 3; k++) { - if (std::isnan(y_vec[k])) { - y_has_nan = true; - break; - } - if (std::isnan(d_vec[k])) { - d_has_nan = true; - break; - } - } - - if (y_has_nan) { - logFile << "Point " << i << ", Meas " << j << ": NaN in measurement vector" << std::endl; - point_valid = false; - } - - if (d_has_nan) { - logFile << "Point " << i << ", Meas " << j << ": NaN in reference direction" << std::endl; - point_valid = false; - } - - // Calculate norm using Vector3 norms - double y_norm = y_vec.norm(); - double d_norm = d_vec.norm(); - - if (std::abs(y_norm - 1.0) > 1e-5) { - logFile << "Point " << i << ", Meas " << j - << ": Measurement vector not normalized. Norm = " << y_norm << std::endl; - point_valid = false; - } - - if (std::abs(d_norm - 1.0) > 1e-5) { - logFile << "Point " << i << ", Meas " << j - << ": Reference direction not normalized. Norm = " << d_norm << std::endl; - point_valid = false; - } - } - - if (!point_valid) { - invalid_count++; - - // Print first few invalid points to console - if (invalid_count <= 5) { - std::cerr << "Invalid data at point " << i << " (t=" << data.t << ")" << std::endl; - } - } - } - - // Close the log - logFile << std::endl << "Summary: " << invalid_count << " invalid data points out of " - << data_list.size() << std::endl; - logFile.close(); - - - - - - - - - - // Print summary - std::cout << "Data validation complete. " << invalid_count << " invalid points found." << std::endl; - if (invalid_count > 0) { - std::cout << "See data_validation.log for details." << std::endl; - } - - return (invalid_count == 0); -} - -inline void processDataWithEqF(EqF& filter, - const std::vector& data_list, - bool saveResults = false, - const std::string& resultFilename = "eqf_results.csv") { - std::ofstream resultFile; - if (saveResults) { - resultFile.open(resultFilename); - if (!resultFile.is_open()) { - throw std::runtime_error("Failed to open result file: " + resultFilename); - } - - // Write header - now adding roll, pitch, yaw columns for estimated and true values - resultFile << "t,"; - // Estimated state quaternion - resultFile << "est_qw,est_qx,est_qy,est_qz,"; - // Estimated bias - resultFile << "est_bx,est_by,est_bz,"; - // Estimated calibration quaternion - resultFile << "est_cqw,est_cqx,est_cqy,est_cqz,"; - // True state quaternion - resultFile << "true_qw,true_qx,true_qy,true_qz,"; - // True bias - resultFile << "true_bx,true_by,true_bz,"; - // True calibration quaternion - resultFile << "true_cqw,true_cqx,true_cqy,true_cqz,"; - // Add Euler angles for estimated state - resultFile << "est_roll,est_pitch,est_yaw,"; - // Add Euler angles for true state - resultFile << "true_roll,true_pitch,true_yaw,"; - // Add Euler angles for estimated calibration - resultFile << "est_cal_roll,est_cal_pitch,est_cal_yaw,"; - // Add Euler angles for true calibration - resultFile << "true_cal_roll,true_cal_pitch,true_cal_yaw"; - resultFile << std::endl; - } - - std::cout << "Processing data with EqF..." << std::endl; - - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); - - // Store error metrics - std::vector att_errors; - std::vector bias_errors; - std::vector cal_errors; - - int total_measurements = 0; - int valid_measurements = 0; - int invalid_measurements = 0; - - for (size_t i = 0; i < data_list.size(); i++) { - const Data& data = data_list[i]; - - // Propagation - filter.propagation(data.u, data.dt); - - // Update with measurements - for (const auto& y : data.y) { - total_measurements++; - - // Check for NaN values in measurement vectors - bool has_nan = false; - Vector3 y_vec = y.y.d.unitVector(); - Vector3 d_vec = y.d.d.unitVector(); - - for (int j = 0; j < 3; j++) { - if (std::isnan(y_vec[j]) || std::isnan(d_vec[j])) { - has_nan = true; - break; - } - } - - if (!has_nan) { - try { - filter.update(y); - valid_measurements++; - } catch (const std::exception& e) { - std::cerr << "Error updating at t=" << data.t << ": " << e.what() << std::endl; - invalid_measurements++; - } - } else { - invalid_measurements++; - } - } - - // Get state estimate - State estimate = filter.stateEstimate(); - - // Compute 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(); - if (!data.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); - } - - // Store errors - att_errors.push_back(att_error.norm()); - bias_errors.push_back(bias_error.norm()); - cal_errors.push_back(cal_error.norm()); - - // Save results - if (saveResults) { - // Extract quaternions - Quaternion est_q = estimate.R.toQuaternion(); - Quaternion true_q = data.xi.R.toQuaternion(); - - // Extract Euler angles (roll, pitch, yaw) from estimated rotation - Vector3 est_rpy = estimate.R.rpy(); - // Convert to degrees for easier comparison - Vector3 est_rpy_deg = est_rpy * 180.0 / M_PI; - - // Extract Euler angles from true rotation - Vector3 true_rpy = data.xi.R.rpy(); - // Convert to degrees - Vector3 true_rpy_deg = true_rpy * 180.0 / M_PI; - - // Get calibration quaternions and Euler angles - Quaternion est_cal_q, true_cal_q; - Vector3 est_cal_rpy_deg = Vector3::Zero(); - Vector3 true_cal_rpy_deg = Vector3::Zero(); - - if (!estimate.S.empty() && !data.xi.S.empty()) { - est_cal_q = estimate.S[0].toQuaternion(); - true_cal_q = data.xi.S[0].toQuaternion(); - - // Get Euler angles for calibrations - Vector3 est_cal_rpy = estimate.S[0].rpy(); - est_cal_rpy_deg = est_cal_rpy * 180.0 / M_PI; - - Vector3 true_cal_rpy = data.xi.S[0].rpy(); - true_cal_rpy_deg = true_cal_rpy * 180.0 / M_PI; - } else { - est_cal_q = Quaternion(1, 0, 0, 0); // Identity quaternion - true_cal_q = Quaternion(1, 0, 0, 0); - } - - // Write to file - resultFile << data.t << ","; - // Estimated quaternion - resultFile << est_q.w() << "," << est_q.x() << "," << est_q.y() << "," << est_q.z() << ","; - // Estimated bias - resultFile << estimate.b[0] << "," << estimate.b[1] << "," << estimate.b[2] << ","; - // Estimated calibration quaternion - resultFile << est_cal_q.w() << "," << est_cal_q.x() << "," << est_cal_q.y() << "," << est_cal_q.z() << ","; - // True quaternion - resultFile << true_q.w() << "," << true_q.x() << "," << true_q.y() << "," << true_q.z() << ","; - // True bias - resultFile << data.xi.b[0] << "," << data.xi.b[1] << "," << data.xi.b[2] << ","; - // True calibration quaternion - resultFile << true_cal_q.w() << "," << true_cal_q.x() << "," << true_cal_q.y() << "," << true_cal_q.z() << ","; - - // Add Euler angles (in degrees) for estimated state - resultFile << est_rpy_deg[0] << "," << est_rpy_deg[1] << "," << est_rpy_deg[2] << ","; - // Add Euler angles (in degrees) for true state - resultFile << true_rpy_deg[0] << "," << true_rpy_deg[1] << "," << true_rpy_deg[2] << ","; - // Add Euler angles (in degrees) for estimated calibration - resultFile << est_cal_rpy_deg[0] << "," << est_cal_rpy_deg[1] << "," << est_cal_rpy_deg[2] << ","; - // Add Euler angles (in degrees) for true calibration - resultFile << true_cal_rpy_deg[0] << "," << true_cal_rpy_deg[1] << "," << true_cal_rpy_deg[2]; - - resultFile << std::endl; - } - - // Print progress - if (i % 1000 == 0 || i == data_list.size() - 1) { - std::cout << "Processed " << i+1 << "/" << data_list.size() - << " (" << (100.0 * (i+1) / data_list.size()) << "%) "; - std::cout << "Attitude error: " << (att_error.norm() * 180.0/M_PI) << " deg, "; - std::cout << "Bias error: " << bias_error.norm() << ", "; - std::cout << "Calibration error: " << (cal_error.norm() * 180.0/M_PI) << " deg" << std::endl; - } - } - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end - start; - - // Calculate average errors - double avg_att_error = 0.0; - double avg_bias_error = 0.0; - double avg_cal_error = 0.0; - - if (!att_errors.empty()) { - avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); - avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); - avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); - } - - std::cout << std::endl; - std::cout << "EqF Processing completed in " << elapsed.count() << " seconds" << std::endl; - std::cout << "Average attitude error: " << (avg_att_error * 180.0/M_PI) << " deg" << std::endl; - std::cout << "Average bias error: " << avg_bias_error << std::endl; - std::cout << "Average calibration error: " << (avg_cal_error * 180.0/M_PI) << " deg" << std::endl; - std::cout << "Total measurements: " << total_measurements << std::endl; - std::cout << "Valid measurements processed: " << valid_measurements << std::endl; - std::cout << "Invalid measurements skipped: " << invalid_measurements << std::endl; - - if (saveResults) { - resultFile.close(); - std::cout << "Results saved to " << resultFilename << std::endl; - } -} - - - - - -inline void runEqFWithCSVData(const std::string& filename) { - try { - // Load data from CSV file with optional parameters - int startRow = 0; - int maxRows = -1; - int downsample = 1; - - std::vector data = loadDataFromCSV(filename, startRow, maxRows, downsample); - - if (data.empty()) { - std::cerr << "No data loaded from CSV file." << std::endl; - return; - } - - // Print sample data points to inspect the loaded data - std::cout << "Data loaded, displaying samples..." << std::endl; - printDataSamples(data); - - // Validate the data to check for issues - std::cout << "Validating data integrity..." << std::endl; - bool dataValid = validateData(data); - - if (!dataValid) { - std::cout << "Warning: Data validation found issues." << std::endl; - std::string proceed; - std::cout << "Do you want to proceed anyway? (y/n): "; - std::cin >> proceed; - if (proceed != "y" && proceed != "Y") { - std::cout << "Operation cancelled by user." << std::endl; - return; - } - } - - // Initialize EqF filter - int n_cal = 1; // Number of calibration states (from the data) - int n_sensors = 2; // Number of sensors (from the data) - - // Initial covariance - Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); - initialSigma.diagonal().head<3>() = Vector3::Constant(0.1); // Reduced attitude uncertainty - initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.01); // Reduced bias uncertainty - initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Reduced calibration uncertainty - - // Create filter - EqF filter(initialSigma, n_cal, n_sensors); - - // Initialize filter state from the first ground truth if possible - if (!data.empty()) { - // You'll need to add a method to your EqF class to set the initial state - // Something like: - // filter.setInitialState(data[0].xi); - - // If you don't have such a method, you can print the first ground truth - // to see if it makes sense - std::cout << "First ground truth state:" << std::endl; - std::cout << "Attitude: " << data[0].xi.R.matrix() << std::endl; - std::cout << "Bias: " << data[0].xi.b.transpose() << std::endl; - std::cout << "Calibration: " << data[0].xi.S[0].matrix() << std::endl; - } - - // Process data with the filter and save results - processDataWithEqF(filter, data, true, "eqf_results.csv"); - - } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; - } -} -/** - * Example usage function to demonstrate how to use the data loader with the EqF - */ - -#endif //RUNEQF_WITHCSV_H diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp deleted file mode 100644 index 5166cb0d6..000000000 --- a/examples/ABC_EQF/utilities.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "utilities.h" -#include - -// Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" - - -bool checkNorm(const Vector3& x, double tol) { - return abs(x.norm() - 1) < tol || std::isnan(x.norm()); -} - -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; - } -} - -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; -} - -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; -} \ No newline at end of file diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h deleted file mode 100644 index 50bbf88aa..000000000 --- a/examples/ABC_EQF/utilities.h +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef UTILITIES_H -#define UTILITIES_H - -#pragma once - -#include -#include -#include -#include - -using namespace gtsam; - -// Global configuration -extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL" - -/** - * Utility functions - */ -Matrix3 wedge(const Vector3& vec); -bool checkNorm(const Vector3& x, double tol = 1e-3); -Matrix blockDiag(const Matrix& A, const Matrix& B); -Matrix repBlock(const Matrix& A, int n); -Matrix numericalDifferential(std::function f, const Vector& x); -#endif //UTILITIES_H diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp new file mode 100644 index 000000000..6b0f4389d --- /dev/null +++ b/examples/ABC_EQF_Demo.cpp @@ -0,0 +1,89 @@ +/** + * @file ABC_EQF_Demo.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 + */ + +#include "ABC_EQF.h" + +// Use namespace for convenience +using namespace abc_eqf_lib; +using namespace gtsam; + +/** + * Main function for the EqF demo + * @param argc Number of arguments + * @param argv Array of arguments + * @return Exit code + */ + + +int main(int argc, char* argv[]) { + std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo" << std::endl; + std::cout << "==============================================================" << std::endl; + + try { + // Parse command line options + std::string csvFilePath; + int maxRows = -1; // Process all rows by default + int downsample = 1; // No downsampling by default + + if (argc > 1) { + csvFilePath = argv[1]; + } else { + // Try to find the EQFdata file in the GTSAM examples directory + try { + csvFilePath = findExampleDataFile("EqFdata.csv"); + } catch (const std::exception& e) { + std::cerr << "Error: Could not find EqFdata.csv" << std::endl; + std::cerr << "Usage: " << argv[0] << " [csv_file_path] [max_rows] [downsample]" << std::endl; + return 1; + } + } + + // Optional command line parameters + if (argc > 2) { + maxRows = std::stoi(argv[2]); + } + + if (argc > 3) { + downsample = std::stoi(argv[3]); + } + + // Load data from CSV file + std::vector data = loadDataFromCSV(csvFilePath, 0, maxRows, downsample); + + if (data.empty()) { + std::cerr << "No data available to process. Exiting." << std::endl; + return 1; + } + + // Initialize the EqF filter with one calibration state + int n_cal = 1; + int n_sensors = 2; + + // Initial covariance - larger values allow faster convergence + Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); + 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 + + // Create filter + EqF filter(initialSigma, n_cal, n_sensors); + + // Process data + processDataWithEqF(filter, data); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return 1; + } + + std::cout << "\nEqF demonstration completed successfully." << std::endl; + return 0; +} \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 581535c9f..8da95722c 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,4 +17,3 @@ if (NOT GTSAM_USE_BOOST_FEATURES) endif() gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) -add_subdirectory(ABC_EQF) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index a5a6f19af..7f3183aa2 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -86,7 +86,10 @@ endforeach(subdir) # To add additional sources to gtsam when building the full library (static or shared) # append the subfolder with _srcs appended to the end to this list -set(gtsam_srcs ${3rdparty_srcs}) +set(gtsam_srcs ${3rdparty_srcs} + ../examples/ABC_EQF_Demo.cpp + ../examples/ABC_EQF.cpp + ../examples/ABC_EQF.h) foreach(subdir ${gtsam_subdirs}) list(APPEND gtsam_srcs ${${subdir}_srcs}) endforeach(subdir) From d48bf56ff8c5f198bf61064a92665f274eb54d3c Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Wed, 16 Apr 2025 20:59:19 -0700 Subject: [PATCH 07/38] Removed older linux dependencies, updated cmake version requirement in METIS CMakeLists.txt & removed variables that were not used --- examples/ABC_EQF.cpp | 2 +- examples/ABC_EQF.h | 1 - gtsam/3rdparty/metis/CMakeLists.txt | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/examples/ABC_EQF.cpp b/examples/ABC_EQF.cpp index 8209e7b29..5bc777458 100644 --- a/examples/ABC_EQF.cpp +++ b/examples/ABC_EQF.cpp @@ -502,7 +502,7 @@ Matrix stateActionDiff(const State& xi) { * Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse() */ EqF::EqF(const Matrix& Sigma, int n, int m) - : __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)), + : __dof(6 + 3 * n), __n_cal(n), __X_hat(G::identity(n)), __Sigma(Sigma), __xi_0(State::identity(n)) { if (Sigma.rows() != __dof || Sigma.cols() != __dof) { diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h index 6d381b1ee..6f069020c 100644 --- a/examples/ABC_EQF.h +++ b/examples/ABC_EQF.h @@ -312,7 +312,6 @@ class EqF { private: int __dof; // Degrees of freedom int __n_cal; // Number of calibration states - int __n_sensor; // Number of sensors G __X_hat; // Filter state Matrix __Sigma; // Error covariance State __xi_0; // Origin state diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index f8da7ce6f..5f9f7ce23 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.5...3.22) project(METIS) # Add flags for currect directory and below From e5f4978539fd70a9bcb55f18b353135b0780d1d2 Mon Sep 17 00:00:00 2001 From: jenniferoum Date: Sun, 20 Apr 2025 20:11:08 -0700 Subject: [PATCH 08/38] Address some pr comments - remove ABC_EQF.cpp, clean up comments, change Input class to struct, remove changes to target --- examples/ABC_EQF.cpp | 1009 --------------------------- examples/ABC_EQF.h | 851 +++++++++++++++++++--- examples/ABC_EQF_Demo.cpp | 340 +++++++++ gtsam/3rdparty/metis/CMakeLists.txt | 4 +- gtsam/CMakeLists.txt | 5 +- 5 files changed, 1084 insertions(+), 1125 deletions(-) delete mode 100644 examples/ABC_EQF.cpp diff --git a/examples/ABC_EQF.cpp b/examples/ABC_EQF.cpp deleted file mode 100644 index 5bc777458..000000000 --- a/examples/ABC_EQF.cpp +++ /dev/null @@ -1,1009 +0,0 @@ -/** - * @file ABC_EQF.cpp - * @brief Implementation of the Attitude-Bias-Calibration Equivariant Filter - * - * This file contains the implementation 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 - */ - -#include "ABC_EQF.h" - -namespace abc_eqf_lib { - -// Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Denotes how the states are mapped to the vector representations - -//======================================================================== -// Utility Functions -//======================================================================== -/** - * @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 - * Uses Vector3 norm() method to calculate vector magnitude - */ -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 - * Uses Matrix's rows(), cols(), setZero(), and block() methods - */ - -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 - * Recursively uses blockDiag() function - */ -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; -} - -/** - * @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; -} - -//======================================================================== -// Direction Class Implementation -//======================================================================== - -/** - * @brief Initializes a direction object vector from a provided 3D vector input - * @param d_vec A 3-D vector that should have a unit norm(This represents a - * direction in 3D space) Uses Unit3's constructor which normalizes vectors - */ - - -Direction::Direction(const Vector3& d_vec) : d(d_vec) { - if (!checkNorm(d_vec)) { - throw std::invalid_argument("Direction must be a unit vector"); - } -} - /** Access the individual components of the direction vector defined above using this methods below - * Uses the Unit3 object from GTSAM to compute the components - */ - -double Direction::x() const { - return d.unitVector()[0]; -} - -double Direction::y() const { - return d.unitVector()[1]; -} - -double Direction::z() const { - return d.unitVector()[2]; -} - -bool Direction::hasNaN() const { - Vector3 vec = d.unitVector(); - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); -} - -//======================================================================== -// Input Class Implementation -//======================================================================== -/** - * @brief Constructs an input object using the Angular velocity vector and the - * covariance matrix - * @param w Angular vector - * @param Sigma 6X6 covariance matrix - * Uses Matrix's rows(), cols() and Eigen's SelfAdjointEigenSolver - */ -Input::Input(const Vector3& w, const Matrix& Sigma) : w(w), Sigma(Sigma) { - if (Sigma.rows() != 6 || Sigma.cols() != 6) { - throw std::invalid_argument("Input measurement noise covariance must be 6x6"); - } - - // 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"); - } -} -/** - * - * @return 3X3 skey symmetric matrix when called - * Uses Rot3's Hat() to create skew-symmetric matrix - */ -Matrix3 Input::W() const { - return Rot3::Hat(w); -} - - -//======================================================================== -// Measurement Class Implementation -//======================================================================== -/** - * @brief Constructs measurement with directions and covariance. - * @param y_vec A 3D vector representing the measured direction in the sensor frame - * @param d_vec A 3D vector representing the known reference direction in the global frame aka ground truth direction - * @param Sigma 3x3 positive definite covariance vector representing the uncertainty in the measurements - * @param i Calibration index - A non-negative integer specifies the element in the calibration vector - * that corresponds to the sensor of interest. A value of -1 indicates that all the sensors have been calibrated - * - * Creates a measurement object that stores the measured direction(y), reference direction(d), measurement noise covariance(Sigma) - * and Calibration Index cal_idx - * - * Uses Eigen's SelfAdjointEigenSolver - * - */ - -Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i) - : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { - - // 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"); - } -} - -//======================================================================== -// State Class Implementation -//======================================================================== -/** - * - * @param R Rot3 (Attitude) - * @param b Vector (Bias) - * @param S Vector (Rot 3 calibration states) - * Combines the navigation and the calibration states together and provides a - * mechanism to represent the complete system - * - */ -State::State(const Rot3& R, const Vector3& b, const std::vector& S) - : R(R), b(b), S(S) {} - -/** - * - * @param n Number of Calibration states - * @return State object intitialized to identity - * Creates a default/ initial state - * Uses GTSAM's Rot3::identity and Vector3 zero function - */ -State State::identity(int n) { - std::vector calibrations(n, Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), calibrations); -} - -//======================================================================== -// Data Struct Implementation -//======================================================================== - -/** - * - * @param xi Ground Truth state - * @param n_cal Number of calibration states - * @param u Input measurements - * @param y Vector of y measurements - * @param n_meas number of measurements - * @param t timestamp - * @param dt time step - * Used to organize the state, input and measurement data with timestamps for - * testing Uses Rot3, Vector 3 and Unit3 classes - */ -Data::Data(const State& xi, int n_cal, const Input& u, - const std::vector& y, int n_meas, - double t, double dt) - : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} - -//======================================================================== -// Symmetry Group Implementation - Group Elements and actions -//======================================================================== -/** - * - * @param A Attitude element of Rot3 type - * @param a Matrix3 bias element - * @param B Rot3 vector containing calibration elements - * Ouptuts a G object using Rot3 for rotation representation - */ -G::G(const Rot3& A, const Matrix3& a, const std::vector& B) - : A(A), a(a), B(B) {} - -/** - * Defines the group operation (multiplication) - * @param other Another Group element - * @return G a product of two group elements - * Uses Rot3 Hat, Rot3 Vee for multiplication - * - */ -G G::operator*(const G& other) const { - if (B.size() != other.B.size()) { - throw std::invalid_argument("Group elements must have the same number of calibration elements"); - } - - std::vector new_B; - for (size_t i = 0; i < B.size(); i++) { - new_B.push_back(B[i] * other.B[i]); - } - - return G(A * other.A, - a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), - new_B); -} - -/** - * Used to compute the Group inverse - * @return The inverse of group element - * Uses Rot3 inverse, Rot3 matrix, hat and vee functions - * The left invariant property of the semi-direct product group structure is implemented here by using the -ve sign - */ -G G::inv() const { - Matrix3 A_inv = A.inverse().matrix(); - - std::vector B_inv; - for (const auto& b : B) { - B_inv.push_back(b.inverse()); - } - - return G(A.inverse(), - -Rot3::Hat(A_inv * Rot3::Vee(a)), - B_inv); -} - -/** - * Creates the identity element of the group - * @param n Number of calibration elements - * @return the identity element - * Uses Rot3 Identity and Matrix zero - */ -G G::identity(int n) { - std::vector B(n, Rot3::Identity()); - return G(Rot3::Identity(), Matrix3::Zero(), B); -} -/** - * Maps the tangent space elements to the group - * @param x Vector in lie algebra - * @return the group element G - * Uses Rot3 expmap and Expmapderivative function - */ -G G::exp(const Vector& x) { - if (x.size() < 6 || x.size() % 3 != 0) { - throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); - } - - int n = (x.size() - 6) / 3; - 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::vector B; - for (int i = 0; i < n; i++) { - B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); - } - - return G(A, a, B); -} - -//======================================================================== -// 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 - */ -Vector lift(const State& xi, const Input& u) { - int n = xi.S.size(); - 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 (int 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 - */ -State stateAction(const G& X, const State& xi) { - if (xi.S.size() != X.B.size()) { - throw std::invalid_argument("Number of calibration states and B elements must match"); - } - - std::vector new_S; - for (size_t i = 0; i < X.B.size(); i++) { - new_S.push_back(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 - */ -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 - */ -Vector3 outputAction(const G& X, const Direction& y, int idx) { - if (idx == -1) { - return X.A.inverse().matrix() * y.d.unitVector(); - } else { - if (idx >= static_cast(X.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return X.B[idx].inverse().matrix() * y.d.unitVector(); - } -} - -/** - * Maps the error states to vector representations through exponential - * coordinates - * @param e error state - * @return Vector with local coordinates - * Uses Rot3 logamo for mapping rotations to the tangent space - */ -Vector local_coords(const State& e) { - if (COORDINATE == "EXPONENTIAL") { - Vector eps(6 + 3 * e.S.size()); - - // First 3 elements - eps.head<3>() = Rot3::Logmap(e.R); - - // Next 3 elements - eps.segment<3>(3) = e.b; - - // Remaining elements - for (size_t i = 0; i < e.S.size(); i++) { - eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); - } - - return eps; - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} -/** - * Used to convert the vectorized errors back to state space - * @param eps Local coordinates in the exponential parameterization - * @return State object corresponding to these coordinates - * Uses Rot3 expmap through the G::exp() function - */ -State local_coords_inv(const Vector& eps) { - G X = G::exp(eps); - - if (COORDINATE == "EXPONENTIAL") { - std::vector S = X.B; - return State(X.A, eps.segment<3>(3), S); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} -/** - * 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 - */ -Matrix stateActionDiff(const State& xi) { - std::function coordsAction = - [&xi](const Vector& U) { - return local_coords(stateAction(G::exp(U), xi)); - }; - - Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); - 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() - */ -EqF::EqF(const Matrix& Sigma, int n, int m) - : __dof(6 + 3 * n), __n_cal(n), __X_hat(G::identity(n)), - __Sigma(Sigma), __xi_0(State::identity(n)) { - - 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 - */ -State EqF::stateEstimate() const { - return stateAction(__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 - */ -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 * Matrix3::Identity(), __n_cal)); - 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 - */ -void EqF::update(const Measurement& y) { - if (y.cal_idx > __n_cal) { - throw std::invalid_argument("Calibration index out of range"); - } - - // Get vector representations for checking - Vector3 y_vec = y.y.d.unitVector(); - Vector3 d_vec = y.d.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.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 - */ -Matrix EqF::__stateMatrixA(const Input& u) const { - Matrix3 W0 = velocityAction(__X_hat.inv(), u).W(); - Matrix A1 = Matrix::Zero(6, 6); - - if (COORDINATE == "EXPONENTIAL") { - A1.block<3, 3>(0, 3) = -Matrix3::Identity(); - A1.block<3, 3>(3, 3) = W0; - Matrix A2 = repBlock(W0, __n_cal); - return blockDiag(A1, A2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -/** - * Computes the discrete time state transition matrix - * @param u Angular velocity - * @param dt time step - * @return State transition matrix in discrete time - */ -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 * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); - Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; - - if (COORDINATE == "EXPONENTIAL") { - Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); - Phi1.block<3, 3>(0, 3) = Phi12; - Phi1.block<3, 3>(3, 3) = Phi22; - Matrix Phi2 = repBlock(Phi22, __n_cal); - return blockDiag(Phi1, Phi2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} -/** - * Computes the input uncertainty propagation matrix - * @return - * Uses the blockdiag matrix - */ -Matrix EqF::__inputMatrixBt() const { - if (COORDINATE == "EXPONENTIAL") { - Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix()); - Matrix B2; - - for (const auto& B : __X_hat.B) { - if (B2.size() == 0) { - B2 = B.matrix(); - } else { - B2 = blockDiag(B2, B.matrix()); - } - } - - return blockDiag(B1, B2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} -/** - * 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 - */ -Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * __n_cal); - - // 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.d.unitVector()); - } - - Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); - - // Create the combined matrix - Matrix temp(3, 6 + 3 * __n_cal); - temp.block<3, 3>(0, 0) = wedge_d; - temp.block<3, 3>(0, 3) = Matrix3::Zero(); - temp.block(0, 6, 3, 3 * __n_cal) = 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 - */ -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(__X_hat.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return __X_hat.B[idx].matrix(); - } else { - return __X_hat.A.matrix(); - } -} - -//======================================================================== -// Data Processing Functions Implementation -//======================================================================== - -/** - * @brief Loads the test data from the csv file - * @param filename path to the csv file is specified - * @param startRow First row to load based on csv, 0 by default - * @param maxRows maximum rows to load, defaults to all rows - * @param downsample Downsample factor, default 1 - * @return A list of data objects -*/ - - - -std::vector loadDataFromCSV(const std::string& filename, - int startRow, - int maxRows, - int downsample) { - std::vector data_list; - std::ifstream file(filename); - - if (!file.is_open()) { - throw std::runtime_error("Failed to open file: " + filename); - } - - std::cout << "Loading data from " << filename << "..." << std::flush; - - std::string line; - int lineNumber = 0; - int rowCount = 0; - int errorCount = 0; - double prevTime = 0.0; - - // Skip header - std::getline(file, line); - lineNumber++; - - // Skip to startRow - while (lineNumber < startRow && std::getline(file, line)) { - lineNumber++; - } - - // Read data - while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { - lineNumber++; - - // Apply downsampling - if ((lineNumber - startRow - 1) % downsample != 0) { - continue; - } - - std::istringstream ss(line); - std::string token; - std::vector values; - - // Parse line into values - while (std::getline(ss, token, ',')) { - try { - values.push_back(std::stod(token)); - } catch (const std::exception& e) { - errorCount++; - values.push_back(0.0); // Use default value - } - } - - // Check if we have enough values - if (values.size() < 39) { - errorCount++; - continue; - } - - // Extract values - double t = values[0]; - double dt = (rowCount == 0) ? 0.0 : t - prevTime; - prevTime = t; - - // Create ground truth state - Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z - Rot3 R = Rot3(quat); - - Vector3 b(values[5], values[6], values[7]); - - Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z - std::vector S = {Rot3(calQuat)}; - - State xi(R, b, S); - - // Create input - Vector3 w(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); - - // Create measurements - std::vector measurements; - - // First measurement (calibrated sensor, cal_idx = 0) - Vector3 y0(values[21], values[22], values[23]); - Vector3 d0(values[33], values[34], values[35]); - - // Normalize vectors if needed - if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); - if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); - - // Measurement covariance - Matrix3 covY0 = Matrix3::Zero(); - covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 - covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 - covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 - - // Create measurement - measurements.push_back(Measurement(y0, d0, covY0, 0)); - - // Second measurement (calibrated sensor, cal_idx = -1) - Vector3 y1(values[24], values[25], values[26]); - Vector3 d1(values[36], values[37], values[38]); - - // Normalize vectors if needed - if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); - if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); - - // Measurement covariance - Matrix3 covY1 = Matrix3::Zero(); - covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 - covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 - covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 - - // Create measurement - measurements.push_back(Measurement(y1, d1, covY1, -1)); - - // Create Data object and add to list - data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); - - rowCount++; - - // Show loading progress every 1000 rows - if (rowCount % 1000 == 0) { - std::cout << "." << std::flush; - } - } - - std::cout << " Done!" << std::endl; - std::cout << "Loaded " << data_list.size() << " data points"; - - if (errorCount > 0) { - std::cout << " (" << errorCount << " errors encountered)"; - } - - std::cout << std::endl; - - return data_list; -} -/** - * @brief Takes in the data and runs an EqF on it and reports the results - * @param filter Initialized EqF filter - * @param data_list std::vector - * @param printInterval Progress indicator - * Prints the performance statstics like average error etc - * Uses Rot3 between, logmap and rpy functions - */ -void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval) { - if (data_list.empty()) { - std::cerr << "No data to process" << std::endl; - return; - } - - std::cout << "Processing " << data_list.size() << " data points with EqF..." << std::endl; - - // Track performance metrics - std::vector att_errors; - std::vector bias_errors; - std::vector cal_errors; - - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); - - int totalMeasurements = 0; - int validMeasurements = 0; - - // Define constant for converting radians to degrees - const double RAD_TO_DEG = 180.0 / M_PI; - - // Print a progress indicator - int progressStep = data_list.size() / 10; // 10 progress updates - if (progressStep < 1) progressStep = 1; - - std::cout << "Progress: "; - - for (size_t i = 0; i < data_list.size(); i++) { - const Data& data = data_list[i]; - - // Propagate filter with current input and time step - filter.propagation(data.u, data.dt); - - // Process all measurements - for (const auto& y : data.y) { - totalMeasurements++; - - // Skip invalid measurements - if (y.y.hasNaN() || y.d.hasNaN()) { - continue; - } - - try { - filter.update(y); - validMeasurements++; - } catch (const std::exception& e) { - std::cerr << "Error updating at t=" << data.t - << ": " << e.what() << std::endl; - } - } - - // Get current state estimate - State estimate = filter.stateEstimate(); - - // 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(); - if (!data.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); - } - - // Store errors - att_errors.push_back(att_error.norm()); - bias_errors.push_back(bias_error.norm()); - cal_errors.push_back(cal_error.norm()); - - // Show progress dots - if (i % progressStep == 0) { - std::cout << "." << std::flush; - } - } - - std::cout << " Done!" << std::endl; - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end - start; - - // Calculate average errors - double avg_att_error = 0.0; - double avg_bias_error = 0.0; - double avg_cal_error = 0.0; - - if (!att_errors.empty()) { - avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); - avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); - avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); - } - - // Calculate final errors from last data point - const Data& final_data = data_list.back(); - State final_estimate = filter.stateEstimate(); - 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(); - 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])); - } - - // Print summary statistics - std::cout << "\n=== Filter Performance Summary ===" << std::endl; - std::cout << "Processing time: " << elapsed.count() << " seconds" << std::endl; - std::cout << "Processed measurements: " << totalMeasurements << " (valid: " << validMeasurements << ")" << std::endl; - - // Average errors - std::cout << "\n-- Average Errors --" << std::endl; - std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl; - std::cout << "Bias: " << avg_bias_error << std::endl; - std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°" << std::endl; - - // Final errors - std::cout << "\n-- Final Errors --" << std::endl; - std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°" << std::endl; - std::cout << "Bias: " << final_bias_error.norm() << std::endl; - std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°" << std::endl; - - // Print a brief comparison of final estimate vs ground truth - std::cout << "\n-- Final State vs Ground Truth --" << std::endl; - std::cout << "Attitude (RPY) - Estimate: " - << (final_estimate.R.rpy() * RAD_TO_DEG).transpose() << "° | Truth: " - << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; - std::cout << "Bias - Estimate: " << final_estimate.b.transpose() - << " | Truth: " << final_data.xi.b.transpose() << std::endl; - - if (!final_estimate.S.empty() && !final_data.xi.S.empty()) { - std::cout << "Calibration (RPY) - Estimate: " - << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose() << "° | Truth: " - << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; - } -} - -} // namespace abc_eqf_lib \ No newline at end of file diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h index 6f069020c..41cacbb19 100644 --- a/examples/ABC_EQF.h +++ b/examples/ABC_EQF.h @@ -46,38 +46,33 @@ extern const std::string COORDINATE; // Utility Functions //======================================================================== -/** - * Check if a vector is a unit vector - */ + +/// Check if a vector is a unit vector + bool checkNorm(const Vector3& x, double tol = 1e-3); -/** - * Check if vector contains NaN values - */ +/// Check if vector contains NaN values + bool hasNaN(const Vector3& vec); -/** - * Create a block diagonal matrix from two matrices - */ +/// 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 - */ +/// Repeat a block matrix n times along the diagonal + Matrix repBlock(const Matrix& A, int n); -/** - * Calculate numerical differential - */ +/// Calculate numerical differential + Matrix numericalDifferential(std::function f, const Vector& x); //======================================================================== // Core Data Types //======================================================================== -/** - * Direction class as a S2 element - */ +/// Direction class as a S2 element + class Direction { public: Unit3 d; // Direction (unit vector on S2) @@ -97,60 +92,42 @@ public: bool hasNaN() const; }; -/** - * Input class for the Biased Attitude System - */ -class Input { -public: - Vector3 w; // Angular velocity - Matrix Sigma; // Noise covariance +/// Input class for the Biased Attitude System - /** - * Initialize Input - * @param w Angular velocity (3-vector) - * @param Sigma Noise covariance (6x6 matrix) - */ - Input(const Vector3& w, const Matrix& Sigma); +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); + } + static Input create(const Vector3& w, const Matrix& Sigma) { /// Initialize w and Sigma + if (Sigma.rows() != 6 || Sigma.cols() != 6) { + throw std::invalid_argument("Input measurement noise covariance must be 6x6"); + } + /// 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"); + } - /** - * Return the Input as a skew-symmetric matrix - * @return w as a skew-symmetric matrix - */ - Matrix3 W() const; - - /** - * Return a random angular velocity - * @return A random angular velocity as Input element - */ - static Input random(); + return Input{w, Sigma}; // use brace initialization + } }; -/** - * Measurement class - * cal_idx is an index corresponding to the calibration related to the measurement - * cal_idx = -1 indicates the measurement is from a calibrated sensor - */ -class Measurement { -public: - Direction y; // Measurement direction in sensor frame - Direction d; // Known direction in global frame - Matrix3 Sigma; // Covariance matrix of the measurement - int cal_idx = -1; // Calibration index (-1 for calibrated sensor) +/// Measurement class - /** - * Initialize measurement - * @param y_vec Direction measurement in sensor frame - * @param d_vec Known direction in global frame - * @param Sigma Measurement noise covariance - * @param i Calibration index (-1 for calibrated sensor) - */ +struct Measurement { + Direction y; /// Measurement direction in sensor frame + Direction d; /// Known direction in global frame + Matrix3 Sigma; /// Covariance matrix of the measurement + int cal_idx = -1; /// Calibration index (-1 for calibrated sensor) Measurement(const Vector3& y_vec, const Vector3& d_vec, const Matrix3& Sigma, int i = -1); }; -/** - * State class representing the state of the Biased Attitude System - */ +/// State class representing the state of the Biased Attitude System + class State { public: Rot3 R; // Attitude rotation matrix R @@ -164,9 +141,8 @@ public: static State identity(int n); }; -/** - * Data structure for ground-truth, input and output data - */ +/// Data structure for ground-truth, input and output data + struct Data { State xi; // Ground-truth state int n_cal; // Number of calibration states @@ -305,25 +281,24 @@ Matrix stateActionDiff(const State& xi); // Equivariant Filter (EqF) //======================================================================== -/** - * Equivariant Filter (EqF) implementation - */ +/// Equivariant Filter (EqF) implementation + class EqF { private: - int __dof; // Degrees of freedom - int __n_cal; // Number of calibration states - 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 + int dof; // Degrees of freedom + int n_cal; // Number of calibration states + 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; + Matrix stateMatrixA(const Input& u) const; /** * Return the state transition matrix Phi (Equation 17) @@ -331,13 +306,13 @@ private: * @param dt Time step * @return State transition matrix Phi */ - Matrix __stateTransitionMatrix(const Input& u, double dt) const; + Matrix stateTransitionMatrix(const Input& u, double dt) const; /** * Return the Input matrix Bt * @return Input matrix Bt */ - Matrix __inputMatrixBt() const; + Matrix inputMatrixBt() const; /** * Return the measurement matrix C0 (Equation 14b) @@ -345,14 +320,14 @@ private: * @param idx Calibration index * @return Measurement matrix C0 */ - Matrix __measurementMatrixC(const Direction& d, int idx) const; + Matrix measurementMatrixC(const Direction& d, int idx) const; /** * Return the measurement output matrix Dt * @param idx Calibration index * @return Measurement output matrix Dt */ - Matrix __outputMatrixDt(int idx) const; + Matrix outputMatrixDt(int idx) const; public: /** @@ -383,43 +358,699 @@ public: void update(const Measurement& y); }; +// Global configuration +const std::string COORDINATE = "EXPONENTIAL"; // Denotes how the states are mapped to the vector representations + //======================================================================== -// Data Processing Functions +// Utility Functions +//======================================================================== +/** + * @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 + * Uses Vector3 norm() method to calculate vector magnitude + */ +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 + * Uses Matrix's rows(), cols(), setZero(), and block() methods + */ + +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 + * Recursively uses blockDiag() function + */ +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; +} + +/** + * @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; +} + +//======================================================================== +// Direction Class Implementation //======================================================================== /** - * Load data from CSV file into a vector of Data objects for the EqF - * - * CSV format: - * - t: Time - * - q_w, q_x, q_y, q_z: True attitude quaternion - * - b_x, b_y, b_z: True bias - * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion - * - w_x, w_y, w_z: Angular velocity measurements - * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations - * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations - * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements - * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations - * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions - * - * @param filename Path to the CSV file - * @param startRow First row to load (default: 0) - * @param maxRows Maximum number of rows to load (default: all) - * @param downsample Downsample factor (default: 1, which means no downsampling) - * @return Vector of Data objects + * @brief Initializes a direction object vector from a provided 3D vector input + * @param d_vec A 3-D vector that should have a unit norm(This represents a + * direction in 3D space) Uses Unit3's constructor which normalizes vectors */ -std::vector loadDataFromCSV(const std::string& filename, - int startRow = 0, - int maxRows = -1, - int downsample = 1); + + +Direction::Direction(const Vector3& d_vec) : d(d_vec) { + if (!checkNorm(d_vec)) { + throw std::invalid_argument("Direction must be a unit vector"); + } +} + /** Access the individual components of the direction vector defined above using this methods below + * Uses the Unit3 object from GTSAM to compute the components + */ + +double Direction::x() const { + return d.unitVector()[0]; +} + +double Direction::y() const { + return d.unitVector()[1]; +} + +double Direction::z() const { + return d.unitVector()[2]; +} + +bool Direction::hasNaN() const { + Vector3 vec = d.unitVector(); + return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); +} + +//======================================================================== +// Input Class Implementation +//======================================================================== +/** + * @brief Constructs an input object using the Angular velocity vector and the + * covariance matrix + * @param w Angular vector + * @param Sigma 6X6 covariance matrix + * Uses Matrix's rows(), cols() and Eigen's SelfAdjointEigenSolver + */ +// Input::Input(const Vector3& w, const Matrix& Sigma) : w(w), Sigma(Sigma) { +// if (Sigma.rows() != 6 || Sigma.cols() != 6) { +// throw std::invalid_argument("Input measurement noise covariance must be 6x6"); +// } +// +// // 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"); +// } +// } +/** + * + * @return 3X3 skey symmetric matrix when called + * Uses Rot3's Hat() to create skew-symmetric matrix + */ +// Matrix3 Input::W() const { +// return Rot3::Hat(w); +// } + + +//======================================================================== +// Measurement Class Implementation +//======================================================================== +/** + * @brief Constructs measurement with directions and covariance. + * @param y_vec A 3D vector representing the measured direction in the sensor frame + * @param d_vec A 3D vector representing the known reference direction in the global frame aka ground truth direction + * @param Sigma 3x3 positive definite covariance vector representing the uncertainty in the measurements + * @param i Calibration index - A non-negative integer specifies the element in the calibration vector + * that corresponds to the sensor of interest. A value of -1 indicates that all the sensors have been calibrated + * + * Creates a measurement object that stores the measured direction(y), reference direction(d), measurement noise covariance(Sigma) + * and Calibration Index cal_idx + * + * Uses Eigen's SelfAdjointEigenSolver + * + */ + +Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i) + : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { + + // 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"); + } +} + +//======================================================================== +// State Class Implementation +//======================================================================== +/** + * + * @param R Rot3 (Attitude) + * @param b Vector (Bias) + * @param S Vector (Rot 3 calibration states) + * Combines the navigation and the calibration states together and provides a + * mechanism to represent the complete system + * + */ +State::State(const Rot3& R, const Vector3& b, const std::vector& S) + : R(R), b(b), S(S) {} /** - * Process data with EqF and print summary results - * @param filter Initialized EqF filter - * @param data_list Vector of Data objects to process - * @param printInterval Progress indicator interval (used internally) + * + * @param n Number of Calibration states + * @return State object intitialized to identity + * Creates a default/ initial state + * Uses GTSAM's Rot3::identity and Vector3 zero function */ -void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval = 10); +State State::identity(int n) { + std::vector calibrations(n, Rot3::Identity()); + return State(Rot3::Identity(), Vector3::Zero(), calibrations); +} + +//======================================================================== +// Data Struct Implementation +//======================================================================== + +/** + * + * @param xi Ground Truth state + * @param n_cal Number of calibration states + * @param u Input measurements + * @param y Vector of y measurements + * @param n_meas number of measurements + * @param t timestamp + * @param dt time step + * Used to organize the state, input and measurement data with timestamps for + * testing Uses Rot3, Vector 3 and Unit3 classes + */ +Data::Data(const State& xi, int n_cal, const Input& u, + const std::vector& y, int n_meas, + double t, double dt) + : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} + +//======================================================================== +// Symmetry Group Implementation - Group Elements and actions +//======================================================================== +/** + * + * @param A Attitude element of Rot3 type + * @param a Matrix3 bias element + * @param B Rot3 vector containing calibration elements + * Ouptuts a G object using Rot3 for rotation representation + */ +G::G(const Rot3& A, const Matrix3& a, const std::vector& B) + : A(A), a(a), B(B) {} + +/** + * Defines the group operation (multiplication) + * @param other Another Group element + * @return G a product of two group elements + * Uses Rot3 Hat, Rot3 Vee for multiplication + * + */ +G G::operator*(const G& other) const { + if (B.size() != other.B.size()) { + throw std::invalid_argument("Group elements must have the same number of calibration elements"); + } + + std::vector new_B; + for (size_t i = 0; i < B.size(); i++) { + new_B.push_back(B[i] * other.B[i]); + } + + return G(A * other.A, + a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), + new_B); +} + +/** + * Used to compute the Group inverse + * @return The inverse of group element + * Uses Rot3 inverse, Rot3 matrix, hat and vee functions + * The left invariant property of the semi-direct product group structure is implemented here by using the -ve sign + */ +G G::inv() const { + Matrix3 A_inv = A.inverse().matrix(); + + std::vector B_inv; + for (const auto& b : B) { + B_inv.push_back(b.inverse()); + } + + return G(A.inverse(), + -Rot3::Hat(A_inv * Rot3::Vee(a)), + B_inv); +} + +/** + * Creates the identity element of the group + * @param n Number of calibration elements + * @return the identity element + * Uses Rot3 Identity and Matrix zero + */ +G G::identity(int n) { + std::vector B(n, Rot3::Identity()); + return G(Rot3::Identity(), Matrix3::Zero(), B); +} +/** + * Maps the tangent space elements to the group + * @param x Vector in lie algebra + * @return the group element G + * Uses Rot3 expmap and Expmapderivative function + */ +G G::exp(const Vector& x) { + if (x.size() < 6 || x.size() % 3 != 0) { + throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); + } + + int n = (x.size() - 6) / 3; + 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::vector B; + for (int i = 0; i < n; i++) { + B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); + } + + return G(A, a, B); +} + +//======================================================================== +// 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 + */ +Vector lift(const State& xi, const Input& u) { + int n = xi.S.size(); + 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 (int 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 + */ +State stateAction(const G& X, const State& xi) { + if (xi.S.size() != X.B.size()) { + throw std::invalid_argument("Number of calibration states and B elements must match"); + } + + std::vector new_S; + for (size_t i = 0; i < X.B.size(); i++) { + new_S.push_back(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 + */ +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 + */ +Vector3 outputAction(const G& X, const Direction& y, int idx) { + if (idx == -1) { + return X.A.inverse().matrix() * y.d.unitVector(); + } else { + if (idx >= static_cast(X.B.size())) { + throw std::out_of_range("Calibration index out of range"); + } + return X.B[idx].inverse().matrix() * y.d.unitVector(); + } +} + +/** + * Maps the error states to vector representations through exponential + * coordinates + * @param e error state + * @return Vector with local coordinates + * Uses Rot3 logamo for mapping rotations to the tangent space + */ +Vector local_coords(const State& e) { + if (COORDINATE == "EXPONENTIAL") { + Vector eps(6 + 3 * e.S.size()); + + // First 3 elements + eps.head<3>() = Rot3::Logmap(e.R); + + // Next 3 elements + eps.segment<3>(3) = e.b; + + // Remaining elements + for (size_t i = 0; i < e.S.size(); i++) { + eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); + } + + return eps; + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * Used to convert the vectorized errors back to state space + * @param eps Local coordinates in the exponential parameterization + * @return State object corresponding to these coordinates + * Uses Rot3 expmap through the G::exp() function + */ +State local_coords_inv(const Vector& eps) { + G X = G::exp(eps); + + if (COORDINATE == "EXPONENTIAL") { + std::vector S = X.B; + return State(X.A, eps.segment<3>(3), S); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * 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 + */ +Matrix stateActionDiff(const State& xi) { + std::function coordsAction = + [&xi](const Vector& U) { + return local_coords(stateAction(G::exp(U), xi)); + }; + + Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); + 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() + */ +EqF::EqF(const Matrix& Sigma, int n, int m) + : dof(6 + 3 * n), n_cal(n), X_hat(G::identity(n)), + Sigma(Sigma), xi_0(State::identity(n)) { + + 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 + */ +State EqF::stateEstimate() const { + return stateAction(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 + */ +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 * Matrix3::Identity(), n_cal)); + 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 + */ +void EqF::update(const Measurement& y) { + if (y.cal_idx > n_cal) { + throw std::invalid_argument("Calibration index out of range"); + } + + // Get vector representations for checking + Vector3 y_vec = y.y.d.unitVector(); + Vector3 d_vec = y.d.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.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 + */ +Matrix EqF::stateMatrixA(const Input& u) const { + Matrix3 W0 = velocityAction(X_hat.inv(), u).W(); + Matrix A1 = Matrix::Zero(6, 6); + + if (COORDINATE == "EXPONENTIAL") { + A1.block<3, 3>(0, 3) = -Matrix3::Identity(); + A1.block<3, 3>(3, 3) = W0; + Matrix A2 = repBlock(W0, n_cal); + return blockDiag(A1, A2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} + +/** + * Computes the discrete time state transition matrix + * @param u Angular velocity + * @param dt time step + * @return State transition matrix in discrete time + */ +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 * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); + Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; + + if (COORDINATE == "EXPONENTIAL") { + Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + Matrix Phi2 = repBlock(Phi22, n_cal); + return blockDiag(Phi1, Phi2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * Computes the input uncertainty propagation matrix + * @return + * Uses the blockdiag matrix + */ +Matrix EqF::inputMatrixBt() const { + if (COORDINATE == "EXPONENTIAL") { + Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); + Matrix B2; + + for (const auto& B : X_hat.B) { + if (B2.size() == 0) { + B2 = B.matrix(); + } else { + B2 = blockDiag(B2, B.matrix()); + } + } + + return blockDiag(B1, B2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * 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 + */ +Matrix EqF::measurementMatrixC(const Direction& d, int idx) const { + Matrix Cc = Matrix::Zero(3, 3 * n_cal); + + // 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.d.unitVector()); + } + + Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); + + // Create the combined matrix + Matrix temp(3, 6 + 3 * n_cal); + temp.block<3, 3>(0, 0) = wedge_d; + temp.block<3, 3>(0, 3) = Matrix3::Zero(); + temp.block(0, 6, 3, 3 * n_cal) = 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 + */ +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(X_hat.B.size())) { + 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 diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp index 6b0f4389d..a2267c3ac 100644 --- a/examples/ABC_EQF_Demo.cpp +++ b/examples/ABC_EQF_Demo.cpp @@ -15,6 +15,346 @@ using namespace abc_eqf_lib; using namespace gtsam; +//======================================================================== +// Data Processing Functions +//======================================================================== + +/** + * Load data from CSV file into a vector of Data objects for the EqF + * + * CSV format: + * - t: Time + * - q_w, q_x, q_y, q_z: True attitude quaternion + * - b_x, b_y, b_z: True bias + * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion + * - w_x, w_y, w_z: Angular velocity measurements + * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations + * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations + * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements + * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations + * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions + * + * @param filename Path to the CSV file + * @param startRow First row to load (default: 0) + * @param maxRows Maximum number of rows to load (default: all) + * @param downsample Downsample factor (default: 1, which means no downsampling) + * @return Vector of Data objects + */ +std::vector loadDataFromCSV(const std::string& filename, + int startRow = 0, + int maxRows = -1, + int downsample = 1); + +/** + * Process data with EqF and print summary results + * @param filter Initialized EqF filter + * @param data_list Vector of Data objects to process + * @param printInterval Progress indicator interval (used internally) + */ +void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval = 10); + +//======================================================================== +// Data Processing Functions Implementation +//======================================================================== + +/** + * @brief Loads the test data from the csv file + * @param filename path to the csv file is specified + * @param startRow First row to load based on csv, 0 by default + * @param maxRows maximum rows to load, defaults to all rows + * @param downsample Downsample factor, default 1 + * @return A list of data objects +*/ + + + +std::vector loadDataFromCSV(const std::string& filename, + int startRow, + int maxRows, + int downsample) { + std::vector data_list; + std::ifstream file(filename); + + if (!file.is_open()) { + throw std::runtime_error("Failed to open file: " + filename); + } + + std::cout << "Loading data from " << filename << "..." << std::flush; + + std::string line; + int lineNumber = 0; + int rowCount = 0; + int errorCount = 0; + double prevTime = 0.0; + + // Skip header + std::getline(file, line); + lineNumber++; + + // Skip to startRow + while (lineNumber < startRow && std::getline(file, line)) { + lineNumber++; + } + + // Read data + while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { + lineNumber++; + + // Apply downsampling + if ((lineNumber - startRow - 1) % downsample != 0) { + continue; + } + + std::istringstream ss(line); + std::string token; + std::vector values; + + // Parse line into values + while (std::getline(ss, token, ',')) { + try { + values.push_back(std::stod(token)); + } catch (const std::exception& e) { + errorCount++; + values.push_back(0.0); // Use default value + } + } + + // Check if we have enough values + if (values.size() < 39) { + errorCount++; + continue; + } + + // Extract values + double t = values[0]; + double dt = (rowCount == 0) ? 0.0 : t - prevTime; + prevTime = t; + + // Create ground truth state + Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z + Rot3 R = Rot3(quat); + + Vector3 b(values[5], values[6], values[7]); + + Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z + std::vector S = {Rot3(calQuat)}; + + State xi(R, b, S); + + // Create input + Vector3 w(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}; + + // Create measurements + std::vector measurements; + + // First measurement (calibrated sensor, cal_idx = 0) + Vector3 y0(values[21], values[22], values[23]); + Vector3 d0(values[33], values[34], values[35]); + + // Normalize vectors if needed + if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); + if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); + + // Measurement covariance + Matrix3 covY0 = Matrix3::Zero(); + covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 + covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 + covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 + + // Create measurement + measurements.push_back(Measurement(y0, d0, covY0, 0)); + + // Second measurement (calibrated sensor, cal_idx = -1) + Vector3 y1(values[24], values[25], values[26]); + Vector3 d1(values[36], values[37], values[38]); + + // Normalize vectors if needed + if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); + if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); + + // Measurement covariance + Matrix3 covY1 = Matrix3::Zero(); + covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 + covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 + covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 + + // Create measurement + measurements.push_back(Measurement(y1, d1, covY1, -1)); + + // Create Data object and add to list + data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); + + rowCount++; + + // Show loading progress every 1000 rows + if (rowCount % 1000 == 0) { + std::cout << "." << std::flush; + } + } + + std::cout << " Done!" << std::endl; + std::cout << "Loaded " << data_list.size() << " data points"; + + if (errorCount > 0) { + std::cout << " (" << errorCount << " errors encountered)"; + } + + std::cout << std::endl; + + return data_list; +} +/** + * @brief Takes in the data and runs an EqF on it and reports the results + * @param filter Initialized EqF filter + * @param data_list std::vector + * @param printInterval Progress indicator + * Prints the performance statstics like average error etc + * Uses Rot3 between, logmap and rpy functions + */ +void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval) { + if (data_list.empty()) { + std::cerr << "No data to process" << std::endl; + return; + } + + std::cout << "Processing " << data_list.size() << " data points with EqF..." << std::endl; + + // Track performance metrics + std::vector att_errors; + std::vector bias_errors; + std::vector cal_errors; + + // Track time for performance measurement + auto start = std::chrono::high_resolution_clock::now(); + + int totalMeasurements = 0; + int validMeasurements = 0; + + // Define constant for converting radians to degrees + const double RAD_TO_DEG = 180.0 / M_PI; + + // Print a progress indicator + int progressStep = data_list.size() / 10; // 10 progress updates + if (progressStep < 1) progressStep = 1; + + std::cout << "Progress: "; + + for (size_t i = 0; i < data_list.size(); i++) { + const Data& data = data_list[i]; + + // Propagate filter with current input and time step + filter.propagation(data.u, data.dt); + + // Process all measurements + for (const auto& y : data.y) { + totalMeasurements++; + + // Skip invalid measurements + if (y.y.hasNaN() || y.d.hasNaN()) { + continue; + } + + try { + filter.update(y); + validMeasurements++; + } catch (const std::exception& e) { + std::cerr << "Error updating at t=" << data.t + << ": " << e.what() << std::endl; + } + } + + // Get current state estimate + State estimate = filter.stateEstimate(); + + // 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(); + if (!data.xi.S.empty() && !estimate.S.empty()) { + cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); + } + + // Store errors + att_errors.push_back(att_error.norm()); + bias_errors.push_back(bias_error.norm()); + cal_errors.push_back(cal_error.norm()); + + // Show progress dots + if (i % progressStep == 0) { + std::cout << "." << std::flush; + } + } + + std::cout << " Done!" << std::endl; + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = end - start; + + // Calculate average errors + double avg_att_error = 0.0; + double avg_bias_error = 0.0; + double avg_cal_error = 0.0; + + if (!att_errors.empty()) { + avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); + avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); + avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); + } + + // Calculate final errors from last data point + const Data& final_data = data_list.back(); + State final_estimate = filter.stateEstimate(); + 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(); + 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])); + } + + // Print summary statistics + std::cout << "\n=== Filter Performance Summary ===" << std::endl; + std::cout << "Processing time: " << elapsed.count() << " seconds" << std::endl; + std::cout << "Processed measurements: " << totalMeasurements << " (valid: " << validMeasurements << ")" << std::endl; + + // Average errors + std::cout << "\n-- Average Errors --" << std::endl; + std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl; + std::cout << "Bias: " << avg_bias_error << std::endl; + std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°" << std::endl; + + // Final errors + std::cout << "\n-- Final Errors --" << std::endl; + std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°" << std::endl; + std::cout << "Bias: " << final_bias_error.norm() << std::endl; + std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°" << std::endl; + + // Print a brief comparison of final estimate vs ground truth + std::cout << "\n-- Final State vs Ground Truth --" << std::endl; + std::cout << "Attitude (RPY) - Estimate: " + << (final_estimate.R.rpy() * RAD_TO_DEG).transpose() << "° | Truth: " + << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; + std::cout << "Bias - Estimate: " << final_estimate.b.transpose() + << " | Truth: " << final_data.xi.b.transpose() << std::endl; + + if (!final_estimate.S.empty() && !final_data.xi.S.empty()) { + std::cout << "Calibration (RPY) - Estimate: " + << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose() << "° | Truth: " + << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; + } +} + /** * Main function for the EqF demo * @param argc Number of arguments diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index 5f9f7ce23..8a577c73f 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5...3.22) +cmake_minimum_required(VERSION 3.5) project(METIS) # Add flags for currect directory and below @@ -65,4 +65,4 @@ if (TARGET metis AND GKlib_COPTIONS) separate_arguments(GKlib_COPTIONS) # Declare those flags as to-be-imported in "client libraries", i.e. "gtsam" target_compile_options(metis INTERFACE ${GKlib_COPTIONS}) -endif() +endif() \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 7f3183aa2..a5a6f19af 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -86,10 +86,7 @@ endforeach(subdir) # To add additional sources to gtsam when building the full library (static or shared) # append the subfolder with _srcs appended to the end to this list -set(gtsam_srcs ${3rdparty_srcs} - ../examples/ABC_EQF_Demo.cpp - ../examples/ABC_EQF.cpp - ../examples/ABC_EQF.h) +set(gtsam_srcs ${3rdparty_srcs}) foreach(subdir ${gtsam_subdirs}) list(APPEND gtsam_srcs ${${subdir}_srcs}) endforeach(subdir) From 925e94ecc22bf5205b03f6c52bbb6697c79f3610 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Sun, 20 Apr 2025 21:41:44 -0700 Subject: [PATCH 09/38] Resolved the direction class -> Unit3, and local_coords, local_inv functions to be inline with the state class --- examples/ABC_EQF.h | 205 ++++++++++++++++++-------------------- examples/ABC_EQF_Demo.cpp | 7 +- 2 files changed, 102 insertions(+), 110 deletions(-) diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h index 41cacbb19..341149cd3 100644 --- a/examples/ABC_EQF.h +++ b/examples/ABC_EQF.h @@ -71,26 +71,7 @@ Matrix numericalDifferential(std::function f, const Vecto // Core Data Types //======================================================================== -/// Direction class as a S2 element -class Direction { -public: - Unit3 d; // Direction (unit vector on S2) - - /** - * Initialize direction - * @param d_vec Direction vector (must be unit norm) - */ - Direction(const Vector3& d_vec); - - // Accessor methods for vector components - double x() const; - double y() const; - double z() const; - - // Check if the direction contains NaN values - bool hasNaN() const; -}; /// Input class for the Biased Attitude System @@ -118,8 +99,8 @@ struct Input { /// Measurement class struct Measurement { - Direction y; /// Measurement direction in sensor frame - Direction d; /// Known direction in global frame + 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) Measurement(const Vector3& y_vec, const Vector3& d_vec, @@ -138,6 +119,53 @@ public: const Vector3& b = Vector3::Zero(), const std::vector& S = std::vector()); + /** + * 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 * S.size()); + + // 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 < S.size(); 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() < 6 || v.size() % 3 != 0 || v.size() != 6 + 3 * static_cast(S.size())) { + throw std::invalid_argument("Vector size does not match state dimensions"); + } + + // Modify attitude + Rot3 newR = R * Rot3::Expmap(v.head<3>()); + + // Modify bias + Vector3 newB = b + v.segment<3>(3); + + // Modify calibrations + std::vector newS; + for (size_t i = 0; i < S.size(); i++) { + newS.push_back(S[i] * Rot3::Expmap(v.segment<3>(6 + 3*i))); + } + + return State(newR, newB, newS); + } + static State identity(int n); }; @@ -254,21 +282,7 @@ Input velocityAction(const G& X, const Input& u); * @param idx Calibration index * @return New direction after group action */ -Vector3 outputAction(const G& X, const Direction& y, int idx); - -/** - * Local coordinates assuming xi_0 = identity (Equation 9) - * @param e State representing equivariant error - * @return Local coordinates - */ -Vector local_coords(const State& e); - -/** - * Local coordinates inverse assuming xi_0 = identity - * @param eps Local coordinates - * @return Corresponding state - */ -State local_coords_inv(const Vector& eps); +Vector3 outputAction(const G& X, const Unit3& y, int idx); /** * Differential of the phi action at E = Id in local coordinates @@ -320,7 +334,7 @@ private: * @param idx Calibration index * @return Measurement matrix C0 */ - Matrix measurementMatrixC(const Direction& d, int idx) const; + Matrix measurementMatrixC(const Unit3& d, int idx) const; /** * Return the measurement output matrix Dt @@ -460,33 +474,6 @@ Matrix numericalDifferential(std::function f, const Vecto * direction in 3D space) Uses Unit3's constructor which normalizes vectors */ - -Direction::Direction(const Vector3& d_vec) : d(d_vec) { - if (!checkNorm(d_vec)) { - throw std::invalid_argument("Direction must be a unit vector"); - } -} - /** Access the individual components of the direction vector defined above using this methods below - * Uses the Unit3 object from GTSAM to compute the components - */ - -double Direction::x() const { - return d.unitVector()[0]; -} - -double Direction::y() const { - return d.unitVector()[1]; -} - -double Direction::z() const { - return d.unitVector()[2]; -} - -bool Direction::hasNaN() const { - Vector3 vec = d.unitVector(); - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); -} - //======================================================================== // Input Class Implementation //======================================================================== @@ -758,14 +745,14 @@ Input velocityAction(const G& X, const Input& u) { * @return Transformed direction * Uses Rot3 inverse, matric and Unit3 unitvector functions */ -Vector3 outputAction(const G& X, const Direction& y, int idx) { +Vector3 outputAction(const G& X, const Unit3& y, int idx) { if (idx == -1) { - return X.A.inverse().matrix() * y.d.unitVector(); + return X.A.inverse().matrix() * y.unitVector(); } else { if (idx >= static_cast(X.B.size())) { throw std::out_of_range("Calibration index out of range"); } - return X.B[idx].inverse().matrix() * y.d.unitVector(); + return X.B[idx].inverse().matrix() * y.unitVector(); } } @@ -776,46 +763,46 @@ Vector3 outputAction(const G& X, const Direction& y, int idx) { * @return Vector with local coordinates * Uses Rot3 logamo for mapping rotations to the tangent space */ -Vector local_coords(const State& e) { - if (COORDINATE == "EXPONENTIAL") { - Vector eps(6 + 3 * e.S.size()); - - // First 3 elements - eps.head<3>() = Rot3::Logmap(e.R); - - // Next 3 elements - eps.segment<3>(3) = e.b; - - // Remaining elements - for (size_t i = 0; i < e.S.size(); i++) { - eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); - } - - return eps; - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} +// Vector local_coords(const State& e) { +// if (COORDINATE == "EXPONENTIAL") { +// Vector eps(6 + 3 * e.S.size()); +// +// // First 3 elements +// eps.head<3>() = Rot3::Logmap(e.R); +// +// // Next 3 elements +// eps.segment<3>(3) = e.b; +// +// // Remaining elements +// for (size_t i = 0; i < e.S.size(); i++) { +// eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); +// } +// +// return eps; +// } else if (COORDINATE == "NORMAL") { +// throw std::runtime_error("Normal coordinate representation is not implemented yet"); +// } else { +// throw std::invalid_argument("Invalid coordinate representation"); +// } +// } /** * Used to convert the vectorized errors back to state space * @param eps Local coordinates in the exponential parameterization * @return State object corresponding to these coordinates * Uses Rot3 expmap through the G::exp() function */ -State local_coords_inv(const Vector& eps) { - G X = G::exp(eps); - - if (COORDINATE == "EXPONENTIAL") { - std::vector S = X.B; - return State(X.A, eps.segment<3>(3), S); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} +// State local_coords_inv(const Vector& eps) { +// G X = G::exp(eps); +// +// if (COORDINATE == "EXPONENTIAL") { +// std::vector S = X.B; +// return State(X.A, eps.segment<3>(3), S); +// } else if (COORDINATE == "NORMAL") { +// throw std::runtime_error("Normal coordinate representation is not implemented yet"); +// } else { +// throw std::invalid_argument("Invalid coordinate representation"); +// } +// } /** * Computes the differential of a state action at the identity of the symmetry * group @@ -827,7 +814,9 @@ State local_coords_inv(const Vector& eps) { Matrix stateActionDiff(const State& xi) { std::function coordsAction = [&xi](const Vector& U) { - return local_coords(stateAction(G::exp(U), xi)); + G groupElement = G::exp(U); + State transformed = stateAction(groupElement, xi); + return xi.localCoordinates(transformed); }; Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); @@ -914,8 +903,8 @@ void EqF::update(const Measurement& y) { } // Get vector representations for checking - Vector3 y_vec = y.y.d.unitVector(); - Vector3 d_vec = y.d.d.unitVector(); + 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]) || @@ -925,7 +914,7 @@ void EqF::update(const Measurement& y) { 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.d.unitVector()) * action_result; + 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(); @@ -1013,16 +1002,16 @@ Matrix EqF::inputMatrixBt() const { * @return Measurement matrix * Uses the matrix zero, Rot3 hat and the Unitvector functions */ -Matrix EqF::measurementMatrixC(const Direction& d, int idx) const { +Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const { Matrix Cc = Matrix::Zero(3, 3 * n_cal); // 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.d.unitVector()); + Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.unitVector()); } - Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); + Matrix3 wedge_d = Rot3::Hat(d.unitVector()); // Create the combined matrix Matrix temp(3, 6 + 3 * n_cal); diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp index a2267c3ac..e90283873 100644 --- a/examples/ABC_EQF_Demo.cpp +++ b/examples/ABC_EQF_Demo.cpp @@ -262,8 +262,11 @@ void processDataWithEqF(EqF& filter, const std::vector& data_list, int pri totalMeasurements++; // Skip invalid measurements - if (y.y.hasNaN() || y.d.hasNaN()) { - continue; + Vector3 y_vec = y.y.unitVector(); + Vector3 d_vec = y.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])) { + continue; } try { From 51e20eca5841615fcbf407dbd3e0e61764fcf04a Mon Sep 17 00:00:00 2001 From: jenniferoum Date: Mon, 21 Apr 2025 20:24:22 -0700 Subject: [PATCH 10/38] Inline measurement, G, and State functions, use brace initialization --- examples/ABC_EQF.h | 357 ++++++++++---------------------------- examples/ABC_EQF_Demo.cpp | 6 +- 2 files changed, 92 insertions(+), 271 deletions(-) diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h index 341149cd3..4c935962c 100644 --- a/examples/ABC_EQF.h +++ b/examples/ABC_EQF.h @@ -73,7 +73,7 @@ Matrix numericalDifferential(std::function f, const Vecto -/// Input class for the Biased Attitude System +/// Input struct for the Biased Attitude System struct Input { Vector3 w; /// Angular velocity (3-vector) @@ -96,15 +96,22 @@ struct Input { } }; -/// Measurement class +/// 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) - Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i = -1); + static Measurement create(const Unit3& y_vec, const Unit3& d_vec, /// Initialize measurement + const Matrix3& Sigma_in, int i) { + /// Check positive semi-definite + Eigen::SelfAdjointEigenSolver eigensolver(Sigma_in); + if (eigensolver.eigenvalues().minCoeff() < -1e-10) { + throw std::invalid_argument("Covariance matrix must be semi-positive definite"); + } + return Measurement{y_vec, Unit3(d_vec), Sigma_in, i}; // Brace initialization + } }; /// State class representing the state of the Biased Attitude System @@ -115,10 +122,17 @@ public: Vector3 b; // Gyroscope bias b std::vector S; // Sensor calibrations S + /// Constructor State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(), - const std::vector& S = std::vector()); + const std::vector& S = std::vector()) + : R(R), b(b), S(S) {} + /// Identity function + static State identity(int n) { + std::vector calibrations(n, Rot3::Identity()); + return State(Rot3::Identity(), Vector3::Zero(), calibrations); + } /** * Compute Local coordinates in the state relative to another state. * @param other The other state @@ -165,34 +179,24 @@ public: return State(newR, newB, newS); } - - static State identity(int n); }; /// Data structure for ground-truth, input and output data struct Data { - State xi; // Ground-truth state - int n_cal; // Number of calibration states - Input u; // Input measurements - std::vector y; // Output measurements - int n_meas; // Number of measurements - double t; // Time - double dt; // Time step + State xi; /// Ground-truth state + int n_cal; /// Number of calibration states + Input u; /// Input measurements + std::vector y; /// Output measurements + int n_meas; /// Number of measurements + double t; /// Time + double dt; /// Time step - /** - * Initialize Data - * @param xi Ground-truth state - * @param n_cal Number of calibration states - * @param u Input measurements - * @param y Output measurements - * @param n_meas Number of measurements - * @param t Time - * @param dt Time step - */ - Data(const State& xi, int n_cal, const Input& u, - const std::vector& y, int n_meas, - double t, double dt); + static Data create(const State& xi, int n_cal, const Input& u, /// Initialize Data + const std::vector& y, int n_meas, + double t, double dt) { + return Data{xi, n_cal, u, y, n_meas, t, dt}; /// Bracket initialization + } }; //======================================================================== @@ -205,46 +209,70 @@ struct Data { */ class G { public: - Rot3 A; // First SO(3) element - Matrix3 a; // so(3) element (skew-symmetric matrix) - std::vector B; // List of SO(3) elements for calibration + Rot3 A; /// First SO(3) element + Matrix3 a; /// so(3) element (skew-symmetric matrix) + std::vector B; /// List of SO(3) elements for calibration - /** - * Initialize the symmetry group G - * @param A SO3 element - * @param a so(3) element (skew symmetric matrix) - * @param B list of SO3 elements - */ + /// Initialize the symmetry group G G(const Rot3& A = Rot3::Identity(), - const Matrix3& a = Matrix3::Zero(), - const std::vector& B = std::vector()); + const Matrix3& a = Matrix3::Zero(), + const std::vector& B = std::vector()) + : A(A), a(a), B(B) {} - /** - * Define the group operation (multiplication) - * @param other Another group element - * @return The product of this and other - */ - G operator*(const G& other) const; + /// Group multiplication + G operator*(const G& other) const { + if (B.size() != other.B.size()) { + throw std::invalid_argument("Group elements must have the same number of calibration elements"); + } - /** - * Return the inverse element of the symmetry group - * @return The inverse of this group element - */ - G inv() const; + std::vector new_B; + for (size_t i = 0; i < B.size(); i++) { + new_B.push_back(B[i] * other.B[i]); + } - /** - * Return the identity of the symmetry group - * @param n Number of calibration elements - * @return The identity element with n calibration components - */ - static G identity(int n); + return G(A * other.A, + a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), + new_B); + } - /** - * Return a group element X given by X = exp(x) - * @param x Vector representation of Lie algebra element - * @return Group element given by the exponential of x - */ - static G exp(const Vector& x); + /// Group inverse + G inv() const { + Matrix3 A_inv = A.inverse().matrix(); + std::vector B_inv; + for (const auto& b : B) { + B_inv.push_back(b.inverse()); + } + + return G(A.inverse(), + -Rot3::Hat(A_inv * Rot3::Vee(a)), + B_inv); + } + + /// Identity element + static G identity(int n) { + std::vector B(n, 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() < 6 || x.size() % 3 != 0) { + throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); + } + + int n = (x.size() - 6) / 3; + 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::vector B; + for (int i = 0; i < n; i++) { + B.push_back(Rot3::Expmap(x.segment<3>(6 + 3 * i))); + } + + return G(A, a, B); + } }; //======================================================================== @@ -464,213 +492,6 @@ Matrix numericalDifferential(std::function f, const Vecto return Df; } -//======================================================================== -// Direction Class Implementation -//======================================================================== - -/** - * @brief Initializes a direction object vector from a provided 3D vector input - * @param d_vec A 3-D vector that should have a unit norm(This represents a - * direction in 3D space) Uses Unit3's constructor which normalizes vectors - */ - -//======================================================================== -// Input Class Implementation -//======================================================================== -/** - * @brief Constructs an input object using the Angular velocity vector and the - * covariance matrix - * @param w Angular vector - * @param Sigma 6X6 covariance matrix - * Uses Matrix's rows(), cols() and Eigen's SelfAdjointEigenSolver - */ -// Input::Input(const Vector3& w, const Matrix& Sigma) : w(w), Sigma(Sigma) { -// if (Sigma.rows() != 6 || Sigma.cols() != 6) { -// throw std::invalid_argument("Input measurement noise covariance must be 6x6"); -// } -// -// // 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"); -// } -// } -/** - * - * @return 3X3 skey symmetric matrix when called - * Uses Rot3's Hat() to create skew-symmetric matrix - */ -// Matrix3 Input::W() const { -// return Rot3::Hat(w); -// } - - -//======================================================================== -// Measurement Class Implementation -//======================================================================== -/** - * @brief Constructs measurement with directions and covariance. - * @param y_vec A 3D vector representing the measured direction in the sensor frame - * @param d_vec A 3D vector representing the known reference direction in the global frame aka ground truth direction - * @param Sigma 3x3 positive definite covariance vector representing the uncertainty in the measurements - * @param i Calibration index - A non-negative integer specifies the element in the calibration vector - * that corresponds to the sensor of interest. A value of -1 indicates that all the sensors have been calibrated - * - * Creates a measurement object that stores the measured direction(y), reference direction(d), measurement noise covariance(Sigma) - * and Calibration Index cal_idx - * - * Uses Eigen's SelfAdjointEigenSolver - * - */ - -Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i) - : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { - - // 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"); - } -} - -//======================================================================== -// State Class Implementation -//======================================================================== -/** - * - * @param R Rot3 (Attitude) - * @param b Vector (Bias) - * @param S Vector (Rot 3 calibration states) - * Combines the navigation and the calibration states together and provides a - * mechanism to represent the complete system - * - */ -State::State(const Rot3& R, const Vector3& b, const std::vector& S) - : R(R), b(b), S(S) {} - -/** - * - * @param n Number of Calibration states - * @return State object intitialized to identity - * Creates a default/ initial state - * Uses GTSAM's Rot3::identity and Vector3 zero function - */ -State State::identity(int n) { - std::vector calibrations(n, Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), calibrations); -} - -//======================================================================== -// Data Struct Implementation -//======================================================================== - -/** - * - * @param xi Ground Truth state - * @param n_cal Number of calibration states - * @param u Input measurements - * @param y Vector of y measurements - * @param n_meas number of measurements - * @param t timestamp - * @param dt time step - * Used to organize the state, input and measurement data with timestamps for - * testing Uses Rot3, Vector 3 and Unit3 classes - */ -Data::Data(const State& xi, int n_cal, const Input& u, - const std::vector& y, int n_meas, - double t, double dt) - : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} - -//======================================================================== -// Symmetry Group Implementation - Group Elements and actions -//======================================================================== -/** - * - * @param A Attitude element of Rot3 type - * @param a Matrix3 bias element - * @param B Rot3 vector containing calibration elements - * Ouptuts a G object using Rot3 for rotation representation - */ -G::G(const Rot3& A, const Matrix3& a, const std::vector& B) - : A(A), a(a), B(B) {} - -/** - * Defines the group operation (multiplication) - * @param other Another Group element - * @return G a product of two group elements - * Uses Rot3 Hat, Rot3 Vee for multiplication - * - */ -G G::operator*(const G& other) const { - if (B.size() != other.B.size()) { - throw std::invalid_argument("Group elements must have the same number of calibration elements"); - } - - std::vector new_B; - for (size_t i = 0; i < B.size(); i++) { - new_B.push_back(B[i] * other.B[i]); - } - - return G(A * other.A, - a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), - new_B); -} - -/** - * Used to compute the Group inverse - * @return The inverse of group element - * Uses Rot3 inverse, Rot3 matrix, hat and vee functions - * The left invariant property of the semi-direct product group structure is implemented here by using the -ve sign - */ -G G::inv() const { - Matrix3 A_inv = A.inverse().matrix(); - - std::vector B_inv; - for (const auto& b : B) { - B_inv.push_back(b.inverse()); - } - - return G(A.inverse(), - -Rot3::Hat(A_inv * Rot3::Vee(a)), - B_inv); -} - -/** - * Creates the identity element of the group - * @param n Number of calibration elements - * @return the identity element - * Uses Rot3 Identity and Matrix zero - */ -G G::identity(int n) { - std::vector B(n, Rot3::Identity()); - return G(Rot3::Identity(), Matrix3::Zero(), B); -} -/** - * Maps the tangent space elements to the group - * @param x Vector in lie algebra - * @return the group element G - * Uses Rot3 expmap and Expmapderivative function - */ -G G::exp(const Vector& x) { - if (x.size() < 6 || x.size() % 3 != 0) { - throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); - } - - int n = (x.size() - 6) / 3; - 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::vector B; - for (int i = 0; i < n; i++) { - B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); - } - - return G(A, a, B); -} - //======================================================================== // Helper Functions Implementation //======================================================================== diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp index e90283873..13b5a7da7 100644 --- a/examples/ABC_EQF_Demo.cpp +++ b/examples/ABC_EQF_Demo.cpp @@ -174,7 +174,7 @@ std::vector loadDataFromCSV(const std::string& filename, covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 // Create measurement - measurements.push_back(Measurement(y0, d0, covY0, 0)); + measurements.push_back(Measurement{Unit3(y0), Unit3(d0), covY0, 0}); // Second measurement (calibrated sensor, cal_idx = -1) Vector3 y1(values[24], values[25], values[26]); @@ -191,10 +191,10 @@ std::vector loadDataFromCSV(const std::string& filename, covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 // Create measurement - measurements.push_back(Measurement(y1, d1, covY1, -1)); + measurements.push_back(Measurement{Unit3(y1), Unit3(d1), covY1, -1}); // Create Data object and add to list - data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); + data_list.push_back(Data{xi, 1, u, measurements, 2, t, dt}); rowCount++; From 247172dae82f02bcd8d7bbcf84356d132f37a3b1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Apr 2025 23:35:23 -0400 Subject: [PATCH 11/38] Converted PlanarSLAM --- python/gtsam/examples/PlanarSLAMExample.ipynb | 508 ++++++++++++++++++ python/gtsam/examples/PlanarSLAMExample.py | 97 ---- 2 files changed, 508 insertions(+), 97 deletions(-) create mode 100644 python/gtsam/examples/PlanarSLAMExample.ipynb delete mode 100644 python/gtsam/examples/PlanarSLAMExample.py diff --git a/python/gtsam/examples/PlanarSLAMExample.ipynb b/python/gtsam/examples/PlanarSLAMExample.ipynb new file mode 100644 index 000000000..c03805665 --- /dev/null +++ b/python/gtsam/examples/PlanarSLAMExample.ipynb @@ -0,0 +1,508 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "153c8385", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "markdown", + "id": "3ced5f44", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "d2980e5e", + "metadata": {}, + "source": [ + "# Simple Planar SLAM Example\n", + "\n", + "This notebook demonstrates a basic Simultaneous Localization and Mapping (SLAM) problem in 2D using GTSAM.\n", + "\n", + "**What is GTSAM?**\n", + "GTSAM (Georgia Tech Smoothing and Mapping) is a library that implements factor graph-based optimization. It's widely used in robotics for problems like SLAM, Structure from Motion (SfM), and sensor fusion.\n", + "\n", + "**What is a Factor Graph?**\n", + "A factor graph is a graphical model that represents the probabilistic relationships between unknown variables (like robot poses and landmark positions) and measurements (like odometry or sensor readings). Optimization in GTSAM involves finding the most likely configuration of variables given the measurements and their associated uncertainties (noise).\n", + "\n", + "**This Example:**\n", + "We'll simulate a robot moving in a 2D plane. The robot has:\n", + "1. **Odometry:** Measurements of its own motion (how far it moved between steps).\n", + "2. **Bearing-Range Sensor:** A sensor (like a simple laser scanner) that measures the bearing (angle) and range (distance) to landmarks.\n", + "\n", + "We'll build a factor graph representing the robot's poses, landmark positions, odometry measurements, and bearing-range measurements. Then, we'll use GTSAM to optimize the graph and find the best estimate of the robot's trajectory and landmark locations." + ] + }, + { + "cell_type": "markdown", + "id": "9eb7fe9d", + "metadata": {}, + "source": [ + "## 1. Setup and Imports\n", + "\n", + "First, we need to import the necessary libraries: `gtsam` itself and `numpy` for numerical operations." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "eea967e9", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not in Colab" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2c932acb", + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "import gtsam\n", + "import gtsam.utils.plot as gp\n", + "\n", + "# We can use shorthand symbols for variable keys\n", + "# X(i) represents the i-th pose variable\n", + "# L(j) represents the j-th landmark variable\n", + "from gtsam.symbol_shorthand import L, X" + ] + }, + { + "cell_type": "markdown", + "id": "181c929c", + "metadata": {}, + "source": [ + "## 2. Define Noise Models\n", + "\n", + "Real-world measurements are always noisy. In GTSAM, we model this noise using Gaussian noise models. We need to define the uncertainty (standard deviation) for each type of measurement:\n", + "\n", + "* **Prior Noise:** Uncertainty about the very first pose of the robot. We often assume we know the starting position reasonably well, but not perfectly.\n", + "* **Odometry Noise:** Uncertainty in the robot's movement measurements (e.g., wheel encoders). Assumed to be Gaussian noise on the change in x, y, and theta.\n", + "* **Measurement Noise:** Uncertainty in the bearing-range sensor readings. Assumed to be Gaussian noise on the bearing and range measurements." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4a9b8d1b", + "metadata": {}, + "outputs": [], + "source": [ + "# Create noise models with specified standard deviations (sigmas).\n", + "# gtsam.noiseModel.Diagonal.Sigmas takes a numpy array of standard deviations.\n", + "\n", + "# Prior noise on the first pose (x, y, theta) - sigmas = [0.3m, 0.3m, 0.1rad]\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n", + "# Odometry noise (dx, dy, dtheta) - sigmas = [0.2m, 0.2m, 0.1rad]\n", + "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", + "# Measurement noise (bearing, range) - sigmas = [0.1rad, 0.2m]\n", + "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))" + ] + }, + { + "cell_type": "markdown", + "id": "6202026a", + "metadata": {}, + "source": [ + "## 3. Build the Factor Graph\n", + "\n", + "Now, we'll create the factor graph step-by-step.\n", + "\n", + "First, create an empty nonlinear factor graph." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "37e5a43a", + "metadata": {}, + "outputs": [], + "source": [ + "# Create an empty nonlinear factor graph\n", + "graph = gtsam.NonlinearFactorGraph()" + ] + }, + { + "cell_type": "markdown", + "id": "8a1b517b", + "metadata": {}, + "source": [ + "### 3.1 Variable Keys\n", + "\n", + "We need unique keys to identify each unknown variable (robot poses and landmark positions) in the graph. \n", + "We'll have 3 robot poses $(x_1, x_2, x_3)$ and 2 landmarks $(l_1, l_2)$. We will just use `X(1)`..`L(2)` in the code, using the `X(i)` and `L(j)` shorthand imported earlier. Note here we can use base 1 indexing but you can use any integer, and numbering does not have to be consecutive." + ] + }, + { + "cell_type": "markdown", + "id": "d9e1aaaa", + "metadata": {}, + "source": [ + "### 3.2 Add a Prior Factor\n", + "\n", + "To \"anchor\" the graph, we add a prior factor on the first pose $x_1$. This represents our initial belief about where the robot started. We assume it started at the origin (0, 0, 0) with the uncertainty defined by `PRIOR_NOISE`.\n", + "\n", + "A `PriorFactorPose2` connects the single pose variable $x_1$ to a known pose (`gtsam.Pose2(0,0,0)`) with a specific noise model." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0549b0a2", + "metadata": {}, + "outputs": [], + "source": [ + "# Add a prior on pose X(1) at the origin.\n", + "# A prior factor consists of a mean (gtsam.Pose2) and a noise model.\n", + "graph.add(gtsam.PriorFactorPose2(X(1), gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "cb8ffd2d", + "metadata": {}, + "source": [ + "### 3.3 Add Odometry Factors\n", + "\n", + "Next, we add factors representing the robot's movement based on odometry measurements. We assume the robot moved approximately 2 units forward in the x-direction between each pose.\n", + "\n", + "A `BetweenFactorPose2` connects two consecutive poses (e.g., $x_1$ and $x_2$) and represents the measured relative motion between them (`gtsam.Pose2(2.0, 0.0, 0.0)`) with its associated noise (`ODOMETRY_NOISE`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2389fae3", + "metadata": {}, + "outputs": [], + "source": [ + "# Add odometry factors between X(1),X(2) and X(2),X(3), respectively.\n", + "# The measurement is the relative motion: Pose2(dx, dy, dtheta).\n", + "\n", + "# Between X(1) and X(2): Move forward 2m\n", + "graph.add(gtsam.BetweenFactorPose2(X(1), X(2), gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))\n", + "# Between X(2) and X(3): Move forward 2m\n", + "graph.add(gtsam.BetweenFactorPose2(X(2), X(3), gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "17459992", + "metadata": {}, + "source": [ + "### 3.4 Add Measurement Factors\n", + "\n", + "Now, add factors representing the bearing-range measurements from the robot's poses to the landmarks.\n", + "\n", + "A `BearingRangeFactor2D` connects a pose variable (e.g., $x_1$) and a landmark variable (e.g., $l_1$). It includes the measured bearing (`gtsam.Rot2`) and range (distance), along with the measurement noise (`MEASUREMENT_NOISE`).\n", + "\n", + "We have three measurements:\n", + "* From $x_1$ to $l_1$: Bearing 45 degrees, Range sqrt(8)\n", + "* From $x_2$ to $l_1$: Bearing 90 degrees, Range 2.0\n", + "* From $x_3$ to $l_2$: Bearing 90 degrees, Range 2.0" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cc19f4ac", + "metadata": {}, + "outputs": [], + "source": [ + "# Add Range-Bearing measurements to two different landmarks L(1) and L(2).\n", + "# Measurements are Bearing (gtsam.Rot2) and Range (float).\n", + "\n", + "# From X(1) to L(1)\n", + "graph.add(gtsam.BearingRangeFactor2D(X(1), L(1), gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))\n", + "# From X(2) to L(1)\n", + "graph.add(gtsam.BearingRangeFactor2D(X(2), L(1), gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))\n", + "# From X(3) to L(2)\n", + "graph.add(gtsam.BearingRangeFactor2D(X(3), L(2), gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "4820f7b1", + "metadata": {}, + "source": [ + "### 3.5 Inspect the Graph\n", + "\n", + "We can print the factor graph to see a summary of the variables and factors we've added." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "83b8002e", + "metadata": {}, + "outputs": [], + "source": [ + "# Print the graph. This shows the factors and the variables they connect.\n", + "print(\"Factor Graph:\\n{}\".format(graph))" + ] + }, + { + "cell_type": "markdown", + "id": "038f5089", + "metadata": {}, + "source": [ + "## 4. Create Initial Estimate\n", + "\n", + "Factor graph optimization is an iterative process that needs an initial guess (initial estimate) for the values of all unknown variables. The closer the initial estimate is to the true solution, the faster and more reliably the optimizer will converge.\n", + "\n", + "Here, we create a `gtsam.Values` object and deliberately insert slightly *inaccurate* initial estimates for the poses and landmark positions. This demonstrates that the optimizer can correct for errors in the initial guess." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "98c87675", + "metadata": {}, + "outputs": [], + "source": [ + "# Create (deliberately inaccurate) initial estimate.\n", + "# gtsam.Values is a container mapping variable keys to their estimated values.\n", + "initial_estimate = gtsam.Values()\n", + "\n", + "# Insert initial guesses for poses (Pose2: x, y, theta)\n", + "initial_estimate.insert(X(1), gtsam.Pose2(-0.25, 0.20, 0.15))\n", + "initial_estimate.insert(X(2), gtsam.Pose2(2.30, 0.10, -0.20))\n", + "initial_estimate.insert(X(3), gtsam.Pose2(4.10, 0.10, 0.10))\n", + "\n", + "# Insert initial guesses for landmarks (Point2: x, y)\n", + "initial_estimate.insert(L(1), gtsam.Point2(1.80, 2.10))\n", + "initial_estimate.insert(L(2), gtsam.Point2(4.10, 1.80))\n", + "\n", + "# Print the initial estimate\n", + "print(\"Initial Estimate:\\n{}\".format(initial_estimate))" + ] + }, + { + "cell_type": "markdown", + "id": "2d796916", + "metadata": {}, + "source": [ + "Now that we have an initial estimate we can also visualize the graph:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d896ecee", + "metadata": {}, + "outputs": [], + "source": [ + "display(graphviz.Source(graph.dot(initial_estimate)))" + ] + }, + { + "cell_type": "markdown", + "id": "2608bf96", + "metadata": {}, + "source": [ + "## 5. Optimize the Factor Graph\n", + "\n", + "Now, we use an optimizer to find the variable configuration that best fits all the factors (measurements) in the graph, starting from the initial estimate.\n", + "\n", + "We'll use the Levenberg-Marquardt (LM) algorithm, a standard non-linear least-squares optimizer.\n", + "\n", + "1. Create LM parameters (`gtsam.LevenbergMarquardtParams`). We'll use the defaults.\n", + "2. Create the optimizer instance, providing the graph, initial estimate, and parameters.\n", + "3. Run the optimization by calling `optimizer.optimize()`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2ee6b17a", + "metadata": {}, + "outputs": [], + "source": [ + "# Optimize using Levenberg-Marquardt optimization.\n", + "# The optimizer accepts optional parameters, but we'll use the defaults here.\n", + "params = gtsam.LevenbergMarquardtParams()\n", + "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)\n", + "\n", + "# Perform the optimization\n", + "result = optimizer.optimize()\n", + "\n", + "# Print the final optimized result\n", + "# This gtsam.Values object contains the most likely estimates for all variables.\n", + "print(\"\\nFinal Result:\\n{}\".format(result))" + ] + }, + { + "cell_type": "markdown", + "id": "1bda91e1", + "metadata": {}, + "source": [ + "The code below visualizes the optimized poses and landmarks in 2D. It uses GTSAM's plotting utilities to plot the robot's trajectory (poses) and the estimated positions of the landmarks. The poses are represented as coordinate frames indicating the robot's orientation, while the landmarks are plotted as blue points. The aspect ratio is set to ensure equal scaling for both axes." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d827195e", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(1)\n", + "axes = fig.add_subplot()\n", + "axes = fig.axes[0]\n", + "\n", + "# Plot 2D poses\n", + "poses : gtsam.Values = gtsam.utilities.allPose2s(result)\n", + "for key in poses.keys():\n", + " pose = poses.atPose2(key)\n", + " gp.plot_pose2_on_axes(axes, pose, axis_length=0.3)\n", + "\n", + "# Plot 2D landmarks\n", + "landmarks : np.ndarray = gtsam.utilities.extractPoint2(result) # 2xn array\n", + "for landmark in landmarks:\n", + " gp.plot_point2_on_axes(axes, landmark, linespec=\"b\")\n", + "\n", + "axes.set_aspect(\"equal\", adjustable=\"datalim\")" + ] + }, + { + "cell_type": "markdown", + "id": "27bc8e38", + "metadata": {}, + "source": [ + "## 6. Calculate Marginal Covariances\n", + "\n", + "Besides finding the optimal values (the mean), GTSAM can also compute the uncertainty (covariance) associated with each variable estimate after optimization. This tells us how confident we are about the estimated poses and landmark locations.\n", + "\n", + "We use the `gtsam.Marginals` class to calculate the marginal covariance matrices for each variable.\n", + "Each pose covariance matrix is a 3x3 matrix because a pose in 2D (`Pose2`) has three components: `(x, y, theta)` Each landmark covariance matrix is a 2x2 matrix because a landmark in 2D is represented by its `(x, y)` position.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "90ef96ff", + "metadata": {}, + "outputs": [], + "source": [ + "# Calculate and print marginal covariances for all variables.\n", + "# This provides information about the uncertainty of the estimates.\n", + "marginals = gtsam.Marginals(graph, result)\n", + "\n", + "# Print the covariance matrix for each variable\n", + "print(\"X1 covariance:\\n{}\\n\".format(marginals.marginalCovariance(X(1))))\n", + "print(\"X2 covariance:\\n{}\\n\".format(marginals.marginalCovariance(X(2))))\n", + "print(\"X3 covariance:\\n{}\\n\".format(marginals.marginalCovariance(X(3))))\n", + "print(\"L1 covariance:\\n{}\\n\".format(marginals.marginalCovariance(L(1))))\n", + "print(\"L2 covariance:\\n{}\\n\".format(marginals.marginalCovariance(L(2))))" + ] + }, + { + "cell_type": "markdown", + "id": "62fd4f35", + "metadata": {}, + "source": [ + "The code below once again visualizes the optimized poses and landmarks, now with their associated uncertainties (covariances).\n", + "The covariance ellipses plotted on the graph visually represent the uncertainty in the estimates. Larger ellipses indicate higher uncertainty, while smaller ellipses indicate more confident estimates. \n", + "\n", + "The prior is on $x_1$, at the origin, and hence that is the most certain pose, after which uncertainty increases. Note that for poses we only show the uncertainty on translation, although each pose also has an uncertain orientation. The covariance ellipses on the landmarks actually reflect that orientation uncertainty, being oriented the way they are." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d1f03fee", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(2)\n", + "axes = fig.add_subplot()\n", + "axes = fig.axes[0]\n", + "\n", + "# Plot 2D poses\n", + "poses = gtsam.utilities.allPose2s(result)\n", + "for key in poses.keys():\n", + " pose = poses.atPose2(key)\n", + " covariance = marginals.marginalCovariance(key)\n", + "\n", + " gp.plot_pose2_on_axes(axes, pose, covariance=covariance, axis_length=0.3)\n", + "\n", + "# Plot 2D landmarks\n", + "landmarks: np.ndarray = gtsam.utilities.extractPoint2(result) # 2xn array\n", + "for j, landmark in enumerate(landmarks):\n", + " gp.plot_point2_on_axes(axes, landmark, linespec=\"b\")\n", + " covariance = marginals.marginalCovariance(L(j+1))\n", + " gp.plot_covariance_ellipse_2d(axes, landmark, covariance=covariance)\n", + "\n", + "axes.set_aspect(\"equal\", adjustable=\"datalim\")" + ] + }, + { + "cell_type": "markdown", + "id": "74673b3d", + "metadata": {}, + "source": [ + "## Summary\n", + "\n", + "We have successfully:\n", + "1. Defined the structure of a 2D SLAM problem using factors (prior, odometry, measurements) and variables (poses, landmarks).\n", + "2. Represented this problem as a `gtsam.NonlinearFactorGraph`.\n", + "3. Provided noisy measurements and an inaccurate initial estimate.\n", + "4. Used `gtsam.LevenbergMarquardtOptimizer` to find the most likely configuration of poses and landmarks.\n", + "5. Calculated the uncertainty (covariance) of the final estimates.\n", + "\n", + "This demonstrates the basic workflow of using GTSAM for solving SLAM and other robotics estimation problems." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py deleted file mode 100644 index d2ee92c95..000000000 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ /dev/null @@ -1,97 +0,0 @@ -""" -GTSAM Copyright 2010-2018, 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 - -Simple robotics example using odometry measurements and bearing-range (laser) measurements -Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import gtsam -import numpy as np -from gtsam.symbol_shorthand import L, X - -# Create noise models -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) -MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) - - -def main(): - """Main runner""" - - # Create an empty nonlinear factor graph - graph = gtsam.NonlinearFactorGraph() - - # Create the keys corresponding to unknown variables in the factor graph - X1 = X(1) - X2 = X(2) - X3 = X(3) - L1 = L(4) - L2 = L(5) - - # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model - graph.add( - gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) - - # Add odometry factors between X1,X2 and X2,X3, respectively - graph.add( - gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), - ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), - ODOMETRY_NOISE)) - - # Add Range-Bearing measurements to two different landmarks L1 and L2 - graph.add( - gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), - np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE)) - graph.add( - gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, - MEASUREMENT_NOISE)) - graph.add( - gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, - MEASUREMENT_NOISE)) - - # Print graph - print("Factor Graph:\n{}".format(graph)) - - # Create (deliberately inaccurate) initial estimate - initial_estimate = gtsam.Values() - initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) - initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) - initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) - initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) - initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) - - # Print - print("Initial Estimate:\n{}".format(initial_estimate)) - - # Optimize using Levenberg-Marquardt optimization. The optimizer - # accepts an optional set of configuration parameters, controlling - # things like convergence criteria, the type of linear system solver - # to use, and the amount of information displayed during optimization. - # Here we will use the default set of parameters. See the - # documentation for the full set of parameters. - params = gtsam.LevenbergMarquardtParams() - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, - params) - result = optimizer.optimize() - print("\nFinal Result:\n{}".format(result)) - - # Calculate and print marginal covariances for all variables - marginals = gtsam.Marginals(graph, result) - for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), - (L2, "L2")]: - print("{} covariance:\n{}\n".format(s, - marginals.marginalCovariance(key))) - - -if __name__ == "__main__": - main() From e52685d4412f201264026d94e5a7dd62c76dde82 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Apr 2025 23:55:42 -0400 Subject: [PATCH 12/38] Pose-SLAM and rendering --- gtsam/inference/doc/Shortcuts.ipynb | 391 ++++++++++ python/gtsam/examples/PlanarSLAMExample.ipynb | 352 ++++++++- python/gtsam/examples/Pose2SLAMExample.ipynb | 712 ++++++++++++++++++ python/gtsam/examples/Pose2SLAMExample.py | 102 --- 4 files changed, 1431 insertions(+), 126 deletions(-) create mode 100644 gtsam/inference/doc/Shortcuts.ipynb create mode 100644 python/gtsam/examples/Pose2SLAMExample.ipynb delete mode 100644 python/gtsam/examples/Pose2SLAMExample.py diff --git a/gtsam/inference/doc/Shortcuts.ipynb b/gtsam/inference/doc/Shortcuts.ipynb new file mode 100644 index 000000000..66dd0bcf2 --- /dev/null +++ b/gtsam/inference/doc/Shortcuts.ipynb @@ -0,0 +1,391 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Efficent Marginals Computation\n", + "\n", + "GTSAM can very efficiently calculate marginals in Bayes trees. In this post, we illustrate the “shortcut” mechanism for **caching** the conditional distribution $P(S \\mid R)$ in a Bayes tree, allowing efficient other marginal queries. We assume familiarity with **Bayes trees** from [the previous post](#)." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Toy Example\n", + "\n", + "We create a small Bayes tree:\n", + "\n", + "\\begin{equation}\n", + "P(a \\mid b) P(b,c \\mid r) P(f \\mid e) P(d,e \\mid r) P(r).\n", + "\\end{equation}\n", + "\n", + "Below is some Python code (using GTSAM’s discrete wrappers) to define and build the corresponding Bayes tree. We'll use a discrete example, i.e., we'll create a `DiscreteBayesTree`.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import DiscreteConditional, DiscreteBayesTree, DiscreteBayesTreeClique, DecisionTreeFactor" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Make discrete keys (key in elimination order, cardinality):\n", + "keys = [(0, 2), (1, 2), (2, 2), (3, 2), (4, 2), (5, 2), (6, 2)]\n", + "names = {0: 'a', 1: 'f', 2: 'b', 3: 'c', 4: 'd', 5: 'e', 6: 'r'}\n", + "aKey, fKey, bKey, cKey, dKey, eKey, rKey = keys\n", + "keyFormatter = lambda key: names[key]" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# 1. Root Clique: P(r)\n", + "cliqueR = DiscreteBayesTreeClique(DiscreteConditional(rKey, \"0.4/0.6\"))\n", + "\n", + "# 2. Child Clique 1: P(b, c | r)\n", + "cliqueBC = DiscreteBayesTreeClique(\n", + " DiscreteConditional(\n", + " 2, DecisionTreeFactor([bKey, cKey, rKey], \"0.3 0.7 0.1 0.9 0.2 0.8 0.4 0.6\")\n", + " )\n", + ")\n", + "\n", + "# 3. Child Clique 2: P(d, e | r)\n", + "cliqueDE = DiscreteBayesTreeClique(\n", + " DiscreteConditional(\n", + " 2, DecisionTreeFactor([dKey, eKey, rKey], \"0.1 0.9 0.9 0.1 0.2 0.8 0.3 0.7\")\n", + " )\n", + ")\n", + "\n", + "# 4. Leaf Clique from Child 1: P(a | b)\n", + "cliqueA = DiscreteBayesTreeClique(DiscreteConditional(aKey, [bKey], \"1/3 3/1\"))\n", + "\n", + "# 5. Leaf Clique from Child 2: P(f | e)\n", + "cliqueF = DiscreteBayesTreeClique(DiscreteConditional(fKey, [eKey], \"1/3 3/1\"))" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# Build the BayesTree:\n", + "bayesTree = DiscreteBayesTree()\n", + "\n", + "# Insert root:\n", + "bayesTree.insertRoot(cliqueR)\n", + "\n", + "# Attach child cliques to root:\n", + "bayesTree.addClique(cliqueBC, cliqueR)\n", + "bayesTree.addClique(cliqueDE, cliqueR)\n", + "\n", + "# Attach leaf cliques:\n", + "bayesTree.addClique(cliqueA, cliqueBC)\n", + "bayesTree.addClique(cliqueF, cliqueDE)\n", + "\n", + "# bayesTree.print(\"bayesTree\", keyFormatter)" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "0\n", + "\n", + "r\n", + "\n", + "\n", + "\n", + "1\n", + "\n", + "b, c : r\n", + "\n", + "\n", + "\n", + "0->1\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "3\n", + "\n", + "d, e : r\n", + "\n", + "\n", + "\n", + "0->3\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "2\n", + "\n", + "a : b\n", + "\n", + "\n", + "\n", + "1->2\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "4\n", + "\n", + "f : e\n", + "\n", + "\n", + "\n", + "3->4\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import graphviz\n", + "graphviz.Source(bayesTree.dot(keyFormatter))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Naive Computation of P(a)\n", + "The marginal $P(a)$ can be computed by summing out the other variables in the tree:\n", + "$$\n", + "P(a) = \\sum_{b, c, d, e, f, r} P(a, b, c, d, e, f, r)\n", + "$$\n", + "\n", + "Using the Bayes tree structure, we have\n", + "\n", + "$$\n", + "P(a) = \\sum_{b, c, d, e, f, r} P(a \\mid b) P(b, c \\mid r) P(f \\mid e) P(d, e \\mid r) P(r) \n", + "$$\n", + "\n", + "but we can ignore variables $e$ and $f$ not on the path from $a$ to the root $r$. Indeed, by associativity we have\n", + "\n", + "$$\n", + "P(a) = \\sum_{r} \\Bigl\\{ \\sum_{e,f} P(f \\mid e) P(d, e \\mid r) \\Bigr\\} \\sum_{b, c, d} P(a \\mid b) P(b, c \\mid r) P(r)\n", + "$$\n", + "\n", + "where the grouped terms sum to one for any value of $r$, and hence\n", + "\n", + "$$\n", + "P(a) = \\sum_{r, b, c, d} P(a \\mid b) P(b, c \\mid r) P(r).\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Memoization via Shortcuts\n", + "\n", + "In GTSAM, we compute this recursively\n", + "\n", + "#### Step 1\n", + "We want to compute the marginal via\n", + "$$\n", + "P(a) = \\sum_{r, b} P(a \\mid b) P(b).\n", + "$$\n", + "where $P(b)$ is the separator of this clique.\n", + "\n", + "#### Step 2\n", + "To compute the separator marginal, we use the **shortcut** $P(b|r)$:\n", + "$$\n", + "P(b) = \\sum_{r} P(b \\mid r) P(r).\n", + "$$\n", + "In general, a shortcut $P(S|R)$ directly conditions this clique's separator $S$ on the root clique $R$, even if there are many other cliques in-between. That is why it is called a *shortcut*.\n", + "\n", + "#### Step 3 (optional)\n", + "If the shortcut was already computed, then we are done! If not, we compute it recursively:\n", + "$$\n", + "P(S\\mid R) = \\sum_{F_p,\\,S_p \\setminus S}P(F_p \\mid S_p) P(S_p \\mid R).\n", + "$$\n", + "Above $P(F_p \\mid S_p)$ is the parent clique, and by the running intersection property we know that the seprator $S$ is a subset of the parent clique's variables.\n", + "Note that the recursion is because we might not have $P(S_p \\mid R)$ yet, so it might have to be computed in turn, etc. The recursion ends at nodes below the root, and **after we have obtained $P(S\\mid R)$ we cache it**.\n", + "\n", + "In our example, the computation is simply\n", + "$$\n", + "P(b|r) = \\sum_{c} P(b, c \\mid r),\n", + "$$\n", + "because this the parent separator is already the root, so $P(S_p \\mid R)$ is omitted. This is also the end of the recursion.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0\n", + "Marginal P(a):\n", + " Discrete Conditional\n", + " P( 0 ):\n", + " Choice(0) \n", + " 0 Leaf 0.51\n", + " 1 Leaf 0.49\n", + "\n", + "\n", + "3\n" + ] + } + ], + "source": [ + "# Marginal of the leaf variable 'a':\n", + "print(bayesTree.numCachedSeparatorMarginals())\n", + "marg_a = bayesTree.marginalFactor(aKey[0])\n", + "print(\"Marginal P(a):\\n\", marg_a)\n", + "print(bayesTree.numCachedSeparatorMarginals())\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "3\n", + "Marginal P(b):\n", + " Discrete Conditional\n", + " P( 2 ):\n", + " Choice(2) \n", + " 0 Leaf 0.48\n", + " 1 Leaf 0.52\n", + "\n", + "\n", + "3\n" + ] + } + ], + "source": [ + "\n", + "# Marginal of the internal variable 'b':\n", + "print(bayesTree.numCachedSeparatorMarginals())\n", + "marg_b = bayesTree.marginalFactor(bKey[0])\n", + "print(\"Marginal P(b):\\n\", marg_b)\n", + "print(bayesTree.numCachedSeparatorMarginals())\n" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "3\n", + "Joint P(a, f):\n", + " DiscreteBayesNet\n", + " \n", + "size: 2\n", + "conditional 0: P( 0 | 1 ):\n", + " Choice(1) \n", + " 0 Choice(0) \n", + " 0 0 Leaf 0.51758893\n", + " 0 1 Leaf 0.48241107\n", + " 1 Choice(0) \n", + " 1 0 Leaf 0.50222672\n", + " 1 1 Leaf 0.49777328\n", + "\n", + "conditional 1: P( 1 ):\n", + " Choice(1) \n", + " 0 Leaf 0.506\n", + " 1 Leaf 0.494\n", + "\n", + "\n", + "3\n" + ] + } + ], + "source": [ + "\n", + "# Joint of leaf variables 'a' and 'f': P(a, f)\n", + "# This effectively needs to gather info from two different branches\n", + "print(bayesTree.numCachedSeparatorMarginals())\n", + "marg_af = bayesTree.jointBayesNet(aKey[0], fKey[0])\n", + "print(\"Joint P(a, f):\\n\", marg_af)\n", + "print(bayesTree.numCachedSeparatorMarginals())\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/python/gtsam/examples/PlanarSLAMExample.ipynb b/python/gtsam/examples/PlanarSLAMExample.ipynb index c03805665..e96c6ed2c 100644 --- a/python/gtsam/examples/PlanarSLAMExample.ipynb +++ b/python/gtsam/examples/PlanarSLAMExample.ipynb @@ -1,5 +1,13 @@ { "cells": [ + { + "cell_type": "markdown", + "id": "21db97de", + "metadata": {}, + "source": [ + "# Simple Planar SLAM Example" + ] + }, { "cell_type": "markdown", "id": "153c8385", @@ -14,7 +22,10 @@ "All Rights Reserved\n", "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", "\n", - "See LICENSE for the license information" + "See LICENSE for the license information\n", + "\n", + "Simple robotics example using odometry measurements and bearing-range (laser) measurements\n", + "Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)" ] }, { @@ -34,8 +45,6 @@ "id": "d2980e5e", "metadata": {}, "source": [ - "# Simple Planar SLAM Example\n", - "\n", "This notebook demonstrates a basic Simultaneous Localization and Mapping (SLAM) problem in 2D using GTSAM.\n", "\n", "**What is GTSAM?**\n", @@ -64,7 +73,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 15, "id": "eea967e9", "metadata": {}, "outputs": [], @@ -79,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 16, "id": "2c932acb", "metadata": {}, "outputs": [], @@ -113,7 +122,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 17, "id": "4a9b8d1b", "metadata": {}, "outputs": [], @@ -143,7 +152,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 18, "id": "37e5a43a", "metadata": {}, "outputs": [], @@ -177,7 +186,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 19, "id": "0549b0a2", "metadata": {}, "outputs": [], @@ -201,7 +210,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 20, "id": "2389fae3", "metadata": {}, "outputs": [], @@ -234,7 +243,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 21, "id": "cc19f4ac", "metadata": {}, "outputs": [], @@ -262,10 +271,51 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 22, "id": "83b8002e", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor Graph:\n", + "NonlinearFactorGraph: size: 6\n", + "\n", + "Factor 0: PriorFactor on x1\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.3; 0.3; 0.1];\n", + "\n", + "Factor 1: BetweenFactor(x1,x2)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 2: BetweenFactor(x2,x3)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 3: BearingRangeFactor\n", + "Factor 3: keys = { x1 l1 }\n", + " noise model: diagonal sigmas [0.1; 0.2];\n", + "ExpressionFactor with measurement: bearing : 0.785398163\n", + "range 2.82842712\n", + "\n", + "Factor 4: BearingRangeFactor\n", + "Factor 4: keys = { x2 l1 }\n", + " noise model: diagonal sigmas [0.1; 0.2];\n", + "ExpressionFactor with measurement: bearing : 1.57079633\n", + "range 2\n", + "\n", + "Factor 5: BearingRangeFactor\n", + "Factor 5: keys = { x3 l2 }\n", + " noise model: diagonal sigmas [0.1; 0.2];\n", + "ExpressionFactor with measurement: bearing : 1.57079633\n", + "range 2\n", + "\n", + "\n" + ] + } + ], "source": [ "# Print the graph. This shows the factors and the variables they connect.\n", "print(\"Factor Graph:\\n{}\".format(graph))" @@ -285,10 +335,41 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 23, "id": "98c87675", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initial Estimate:\n", + "Values with 5 values:\n", + "Value l1: (Eigen::Matrix)\n", + "[\n", + "\t1.8;\n", + "\t2.1\n", + "]\n", + "\n", + "Value l2: (Eigen::Matrix)\n", + "[\n", + "\t4.1;\n", + "\t1.8\n", + "]\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(-0.25, 0.2, 0.15)\n", + "\n", + "Value x2: (gtsam::Pose2)\n", + "(2.3, 0.1, -0.2)\n", + "\n", + "Value x3: (gtsam::Pose2)\n", + "(4.1, 0.1, 0.1)\n", + "\n", + "\n" + ] + } + ], "source": [ "# Create (deliberately inaccurate) initial estimate.\n", "# gtsam.Values is a container mapping variable keys to their estimated values.\n", @@ -317,10 +398,149 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 24, "id": "d896ecee", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323\n", + "\n", + "x3\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor5\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "display(graphviz.Source(graph.dot(initial_estimate)))" ] @@ -343,10 +563,42 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 25, "id": "2ee6b17a", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Final Result:\n", + "Values with 5 values:\n", + "Value l1: (Eigen::Matrix)\n", + "[\n", + "\t2;\n", + "\t2\n", + "]\n", + "\n", + "Value l2: (Eigen::Matrix)\n", + "[\n", + "\t4;\n", + "\t2\n", + "]\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(-5.72151617e-16, -2.6221043e-16, -8.93525825e-17)\n", + "\n", + "Value x2: (gtsam::Pose2)\n", + "(2, -5.76036948e-15, -6.89367166e-16)\n", + "\n", + "Value x3: (gtsam::Pose2)\n", + "(4, -1.0618198e-14, -6.48560093e-16)\n", + "\n", + "\n" + ] + } + ], "source": [ "# Optimize using Levenberg-Marquardt optimization.\n", "# The optimizer accepts optional parameters, but we'll use the defaults here.\n", @@ -371,10 +623,21 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 26, "id": "d827195e", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "fig = plt.figure(1)\n", "axes = fig.add_subplot()\n", @@ -409,10 +672,40 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 27, "id": "90ef96ff", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "X1 covariance:\n", + "[[ 9.00000000e-02 4.08945493e-33 -3.19744231e-18]\n", + " [ 4.08945493e-33 9.00000000e-02 -1.27897692e-17]\n", + " [-3.19744231e-18 -1.27897692e-17 1.00000000e-02]]\n", + "\n", + "X2 covariance:\n", + "[[ 0.12096774 -0.00129032 0.00451613]\n", + " [-0.00129032 0.1583871 0.02064516]\n", + " [ 0.00451613 0.02064516 0.01774194]]\n", + "\n", + "X3 covariance:\n", + "[[0.16096774 0.00774194 0.00451613]\n", + " [0.00774194 0.35193548 0.05612903]\n", + " [0.00451613 0.05612903 0.02774194]]\n", + "\n", + "L1 covariance:\n", + "[[ 0.16870968 -0.04774194]\n", + " [-0.04774194 0.16354839]]\n", + "\n", + "L2 covariance:\n", + "[[ 0.29387097 -0.10451613]\n", + " [-0.10451613 0.39193548]]\n", + "\n" + ] + } + ], "source": [ "# Calculate and print marginal covariances for all variables.\n", "# This provides information about the uncertainty of the estimates.\n", @@ -439,10 +732,21 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 28, "id": "d1f03fee", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "fig = plt.figure(2)\n", "axes = fig.add_subplot()\n", diff --git a/python/gtsam/examples/Pose2SLAMExample.ipynb b/python/gtsam/examples/Pose2SLAMExample.ipynb new file mode 100644 index 000000000..58326d371 --- /dev/null +++ b/python/gtsam/examples/Pose2SLAMExample.ipynb @@ -0,0 +1,712 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "8be9131e", + "metadata": {}, + "source": [ + "# Pose2 SLAM Example" + ] + }, + { + "cell_type": "markdown", + "id": "copyright-cell", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information\n", + "\n", + "Simple Pose-SLAM example using only odometry measurements\n", + "Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)" + ] + }, + { + "cell_type": "markdown", + "id": "colab-button-cell", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "intro-markdown", + "metadata": {}, + "source": [ + "This notebook demonstrates a simple 2D Simultaneous Localization and Mapping (SLAM) problem involving only robot poses and odometry measurements. A key aspect of this example is the inclusion of a **loop closure** constraint.\n", + "\n", + "**Problem Setup:**\n", + "Imagine a robot moving in a 2D plane. It receives measurements of its own motion (odometry) between consecutive time steps. Odometry is notoriously prone to drift – small errors accumulate over time, causing the robot's estimated position to diverge from its true position.\n", + "\n", + "**Loop Closure:**\n", + "A loop closure occurs when the robot recognizes a previously visited location. This provides a powerful constraint that links a later pose back to an earlier pose, significantly reducing accumulated drift. In this example, we simulate a loop closure by adding a factor that directly connects the last pose ($x_5$) back to an earlier pose ($x_2$).\n", + "\n", + "**Factor Graph:**\n", + "We will build a factor graph representing:\n", + "1. **Variables:** The unknown robot poses ($x_1, x_2, x_3, x_4, x_5$) at different time steps.\n", + "2. **Factors:**\n", + " * A **Prior Factor** on the first pose ($x_1$), anchoring the map.\n", + " * **Odometry Factors** (Between Factors) connecting consecutive poses ($x_1 \to x_2$, $x_2 \to x_3$, etc.), representing the noisy relative motion measurements.\n", + " * A **Loop Closure Factor** (also a Between Factor) connecting the last pose ($x_5$) to an earlier pose ($x_2$), representing the constraint that the robot has returned to a known location.\n", + "\n", + "We will then use GTSAM to optimize this factor graph and find the most likely sequence of robot poses given the measurements and the loop closure." + ] + }, + { + "cell_type": "markdown", + "id": "setup-imports-markdown", + "metadata": {}, + "source": [ + "## 1. Setup and Imports\n", + "\n", + "First, we install GTSAM if needed (e.g., in Google Colab) and import the necessary libraries: `gtsam`, `math` for PI, `matplotlib` for plotting, and `gtsam.utils.plot` for GTSAM-specific plotting functions." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "install-code", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not in Colab" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "id": "imports-code", + "metadata": {}, + "outputs": [], + "source": [ + "import math\n", + "import graphviz\n", + "import numpy as np\n", + "\n", + "import gtsam\n", + "import gtsam.utils.plot as gtsam_plot\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "markdown", + "id": "noise-models-markdown", + "metadata": {}, + "source": [ + "## 2. Define Noise Models\n", + "\n", + "We define Gaussian noise models for our factors:\n", + "\n", + "* **Prior Noise:** Uncertainty on the initial pose ($x_1$). We assume the robot starts at the origin (0, 0, 0), but with some uncertainty.\n", + "* **Odometry Noise:** Uncertainty on the relative motion measurements between poses. This applies to both the sequential odometry factors and the loop closure factor." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "noise-models-code", + "metadata": {}, + "outputs": [], + "source": [ + "# Create noise models with specified standard deviations (sigmas).\n", + "# For Pose2, the noise is on (x, y, theta).\n", + "# Note: gtsam.Point3 is used here to represent the 3 sigmas (dx, dy, dtheta)\n", + "\n", + "# Prior noise on the first pose (x, y, theta) - sigmas = [0.3m, 0.3m, 0.1rad]\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n", + "# Odometry noise (dx, dy, dtheta) - sigmas = [0.2m, 0.2m, 0.1rad]\n", + "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))" + ] + }, + { + "cell_type": "markdown", + "id": "build-graph-markdown", + "metadata": {}, + "source": [ + "## 3. Build the Factor Graph\n", + "\n", + "Now, we create the factor graph. We'll use simple integer keys (1, 2, 3, 4, 5) to represent the poses $x_1$ through $x_5$." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "create-graph-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 1. Create a factor graph container\n", + "graph = gtsam.NonlinearFactorGraph()" + ] + }, + { + "cell_type": "markdown", + "id": "add-prior-markdown", + "metadata": {}, + "source": [ + "### 3.1 Add Prior Factor\n", + "\n", + "Add a `PriorFactorPose2` on the first pose (key `1`), setting it to the origin `gtsam.Pose2(0, 0, 0)` with the defined `PRIOR_NOISE`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "add-prior-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 2a. Add a prior on the first pose (key 1)\n", + "graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "add-odometry-markdown", + "metadata": {}, + "source": [ + "### 3.2 Add Odometry Factors\n", + "\n", + "Add `BetweenFactorPose2` factors for the sequential movements:\n", + "* $x_1 \to x_2$: Move 2m forward.\n", + "* $x_2 \to x_3$: Move 2m forward, turn 90 degrees left ($\\pi/2$).\n", + "* $x_3 \to x_4$: Move 2m forward, turn 90 degrees left ($\\pi/2$).\n", + "* $x_4 \to x_5$: Move 2m forward, turn 90 degrees left ($\\pi/2$).\n", + "\n", + "Each factor uses the `ODOMETRY_NOISE` model." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "add-odometry-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 2b. Add odometry factors (Between Factors)\n", + "# Between poses 1 and 2:\n", + "graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))\n", + "# Between poses 2 and 3:\n", + "graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))\n", + "# Between poses 3 and 4:\n", + "graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))\n", + "# Between poses 4 and 5:\n", + "graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "add-loop-closure-markdown", + "metadata": {}, + "source": [ + "### 3.3 Add Loop Closure Factor\n", + "\n", + "This is the crucial step for correcting drift. We add a `BetweenFactorPose2` connecting the last pose ($x_5$, key `5`) back to the second pose ($x_2$, key `2`). The measurement represents the expected relative transform between pose 5 and pose 2 if the robot correctly returned to the location of $x_2$. We assume this measurement is also subject to `ODOMETRY_NOISE`.\n", + "\n", + "The relative pose `gtsam.Pose2(2, 0, math.pi / 2)` implies that pose 2 is 2m ahead and rotated by +90 degrees relative to pose 5." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "add-loop-closure-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 2c. Add the loop closure constraint\n", + "# This factor connects pose 5 back to pose 2\n", + "# The measurement is the expected relative pose from 5 to 2\n", + "graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "inspect-graph-markdown", + "metadata": {}, + "source": [ + "### 3.4 Inspect the Graph\n", + "\n", + "Print the graph to see the factors and the variables they connect." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "inspect-graph-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor Graph:\n", + "NonlinearFactorGraph: size: 6\n", + "\n", + "Factor 0: PriorFactor on 1\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.3; 0.3; 0.1];\n", + "\n", + "Factor 1: BetweenFactor(1,2)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 2: BetweenFactor(2,3)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 3: BetweenFactor(3,4)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 4: BetweenFactor(4,5)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 5: BetweenFactor(5,2)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "\n" + ] + } + ], + "source": [ + "print(\"\\nFactor Graph:\\n{}\".format(graph))" + ] + }, + { + "cell_type": "markdown", + "id": "initial-estimate-markdown", + "metadata": {}, + "source": [ + "## 4. Create Initial Estimate\n", + "\n", + "We need an initial guess for the optimizer. To illustrate the optimizer's power, we provide deliberately incorrect initial values for the poses in a `gtsam.Values` container. Without the loop closure, these errors would likely accumulate significantly." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "initial-estimate-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Initial Estimate:\n", + "Values with 5 values:\n", + "Value 1: (gtsam::Pose2)\n", + "(0.5, 0, 0.2)\n", + "\n", + "Value 2: (gtsam::Pose2)\n", + "(2.3, 0.1, -0.2)\n", + "\n", + "Value 3: (gtsam::Pose2)\n", + "(4.1, 0.1, 1.57079633)\n", + "\n", + "Value 4: (gtsam::Pose2)\n", + "(4, 2, 3.14159265)\n", + "\n", + "Value 5: (gtsam::Pose2)\n", + "(2.1, 2.1, -1.57079633)\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# 3. Create the initial estimate for the solution\n", + "# These values are deliberately incorrect to show optimization.\n", + "initial_estimate = gtsam.Values()\n", + "initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))\n", + "initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))\n", + "initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))\n", + "initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))\n", + "initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))\n", + "\n", + "print(\"\\nInitial Estimate:\\n{}\".format(initial_estimate))" + ] + }, + { + "cell_type": "markdown", + "id": "4d85a286", + "metadata": {}, + "source": [ + "Now that we have an initial estimate we can also visualize the graph:" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "id": "40471c87", + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var1\n", + "\n", + "1\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var1--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var1--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var2\n", + "\n", + "2\n", + "\n", + "\n", + "\n", + "var2--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var2--factor2\n", + "\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var2--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var3\n", + "\n", + "3\n", + "\n", + "\n", + "\n", + "var3--factor2\n", + "\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var3--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var4\n", + "\n", + "4\n", + "\n", + "\n", + "\n", + "var4--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var4--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var5\n", + "\n", + "5\n", + "\n", + "\n", + "\n", + "var5--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var5--factor5\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "display(graphviz.Source(graph.dot(initial_estimate), engine='neato'))" + ] + }, + { + "cell_type": "markdown", + "id": "optimize-markdown", + "metadata": {}, + "source": [ + "## 5. Optimize the Factor Graph\n", + "\n", + "We'll use the Gauss-Newton optimizer to find the most likely configuration of poses.\n", + "\n", + "1. Set optimization parameters using `gtsam.GaussNewtonParams` (e.g., error tolerance, max iterations).\n", + "2. Create the `gtsam.GaussNewtonOptimizer` instance with the graph, initial estimate, and parameters.\n", + "3. Run `optimizer.optimize()`." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "optimize-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Final Result:\n", + "Values with 5 values:\n", + "Value 1: (gtsam::Pose2)\n", + "(-8.50051783e-21, -7.35289215e-20, -2.34289062e-20)\n", + "\n", + "Value 2: (gtsam::Pose2)\n", + "(2, -1.53066255e-19, -3.05180521e-20)\n", + "\n", + "Value 3: (gtsam::Pose2)\n", + "(4, -3.42173677e-11, 1.57079633)\n", + "\n", + "Value 4: (gtsam::Pose2)\n", + "(4, 2, 3.14159265)\n", + "\n", + "Value 5: (gtsam::Pose2)\n", + "(2, 2, -1.57079633)\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# 4. Optimize the initial values using Gauss-Newton\n", + "parameters = gtsam.GaussNewtonParams()\n", + "\n", + "# Set optimization parameters\n", + "parameters.setRelativeErrorTol(1e-5) # Stop when change in error is small\n", + "parameters.setMaxIterations(100) # Limit iterations\n", + "\n", + "# Create the optimizer\n", + "optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)\n", + "\n", + "# Optimize!\n", + "result = optimizer.optimize()\n", + "\n", + "print(\"\\nFinal Result:\\n{}\".format(result))" + ] + }, + { + "cell_type": "markdown", + "id": "marginals-markdown", + "metadata": {}, + "source": [ + "## 6. Calculate Marginal Covariances\n", + "\n", + "After optimization, we can compute the uncertainty (covariance) associated with each estimated pose using `gtsam.Marginals`." + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "marginals-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Marginal Covariances:\n", + "X1 covariance:\n", + "[[ 9.00000000e-02 1.96306337e-18 -1.49103687e-17]\n", + " [ 1.96306337e-18 9.00000000e-02 -7.03437308e-17]\n", + " [-1.49103687e-17 -7.03437308e-17 1.00000000e-02]]\n", + "\n", + "X2 covariance:\n", + "[[ 1.30000000e-01 -3.89782542e-17 -4.37043325e-17]\n", + " [-3.89782542e-17 1.70000000e-01 2.00000000e-02]\n", + " [-4.37043325e-17 2.00000000e-02 2.00000000e-02]]\n", + "\n", + "X3 covariance:\n", + "[[ 3.62000000e-01 -3.29291732e-12 6.20000000e-02]\n", + " [-3.29291394e-12 1.62000000e-01 -2.00000000e-03]\n", + " [ 6.20000000e-02 -2.00000000e-03 2.65000000e-02]]\n", + "\n", + "X4 covariance:\n", + "[[ 0.268 -0.128 0.048]\n", + " [-0.128 0.378 -0.068]\n", + " [ 0.048 -0.068 0.028]]\n", + "\n", + "X5 covariance:\n", + "[[ 0.202 0.036 -0.018 ]\n", + " [ 0.036 0.26 -0.051 ]\n", + " [-0.018 -0.051 0.0265]]\n", + "\n" + ] + } + ], + "source": [ + "# 5. Calculate and print marginal covariances\n", + "marginals = gtsam.Marginals(graph, result)\n", + "print(\"\\nMarginal Covariances:\")\n", + "for i in range(1, 6):\n", + " print(f\"X{i} covariance:\\n{marginals.marginalCovariance(i)}\\n\")" + ] + }, + { + "cell_type": "markdown", + "id": "visualize-markdown", + "metadata": {}, + "source": [ + "## 7. Visualize Results\n", + "\n", + "Finally, we use `gtsam.utils.plot.plot_pose2` to visualize the optimized poses along with their covariance ellipses. Notice how the poses form a square, and the loop closure (connecting pose 5 back to pose 2) helps maintain this structure despite the initial errors and odometry noise. The covariance ellipses show the uncertainty, which is typically smallest at the prior (pose 1) and might be reduced near the loop closure." + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "visualize-code", + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Plot the results\n", + "fig = plt.figure(0, figsize=(8, 8))\n", + "\n", + "for i in range(1, 6):\n", + " # Plot pose with covariance ellipse\n", + " gtsam_plot.plot_pose2(fig.number, result.atPose2(i), axis_length=0.4,\n", + " covariance=marginals.marginalCovariance(i))\n", + "\n", + "# Adjust plot settings\n", + "plt.title(\"Optimized Poses with Covariance Ellipses\")\n", + "plt.xlabel(\"X axis\")\n", + "plt.ylabel(\"Y axis\")\n", + "plt.axis('equal') # Ensure equal scaling on x and y axes\n", + "plt.show() # Display the plot" + ] + }, + { + "cell_type": "markdown", + "id": "summary-markdown", + "metadata": {}, + "source": [ + "## Summary\n", + "\n", + "This example demonstrated a Pose SLAM problem where:\n", + "1. We modeled robot poses and odometry measurements using a `gtsam.NonlinearFactorGraph`.\n", + "2. A prior was added to the first pose.\n", + "3. Sequential odometry factors were added between consecutive poses.\n", + "4. A crucial loop closure factor was added, connecting the last pose back to an earlier one.\n", + "5. An inaccurate initial estimate was provided.\n", + "6. The `gtsam.GaussNewtonOptimizer` was used to find the optimal pose estimates.\n", + "7. Marginal covariances were calculated to show the uncertainty.\n", + "8. The results, including covariance ellipses, were visualized, highlighting the effect of the loop closure in correcting drift." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py deleted file mode 100644 index 300a70fbd..000000000 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ /dev/null @@ -1,102 +0,0 @@ -""" -GTSAM Copyright 2010-2018, 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 - -Simple robotics example using odometry measurements and bearing-range (laser) measurements -Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import math - -import gtsam -import gtsam.utils.plot as gtsam_plot -import matplotlib.pyplot as plt - - -def main(): - """Main runner.""" - # Create noise models - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1)) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas( - gtsam.Point3(0.2, 0.2, 0.1)) - - # 1. Create a factor graph container and add factors to it - graph = gtsam.NonlinearFactorGraph() - - # 2a. Add a prior on the first pose, setting it to the origin - # A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) - graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) - - # 2b. Add odometry factors - # Create odometry (Between) factors between consecutive poses - graph.add( - gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - - # 2c. Add the loop closure constraint - # This factor encodes the fact that we have returned to the same pose. In real - # systems, these constraints may be identified in many ways, such as appearance-based - # techniques with camera images. We will use another Between Factor to enforce this constraint: - graph.add( - gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - print("\nFactor Graph:\n{}".format(graph)) # print - - # 3. Create the data structure to hold the initial_estimate estimate to the - # solution. For illustrative purposes, these have been deliberately set to incorrect values - initial_estimate = gtsam.Values() - initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) - initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) - initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) - initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) - print("\nInitial Estimate:\n{}".format(initial_estimate)) # print - - # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer - # The optimizer accepts an optional set of configuration parameters, - # controlling things like convergence criteria, the type of linear - # system solver to use, and the amount of information displayed during - # optimization. We will set a few parameters as a demonstration. - parameters = gtsam.GaussNewtonParams() - - # Stop iterating once the change in error between steps is less than this value - parameters.setRelativeErrorTol(1e-5) - # Do not perform more than N iteration steps - parameters.setMaxIterations(100) - # Create the optimizer ... - optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) - # ... and optimize - result = optimizer.optimize() - print("Final Result:\n{}".format(result)) - - # 5. Calculate and print marginal covariances for all variables - marginals = gtsam.Marginals(graph, result) - for i in range(1, 6): - print("X{} covariance:\n{}\n".format(i, - marginals.marginalCovariance(i))) - - for i in range(1, 6): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, - marginals.marginalCovariance(i)) - - plt.axis('equal') - plt.show() - - -if __name__ == "__main__": - main() From 104bba242225d86d18f7f1bf6e8ec9050c2acdda Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Apr 2025 08:54:38 -0400 Subject: [PATCH 13/38] EKF-SLAM proto --- python/gtsam/examples/EKF_SLAM.ipynb | 619 +++++++++++++++++++++++++++ 1 file changed, 619 insertions(+) create mode 100644 python/gtsam/examples/EKF_SLAM.ipynb diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb new file mode 100644 index 000000000..9422204f1 --- /dev/null +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -0,0 +1,619 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "27cd96fc", + "metadata": {}, + "source": [ + "# EKF-SLAM using Factor Graphs (SRIF-like)" + ] + }, + { + "cell_type": "markdown", + "id": "copyright-cell-srif", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2023, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "markdown", + "id": "colab-button-cell-srif", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "intro-srif", + "metadata": {}, + "source": [ + "This notebook demonstrates 2D SLAM using GTSAM's factor graph tools in an **iterative, filter-like manner**, similar in structure to a Square Root Information Filter (SRIF) or an EKF, but leveraging factor graph elimination instead of explicit matrix operations.\n", + "\n", + "**Scenario:**\n", + "A robot moves in a circular path within a 10x10 meter area containing randomly placed landmarks. It receives noisy odometry and bearing-range measurements.\n", + "\n", + "**Factor Graph Filtering Approach:**\n", + "Instead of maintaining a large state vector and covariance matrix explicitly (like in classic EKF), we use GTSAM's `NonlinearFactorGraph` and `GaussianFactorGraph`:\n", + "\n", + "1. **Belief Representation:** The robot's belief (estimated state and uncertainty) after each step is implicitly represented by a factor graph (or the resulting Bayes Net after elimination).\n", + "2. **Prediction:**\n", + " * Start with the factors representing the belief from the *previous* step.\n", + " * Add a new `BetweenFactorPose2` representing the noisy odometry measurement, connecting the previous pose `X(k)` to the new pose `X(k+1)`.\n", + " * Linearize all relevant factors around the current best estimate (`Values`).\n", + " * Eliminate the resulting `GaussianFactorGraph` using an ordering that marginalizes out the previous pose `X(k)`, yielding a `GaussianBayesNet` representing the *predictive* distribution over landmarks and `X(k+1)`.\n", + "3. **Update:**\n", + " * Start with the factors representing the *predictive* belief.\n", + " * For each landmark measurement at the new pose `X(k+1)`:\n", + " * If the landmark `L(j)` is new, add it to the `Values` (initial estimate).\n", + " * Add a `BearingRangeFactor2D` connecting `X(k+1)` and `L(j)`.\n", + " * Linearize new/updated factors.\n", + " * Eliminate the `GaussianFactorGraph` to incorporate the measurement information, yielding a `GaussianBayesNet` representing the *posterior* belief.\n", + " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", + "4. **Iteration:** Repeat prediction and update for each time step.\n", + "\n", + "**Advantages:**\n", + "* Leverages GTSAM's robust factor graph machinery.\n", + "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", + "* Avoids explicit management of large, dense covariance matrices.\n", + "\n", + "**Output:** The process will be visualized using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + ] + }, + { + "cell_type": "markdown", + "id": "setup-imports-srif", + "metadata": {}, + "source": [ + "## 1. Setup and Imports" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "install-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM and Plotly from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop plotly\n", + "except ImportError:\n", + " pass # Not in Colab" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "imports-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt # For initial plot if desired\n", + "import plotly.graph_objects as go\n", + "from tqdm.notebook import tqdm # Progress bar\n", + "import math\n", + "\n", + "import gtsam\n", + "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks" + ] + }, + { + "cell_type": "markdown", + "id": "params-srif", + "metadata": {}, + "source": [ + "## 2. Simulation Parameters" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "params-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "# World parameters\n", + "NUM_LANDMARKS = 15\n", + "WORLD_SIZE = 10.0 # Environment bounds [-WORLD_SIZE/2, WORLD_SIZE/2]\n", + "\n", + "# Robot parameters\n", + "ROBOT_RADIUS = 3.0\n", + "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", + "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", + "DT = 1.0 # Time step duration (simplified)\n", + "\n", + "# Noise parameters (GTSAM Noise Models)\n", + "# Prior noise on the first pose (x, y, theta)\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, np.deg2rad(1.0)]))\n", + "# Odometry noise (dx, dy, dtheta)\n", + "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.05, np.deg2rad(2.0)]))\n", + "# Measurement noise (bearing, range)\n", + "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", + "\n", + "# Create samplers for noise models\n", + "prior_noise_sampler = gtsam.Sampler(PRIOR_NOISE, 42)\n", + "odometry_noise_sampler = gtsam.Sampler(ODOMETRY_NOISE, 42)\n", + "measurement_noise_sampler = gtsam.Sampler(MEASUREMENT_NOISE, 42)\n", + "\n", + "# Sensor parameters\n", + "MAX_SENSOR_RANGE = 5.0" + ] + }, + { + "cell_type": "markdown", + "id": "ground-truth-srif", + "metadata": {}, + "source": [ + "## 3. Generate Ground Truth Data\n", + "\n", + "Create true landmark positions, robot path, and simulate noisy measurements." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ground-truth-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "# 3.1 Ground Truth Landmarks\n", + "landmarks_gt = (np.random.rand(2, NUM_LANDMARKS) - 0.5) * WORLD_SIZE\n", + "landmark_ids = list(range(NUM_LANDMARKS))\n", + "landmarks_gt_dict = {L(i): gtsam.Point2(landmarks_gt[:, i]) for i in range(NUM_LANDMARKS)}\n", + "\n", + "# 3.2 Ground Truth Robot Path\n", + "poses_gt = []\n", + "current_pose_gt = gtsam.Pose2(ROBOT_RADIUS, 0, np.pi / 2) # Start on circle edge\n", + "poses_gt.append(current_pose_gt)\n", + "\n", + "for k in range(NUM_STEPS):\n", + " delta_theta = ROBOT_ANGULAR_VEL * DT\n", + " arc_length = ROBOT_ANGULAR_VEL * ROBOT_RADIUS * DT\n", + " motion_command = gtsam.Pose2(arc_length, 0, delta_theta)\n", + " current_pose_gt = current_pose_gt.compose(motion_command)\n", + " poses_gt.append(current_pose_gt)\n", + "\n", + "# 3.3 Simulate Noisy Odometry Measurements (as Pose2 objects)\n", + "odometry_measurements = []\n", + "for k in range(NUM_STEPS):\n", + " pose_k = poses_gt[k]\n", + " pose_k1 = poses_gt[k+1]\n", + " true_odom = pose_k.between(pose_k1)\n", + " \n", + " # Sample noise directly for Pose2 composition (approximate)\n", + " # A more rigorous approach samples from the tangent space\n", + " odom_noise_vec = odometry_noise_sampler.sample()\n", + " noisy_odom = true_odom.compose(gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]))\n", + " odometry_measurements.append(noisy_odom)\n", + "\n", + "# 3.4 Simulate Noisy Bearing-Range Measurements\n", + "# measurements_sim[k] = list of (L(lm_id), range, bearing) for measurements *at* pose k\n", + "measurements_sim = [[] for _ in range(NUM_STEPS + 1)]\n", + "for k in range(NUM_STEPS + 1):\n", + " robot_pose = poses_gt[k]\n", + " for lm_id in range(NUM_LANDMARKS):\n", + " lm_gt_pt = landmarks_gt_dict[L(lm_id)]\n", + " try:\n", + " # Use BearingRangeFactor2D to simulate the measurement model\n", + " measurement_factor = gtsam.BearingRangeFactor2D(X(k), L(lm_id), \n", + " robot_pose.bearing(lm_gt_pt),\n", + " robot_pose.range(lm_gt_pt), \n", + " MEASUREMENT_NOISE)\n", + " true_range = measurement_factor.measured().range()\n", + " true_bearing = measurement_factor.measured().bearing()\n", + "\n", + " if true_range <= MAX_SENSOR_RANGE and abs(true_bearing.theta()) 0: # Ensure range is positive\n", + " measurements_sim[k].append((L(lm_id), noisy_bearing, noisy_range))\n", + " except Exception as e:\n", + " # Catch potential errors like point being too close to the pose\n", + " print(f\"Sim Warning at step {k}, landmark {lm_id}: {e}\")\n", + " pass\n", + "\n", + "print(f\"Generated {NUM_LANDMARKS} landmarks.\")\n", + "print(f\"Generated {NUM_STEPS} ground truth poses and odometry measurements.\")\n", + "num_meas_total = sum(len(m_list) for m_list in measurements_sim)\n", + "print(f\"Generated {num_meas_total} bearing-range measurements across all steps.\")" + ] + }, + { + "cell_type": "markdown", + "id": "factor-graph-slam-impl", + "metadata": {}, + "source": [ + "## 4. Iterative Factor Graph SLAM Implementation\n", + "\n", + "Here we perform the step-by-step prediction and update using factor graphs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "factor-graph-init-code", + "metadata": {}, + "outputs": [], + "source": [ + "# --- Initialization ---\n", + "current_estimate = gtsam.Values() # Stores current best estimates (linearization points)\n", + "current_graph = gtsam.NonlinearFactorGraph() # Stores factors added so far\n", + "current_pose_key = X(0)\n", + "\n", + "# Add prior on the initial pose X(0)\n", + "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", + "current_estimate.insert(current_pose_key, initial_pose)\n", + "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", + "\n", + "# Variables to store results for animation\n", + "results_history = [current_estimate] # Store Values object at each step\n", + "marginals_history = [] # Store Marginals object at each step\n", + "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", + "\n", + "# Initial marginals (only for X(0))\n", + "try:\n", + " initial_gaussian_graph = current_graph.linearize(current_estimate)\n", + " initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", + " marginals_history.append(initial_marginals)\n", + "except Exception as e:\n", + " print(f\"Error computing initial marginals: {e}\")\n", + " marginals_history.append(None) # Append placeholder if fails\n" + ] + }, + { + "cell_type": "markdown", + "id": "factor-graph-loop", + "metadata": {}, + "source": [ + "### Main Iterative Loop" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "factor-graph-loop-code", + "metadata": {}, + "outputs": [], + "source": [ + "print(\"Running Iterative Factor Graph SLAM...\")\n", + "\n", + "for k in tqdm(range(NUM_STEPS)):\n", + " # === Prediction Step ===\n", + " prev_pose_key = X(k)\n", + " current_pose_key = X(k + 1)\n", + " odom_k = odometry_measurements[k]\n", + " \n", + " # Predict next pose and add to Values (linearization point)\n", + " predicted_pose = current_estimate.atPose2(prev_pose_key).compose(odom_k)\n", + " current_estimate.insert(current_pose_key, predicted_pose)\n", + " \n", + " # Add odometry factor to the graph\n", + " current_graph.add(gtsam.BetweenFactorPose2(prev_pose_key, current_pose_key, odom_k, ODOMETRY_NOISE))\n", + "\n", + " # --- At this point, the full graph contains the history --- \n", + " # --- To mimic SRIF/EKF, we *could* eliminate prev_pose_key, but --- \n", + " # --- it's simpler here to keep the full graph and optimize it --- \n", + " # --- then extract the current belief for the next step's factors --- \n", + " # --- Let's proceed with adding measurements first ----\n", + "\n", + " # === Update Step ===\n", + " measurements_k1 = measurements_sim[k + 1] # Measurements taken AT pose k+1\n", + " landmarks_observed_this_step = set()\n", + "\n", + " for lm_key, measured_bearing, measured_range in measurements_k1:\n", + " landmarks_observed_this_step.add(lm_key)\n", + " \n", + " # If landmark is new, initialize its estimate\n", + " if not current_estimate.exists(lm_key):\n", + " # Initial guess based on current pose estimate and measurement\n", + " current_pose_val = current_estimate.atPose2(current_pose_key)\n", + " delta_x = measured_range * math.cos(measured_bearing.theta())\n", + " delta_y = measured_range * math.sin(measured_bearing.theta())\n", + " # Transform delta from robot frame to world frame\n", + " lm_initial_guess = current_pose_val.transformFrom(gtsam.Point2(delta_x, delta_y))\n", + " current_estimate.insert(lm_key, lm_initial_guess)\n", + " known_landmarks.add(lm_key)\n", + " # print(f\"Step {k+1}: Initialized Landmark {lm_key.index()}\")\n", + "\n", + " # Add measurement factor\n", + " current_graph.add(gtsam.BearingRangeFactor2D(current_pose_key, lm_key, \n", + " measured_bearing, measured_range,\n", + " MEASUREMENT_NOISE))\n", + " \n", + " # --- Optimization (Incremental Smoothing) ---\n", + " # In a true filter, we'd eliminate previous states.\n", + " # Here, we re-optimize the growing graph. For efficiency in large problems, \n", + " # iSAM2 would be used, but Levenberg-Marquardt on the growing graph \n", + " # demonstrates the concept for this smaller example.\n", + " \n", + " optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n", + " try:\n", + " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", + " except Exception as e:\n", + " print(f\"Optimization failed at step {k+1}: {e}\")\n", + " # Potentially revert estimate update or handle error\n", + " # For simplicity, we continue with the potentially unoptimized estimate\n", + " pass \n", + "\n", + " # Store results for animation\n", + " results_history.append(gtsam.Values(current_estimate)) # Store a copy\n", + " \n", + " # Calculate marginals for visualization (can be slow for large graphs)\n", + " try:\n", + " current_gaussian_graph = current_graph.linearize(current_estimate)\n", + " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", + " marginals_history.append(current_marginals)\n", + " except Exception as e:\n", + " print(f\"Marginals calculation failed at step {k+1}: {e}\")\n", + " marginals_history.append(None) # Append placeholder\n", + "\n", + "print(\"Iterative Factor Graph SLAM finished.\")\n", + "print(f\"Final number of poses: {k+2}\")\n", + "print(f\"Number of landmarks mapped: {len(known_landmarks)}\")" + ] + }, + { + "cell_type": "markdown", + "id": "plotly-animation", + "metadata": {}, + "source": [ + "## 5. Create Plotly Animation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "plotly-animation-code", + "metadata": { + "scrolled": false, + "tags": [ + "remove-input" + ] + }, + "outputs": [], + "source": [ + "print(\"Generating Plotly animation...\")\n", + "\n", + "def ellipse_path(cx, cy, sizex, sizey, angle, N=60):\n", + " \"\"\"SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.\"\"\"\n", + " angle_rad = np.radians(angle)\n", + " t = np.linspace(0, 2 * np.pi, N)\n", + " x = (sizex / 2) * np.cos(t)\n", + " y = (sizey / 2) * np.sin(t)\n", + "\n", + " x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad)\n", + " y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad)\n", + "\n", + " path = f\"M {x_rot[0]},{y_rot[0]} \" + \" \".join(\n", + " f\"L{x_},{y_}\" for x_, y_ in zip(x_rot[1:], y_rot[1:])\n", + " ) + \" Z\"\n", + " return path\n", + "\n", + "# Helper to convert GTSAM covariance to Plotly ellipse parameters\n", + "def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0):\n", + " \"\"\"Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.\"\"\"\n", + " # Ensure positive definite - add small epsilon if needed\n", + " cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 \n", + " try:\n", + " eigvals, eigvecs = np.linalg.eigh(cov)\n", + " # Ensure eigenvalues are positive for sqrt\n", + " eigvals = np.maximum(eigvals, 1e-9)\n", + " except np.linalg.LinAlgError:\n", + " print(\"Warning: Covariance matrix SVD failed, using default ellipse.\")\n", + " return 0, 0.1*scale, 0.1*scale # Default small ellipse\n", + " \n", + " # Width/Height are 2*scale*sqrt(eigenvalue)\n", + " width = 2 * scale * np.sqrt(eigvals[1])\n", + " height = 2 * scale * np.sqrt(eigvals[0])\n", + " \n", + " # Angle of the major axis (corresponding to largest eigenvalue)\n", + " angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1])\n", + " angle_deg = np.degrees(angle_rad)\n", + " \n", + " return angle_deg, width, height\n", + "\n", + "# --- Create Plotly Figure --- \n", + "fig = go.Figure()\n", + "\n", + "# Add Ground Truth Landmarks (static)\n", + "fig.add_trace(go.Scatter(\n", + " x=landmarks_gt[0, :], \n", + " y=landmarks_gt[1, :], \n", + " mode='markers', \n", + " marker=dict(color='black', size=8, symbol='star'),\n", + " name='Landmarks GT'\n", + "))\n", + "\n", + "# Add Ground Truth Path (static)\n", + "gt_path_x = [p.x() for p in poses_gt]\n", + "gt_path_y = [p.y() for p in poses_gt]\n", + "fig.add_trace(go.Scatter(\n", + " x=gt_path_x,\n", + " y=gt_path_y,\n", + " mode='lines',\n", + " line=dict(color='gray', width=1, dash='dash'),\n", + " name='Path GT'\n", + "))\n", + "\n", + "# --- Animation Frames --- \n", + "frames = []\n", + "steps = list(range(NUM_STEPS + 1))\n", + "\n", + "for k in tqdm(steps):\n", + " frame_data = []\n", + " step_results = results_history[k]\n", + " step_marginals = marginals_history[k]\n", + " \n", + " # Estimated Path up to step k\n", + " est_path_x = [results_history[i].atPose2(X(i)).x() for i in range(k + 1)]\n", + " est_path_y = [results_history[i].atPose2(X(i)).y() for i in range(k + 1)]\n", + " frame_data.append(go.Scatter(\n", + " x=est_path_x,\n", + " y=est_path_y,\n", + " mode='lines+markers',\n", + " line=dict(color='red', width=2),\n", + " marker=dict(size=4, color='red'),\n", + " name='Path Est'\n", + " ))\n", + " \n", + " # Estimated Landmarks known at step k\n", + " est_landmarks_x = []\n", + " est_landmarks_y = []\n", + " landmark_keys_in_frame = []\n", + " for lm_key in step_results.keys():\n", + " if gtsam.symbolChr(lm_key)==108: # Check if it's a landmark key\n", + " lm_pose = step_results.atPoint2(lm_key)\n", + " est_landmarks_x.append(lm_pose[0])\n", + " est_landmarks_y.append(lm_pose[1])\n", + " landmark_keys_in_frame.append(lm_key)\n", + " \n", + " if est_landmarks_x:\n", + " frame_data.append(go.Scatter(\n", + " x=est_landmarks_x, \n", + " y=est_landmarks_y, \n", + " mode='markers',\n", + " marker=dict(color='blue', size=6, symbol='x'),\n", + " name='Landmarks Est'\n", + " ))\n", + "\n", + " # Current Pose Covariance Ellipse\n", + " shapes = [] # List to hold ellipse shapes for this frame\n", + " if step_marginals is not None:\n", + " try:\n", + " current_pose_key = X(k)\n", + " pose_cov = step_marginals.marginalCovariance(current_pose_key)\n", + " pose_mean = step_results.atPose2(current_pose_key).translation()\n", + " angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov)\n", + " cx, cy = pose_mean[0], pose_mean[1]\n", + " shapes.append(dict(\n", + " type=\"path\",\n", + " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", + " xref=\"x\", yref=\"y\",\n", + " fillcolor=\"rgba(255,0,255,0.2)\",\n", + " line_color=\"rgba(255,0,255,0.5)\",\n", + " name=f'Pose {k} Cov'\n", + " ))\n", + " except Exception as e:\n", + " print(f\"Warning: Failed getting pose cov ellipse at step {k}: {e}\")\n", + "\n", + " # Landmark Covariance Ellipses\n", + " for lm_key in landmark_keys_in_frame:\n", + " index = gtsam.symbolIndex(lm_key)\n", + " try:\n", + " lm_cov = step_marginals.marginalCovariance(lm_key)\n", + " lm_mean = step_results.atPoint2(lm_key)\n", + " angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov)\n", + " cx, cy = lm_mean[0], lm_mean[1]\n", + " shapes.append(dict(\n", + " type=\"path\",\n", + " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", + " xref=\"x\", yref=\"y\",\n", + " fillcolor=\"rgba(0,0,255,0.1)\",\n", + " line_color=\"rgba(0,0,255,0.3)\",\n", + " name=f'LM {index} Cov'\n", + " ))\n", + " except Exception as e:\n", + " print(f\"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}\")\n", + "\n", + " frames.append(go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)))\n", + "\n", + "# --- Set Initial State and Layout --- \n", + "fig.update(frames=frames)\n", + "\n", + "# Set initial data to the first frame's data\n", + "if frames:\n", + " fig.add_traces(frames[0].data)\n", + " initial_shapes = frames[0].layout.shapes if frames[0].layout else []\n", + "else:\n", + " initial_shapes = []\n", + "\n", + "# Define slider\n", + "sliders = [dict(\n", + " active=0,\n", + " currentvalue={\"prefix\": \"Step: \"},\n", + " pad={\"t\": 50},\n", + " steps=[dict(label=str(k), \n", + " method='animate', \n", + " args=[[str(k)], \n", + " dict(mode='immediate', \n", + " frame=dict(duration=100, redraw=True), \n", + " transition=dict(duration=0))])\n", + " for k in steps]\n", + ")]\n", + "\n", + "# Update layout\n", + "fig.update_layout(\n", + " title='Iterative Factor Graph SLAM Animation',\n", + " xaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], constrain='domain'),\n", + " yaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], scaleanchor='x', scaleratio=1),\n", + " width=800, height=800,\n", + " hovermode='closest',\n", + " updatemenus=[dict(type='buttons', \n", + " showactive=False,\n", + " buttons=[dict(label='Play',\n", + " method='animate', \n", + " args=[None, \n", + " dict(mode='immediate', \n", + " frame=dict(duration=100, redraw=True), \n", + " transition=dict(duration=0), \n", + " fromcurrent=True)])])],\n", + " sliders=sliders,\n", + " shapes=initial_shapes # Set initial shapes\n", + ")\n", + "\n", + "print(\"Displaying animation...\")\n", + "fig.show()" + ] + }, + { + "cell_type": "markdown", + "id": "discussion-srif", + "metadata": {}, + "source": [ + "## 6. Discussion\n", + "\n", + "* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n", + "* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n", + "* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n", + "* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.12" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} From 5b91110c38e1bdd5b5f3627dda0afac02b4758d7 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 23 Apr 2025 21:21:23 -0700 Subject: [PATCH 14/38] Add wrapper for imuBias in iSAM2 --- gtsam/nonlinear/nonlinear.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 43f5913eb..6f71fe207 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -549,7 +549,7 @@ class ISAM2 { gtsam::Values calculateEstimate() const; template , gtsam::PinholeCamera, @@ -626,6 +626,7 @@ template , gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::NavState, gtsam::imuBias::ConstantBias}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, From c28c9b58d6442170b980feb4000ccd7411bd48d5 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 24 Apr 2025 10:52:25 -0400 Subject: [PATCH 15/38] New SLAM notebooks --- gtsam/slam/doc/BetweenFactor.ipynb | 322 ++++++++++++++ .../slam/doc/EssentialMatrixConstraint.ipynb | 212 +++++++++ gtsam/slam/doc/EssentialMatrixFactor.ipynb | 392 ++++++++++++++++ gtsam/slam/doc/FrobeniusFactor.ipynb | 269 +++++++++++ gtsam/slam/doc/GeneralSFMFactor.ipynb | 223 ++++++++++ gtsam/slam/doc/InitializePose3.ipynb | 256 +++++++++++ gtsam/slam/doc/KarcherMeanFactor.ipynb | 245 ++++++++++ gtsam/slam/doc/OrientedPlane3Factor.ipynb | 230 ++++++++++ gtsam/slam/doc/PlanarProjectionFactor.ipynb | 293 ++++++++++++ gtsam/slam/doc/PoseRotationPrior.ipynb | 215 +++++++++ gtsam/slam/doc/PoseTranslationPrior.ipynb | 214 +++++++++ gtsam/slam/doc/ProjectionFactor.ipynb | 238 ++++++++++ gtsam/slam/doc/ReferenceFrameFactor.ipynb | 219 +++++++++ gtsam/slam/doc/RotateFactor.ipynb | 229 ++++++++++ gtsam/slam/doc/SmartFactorParams.ipynb | 221 +++++++++ gtsam/slam/doc/SmartProjectionFactor.ipynb | 343 ++++++++++++++ .../slam/doc/SmartProjectionPoseFactor.ipynb | 354 +++++++++++++++ gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 418 ++++++++++++++++++ gtsam/slam/doc/StereoFactor.ipynb | 246 +++++++++++ gtsam/slam/doc/TriangulationFactor.ipynb | 283 ++++++++++++ gtsam/slam/doc/dataset.ipynb | 252 +++++++++++ gtsam/slam/doc/lago.ipynb | 242 ++++++++++ 22 files changed, 5916 insertions(+) create mode 100644 gtsam/slam/doc/BetweenFactor.ipynb create mode 100644 gtsam/slam/doc/EssentialMatrixConstraint.ipynb create mode 100644 gtsam/slam/doc/EssentialMatrixFactor.ipynb create mode 100644 gtsam/slam/doc/FrobeniusFactor.ipynb create mode 100644 gtsam/slam/doc/GeneralSFMFactor.ipynb create mode 100644 gtsam/slam/doc/InitializePose3.ipynb create mode 100644 gtsam/slam/doc/KarcherMeanFactor.ipynb create mode 100644 gtsam/slam/doc/OrientedPlane3Factor.ipynb create mode 100644 gtsam/slam/doc/PlanarProjectionFactor.ipynb create mode 100644 gtsam/slam/doc/PoseRotationPrior.ipynb create mode 100644 gtsam/slam/doc/PoseTranslationPrior.ipynb create mode 100644 gtsam/slam/doc/ProjectionFactor.ipynb create mode 100644 gtsam/slam/doc/ReferenceFrameFactor.ipynb create mode 100644 gtsam/slam/doc/RotateFactor.ipynb create mode 100644 gtsam/slam/doc/SmartFactorParams.ipynb create mode 100644 gtsam/slam/doc/SmartProjectionFactor.ipynb create mode 100644 gtsam/slam/doc/SmartProjectionPoseFactor.ipynb create mode 100644 gtsam/slam/doc/SmartProjectionRigFactor.ipynb create mode 100644 gtsam/slam/doc/StereoFactor.ipynb create mode 100644 gtsam/slam/doc/TriangulationFactor.ipynb create mode 100644 gtsam/slam/doc/dataset.ipynb create mode 100644 gtsam/slam/doc/lago.ipynb diff --git a/gtsam/slam/doc/BetweenFactor.ipynb b/gtsam/slam/doc/BetweenFactor.ipynb new file mode 100644 index 000000000..4eef414bb --- /dev/null +++ b/gtsam/slam/doc/BetweenFactor.ipynb @@ -0,0 +1,322 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# BetweenFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`BetweenFactor` is a fundamental factor in GTSAM used to represent measurements of the relative transformation (motion) between two variables of the same type `VALUE`.\n", + "Common examples include:\n", + "* `BetweenFactorPose2`: Represents odometry measurements between 2D robot poses.\n", + "* `BetweenFactorPose3`: Represents odometry or relative pose estimates between 3D poses.\n", + "\n", + "The `VALUE` type must be a Lie Group (e.g., `Pose2`, `Pose3`, `Rot2`, `Rot3`).\n", + "\n", + "The factor encodes a constraint based on the measurement `measured_`, such that the error is calculated based on the difference between the predicted relative transformation and the measured one. Specifically, if the factor connects keys $k_1$ and $k_2$ corresponding to values $X_1$ and $X_2$, and the measurement is $Z$, the factor aims to minimize:\n", + "\n", + "$$ \\text{error}(X_1, X_2) = \\text{Log}(Z^{-1} \\cdot (X_1^{-1} \\cdot X_2)) $$\n", + "\n", + "where `Log` is the logarithmic map for the Lie group `VALUE`, $X_1^{-1} \\cdot X_2$ is the predicted relative transformation `between(X1, X2)`, and $Z^{-1}$ is the inverse of the measurement. The error vector lives in the tangent space of the identity element of the group.\n", + "\n", + "`BetweenConstraint` is a derived class that uses a `noiseModel::Constrained` noise model, effectively enforcing the relative transformation to be exactly the measured value." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import BetweenFactorPose2, BetweenFactorPose3\n", + "from gtsam import Pose2, Pose3, Rot3, Point3\n", + "from gtsam import NonlinearFactorGraph, Values\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a BetweenFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "You create a `BetweenFactor` by specifying:\n", + "1. The keys of the two variables it connects (e.g., `X(0)`, `X(1)`).\n", + "2. The measured relative transformation (e.g., a `Pose2` or `Pose3`).\n", + "3. A noise model describing the uncertainty of the measurement." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "BetweenFactorPose2: BetweenFactor(x0,x1)\n", + " measured: (1, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "BetweenFactorPose3: BetweenFactor(x1,x2)\n", + " measured: R: [\n", + "\t0.995004165, -0.0998334166, 0;\n", + "\t0.0998334166, 0.995004165, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.5 0 0\n", + " noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.1; 0.1; 0.1];\n" + ] + } + ], + "source": [ + "# Example for Pose2 (2D SLAM odometry)\n", + "key1 = X(0)\n", + "key2 = X(1)\n", + "measured_pose2 = Pose2(1.0, 0.0, 0.0) # Move 1 meter forward\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", + "\n", + "between_factor_pose2 = BetweenFactorPose2(key1, key2, measured_pose2, odometry_noise)\n", + "between_factor_pose2.print(\"BetweenFactorPose2: \")\n", + "\n", + "# Example for Pose3 (3D SLAM odometry)\n", + "measured_pose3 = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0)) # Move 0.5m forward, yaw 0.1 rad\n", + "odometry_noise_3d = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1]))\n", + "\n", + "between_factor_pose3 = BetweenFactorPose3(X(1), X(2), measured_pose3, odometry_noise_3d)\n", + "between_factor_pose3.print(\"\\nBetweenFactorPose3: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method calculates the error vector based on the current estimates of the variables in the `Values` object." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error vector for BetweenFactorPose2: 0.3750000000000002\n", + "Manual unwhitened error calculation: [0.10247917 0.09747917 0.05 ]\n", + "Factor unwhitened error: [0.1 0.1 0.05]\n", + "Manually whitened error: [0.51239583 0.48739583 0.5 ]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "values.insert(X(0), Pose2(0.0, 0.0, 0.0))\n", + "values.insert(X(1), Pose2(1.1, 0.1, 0.05)) # Slightly off from measurement\n", + "\n", + "# Evaluate the error for the Pose2 factor\n", + "error_vector = between_factor_pose2.error(values)\n", + "print(f\"Error vector for BetweenFactorPose2: {error_vector}\")\n", + "\n", + "# The unwhitened error is Log(Z^-1 * (X1^-1 * X2))\n", + "pose0 = values.atPose2(X(0))\n", + "pose1 = values.atPose2(X(1))\n", + "predicted_relative = pose0.between(pose1)\n", + "error_pose = measured_pose2.inverse() * predicted_relative\n", + "unwhitened_error_expected = Pose2.Logmap(error_pose)\n", + "print(f\"Manual unwhitened error calculation: {unwhitened_error_expected}\")\n", + "print(f\"Factor unwhitened error: {between_factor_pose2.unwhitenedError(values)}\")\n", + "\n", + "# The whitened error (returned by error()) applies the noise model\n", + "# For diagonal noise model, error_vector = unwhitened_error / sigmas\n", + "sigmas = odometry_noise.sigmas()\n", + "whitened_expected = unwhitened_error_expected / sigmas\n", + "print(f\"Manually whitened error: {whitened_expected}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "viz_header_md" + }, + "source": [ + "## Visualization" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "viz_example_code" + }, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320\n", + "\n", + "x0\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352320--factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor1\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "graph.add(between_factor_pose2)\n", + "graph.add(between_factor_pose3)\n", + "\n", + "dot_string = graph.dot(values)\n", + "graphviz.Source(dot_string)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/EssentialMatrixConstraint.ipynb b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb new file mode 100644 index 000000000..fe54c0515 --- /dev/null +++ b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb @@ -0,0 +1,212 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# EssentialMatrixConstraint" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`EssentialMatrixConstraint` is a binary factor connecting two `Pose3` variables.\n", + "It represents a constraint derived from a measured Essential matrix ($E$) between the two camera views corresponding to the poses.\n", + "The Essential matrix $E$ encapsulates the relative rotation $R$ and translation direction $t$ between two *calibrated* cameras according to the epipolar constraint:\n", + "$$ p_2^T E p_1 = 0 $$\n", + "where $p_1$ and $p_2$ are the homogeneous coordinates of corresponding points in the *normalized (calibrated)* image planes.\n", + "\n", + "This factor takes the measured $E_{12}$ (from pose 1 to pose 2) and compares it to the Essential matrix predicted from the estimated poses $P_1$ and $P_2$.\n", + "The error is computed in the 5-dimensional tangent space of the Essential matrix manifold.\n", + "\n", + "**Note:** This factor requires known camera calibration, as the Essential matrix operates on normalized image coordinates." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import EssentialMatrixConstraint, EssentialMatrix, Pose3, Rot3, Point3, Values\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating an EssentialMatrixConstraint" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "To create the factor, provide:\n", + "1. Keys for the two `Pose3` variables (e.g., `X(1)`, `X(2)`).\n", + "2. The measured `gtsam.EssentialMatrix` ($E_{12}$).\n", + "3. A 5-dimensional noise model (`gtsam.noiseModel`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "EssentialMatrixConstraint: EssentialMatrixConstraint(x1,x2)\n", + " measured: R:\n", + " [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "d: :1\n", + "0\n", + "0\n", + "isotropic dim=5 sigma=0.01\n" + ] + } + ], + "source": [ + "# Assume we have two poses\n", + "pose1 = Pose3(Rot3.Yaw(0.0), Point3(0, 0, 0))\n", + "pose2 = Pose3(Rot3.Yaw(0.1), Point3(1, 0, 0))\n", + "\n", + "# Calculate the ground truth Essential matrix between them\n", + "gt_E12 = EssentialMatrix.FromPose3(pose1.between(pose2))\n", + "\n", + "# Add some noise (conceptual, E lives on a manifold)\n", + "# In practice, E would be estimated from image correspondences\n", + "measured_E = gt_E12 # Use ground truth for this example\n", + "\n", + "# Define a noise model (5 dimensional!)\n", + "noise_dim = 5\n", + "E_noise = gtsam.noiseModel.Isotropic.Sigma(noise_dim, 0.01)\n", + "\n", + "# Create the factor\n", + "key1 = X(1)\n", + "key2 = X(2)\n", + "factor = EssentialMatrixConstraint(key1, key2, measured_E, E_noise)\n", + "\n", + "factor.print(\"EssentialMatrixConstraint: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method computes the error vector in the 5D tangent space of the Essential matrix manifold. The error represents the difference between the measured E and the E predicted from the current pose estimates in `values`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error vector at ground truth: 0.0\n", + "Error vector at noisy pose: 1.4069198000486227\n" + ] + } + ], + "source": [ + "values = Values()\n", + "# Insert values close to ground truth\n", + "values.insert(key1, pose1)\n", + "values.insert(key2, pose2)\n", + "\n", + "error_vector = factor.error(values)\n", + "print(f\"Error vector at ground truth: {error_vector}\")\n", + "\n", + "# Insert values slightly different\n", + "noisy_pose2 = Pose3(Rot3.Yaw(0.11), Point3(1.05, 0.01, -0.01))\n", + "values.update(key2, noisy_pose2)\n", + "\n", + "error_vector_noisy = factor.error(values)\n", + "print(f\"Error vector at noisy pose: {error_vector_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/EssentialMatrixFactor.ipynb b/gtsam/slam/doc/EssentialMatrixFactor.ipynb new file mode 100644 index 000000000..cac43612c --- /dev/null +++ b/gtsam/slam/doc/EssentialMatrixFactor.ipynb @@ -0,0 +1,392 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# EssentialMatrixFactor Variants" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines several factors related to the Essential matrix ($E$), which encodes the relative rotation and translation direction between two *calibrated* cameras.\n", + "They are primarily used in Structure from Motion (SfM) problems where point correspondences between views are available but the 3D structure and/or camera poses/calibration are unknown.\n", + "\n", + "The core constraint is the epipolar constraint: $p_2^T E p_1 = 0$, where $p_1$ and $p_2$ are corresponding points in *normalized (calibrated)* image coordinates.\n", + "\n", + "Factors defined here:\n", + "* `EssentialMatrixFactor`: Constrains an unknown `EssentialMatrix` variable using a single point correspondence $(p_1, p_2)$.\n", + "* `EssentialMatrixFactor2`: Constrains an `EssentialMatrix` and an unknown inverse depth variable using a point correspondence.\n", + "* `EssentialMatrixFactor3`: Like Factor2, but incorporates an additional fixed extrinsic rotation (useful for camera rigs).\n", + "* `EssentialMatrixFactor4`: Constrains an `EssentialMatrix` and a *shared* `CALIBRATION` variable using a point correspondence given in *pixel* coordinates.\n", + "* `EssentialMatrixFactor5`: Constrains an `EssentialMatrix` and *two* unknown `CALIBRATION` variables (one for each camera) using a point correspondence given in *pixel* coordinates." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (EssentialMatrix, Point2, Point3, Rot3, Unit3, Pose3, Cal3_S2, EssentialMatrixFactor,\n", + " EssentialMatrixFactor2, EssentialMatrixFactor3, EssentialMatrixFactor4Cal3_S2,\n", + " EssentialMatrixFactor5Cal3_S2, Values)\n", + "from gtsam import symbol_shorthand\n", + "\n", + "E = symbol_shorthand.E\n", + "K = symbol_shorthand.K\n", + "D = symbol_shorthand.D # For inverse depth" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `EssentialMatrixFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "This factor involves a single `EssentialMatrix` variable. It takes a pair of corresponding points $(p_A, p_B)$ in *normalized (calibrated)* image coordinates and penalizes deviations from the epipolar constraint $p_B^T E p_A = 0$.\n", + "The error is $p_B^T E p_A$." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor 1: keys = { e0 }\n", + "isotropic dim=1 sigma=0.01\n", + " EssentialMatrixFactor with measurements\n", + " (0.5 0.2 1)' and ( 0.4 0.25 1)'\n", + "\n", + "Error for Factor 1: 12.499999999999995\n" + ] + } + ], + "source": [ + "# Assume normalized coordinates\n", + "pA_calibrated = Point2(0.5, 0.2)\n", + "pB_calibrated = Point2(0.4, 0.25)\n", + "\n", + "# Noise model on the epipolar error (scalar)\n", + "epipolar_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)\n", + "\n", + "# Key for the unknown Essential Matrix\n", + "keyE = E(0)\n", + "\n", + "factor1 = EssentialMatrixFactor(keyE, pA_calibrated, pB_calibrated, epipolar_noise)\n", + "factor1.print(\"Factor 1: \")\n", + "\n", + "# Evaluate error (requires an EssentialMatrix value)\n", + "values = Values()\n", + "# Example: E for identity rotation, translation (1,0,0)\n", + "example_E = EssentialMatrix(Rot3(), Unit3(1, 0, 0))\n", + "values.insert(keyE, example_E)\n", + "error1 = factor1.error(values)\n", + "print(f\"\\nError for Factor 1: {error1}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `EssentialMatrixFactor2`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "This factor involves an `EssentialMatrix` variable and a `double` variable representing the *inverse depth* of the 3D point corresponding to the measurement $p_A$ in the first camera's frame.\n", + "It assumes the measurement $p_B$ is perfect and calculates the reprojection error of the point (reconstructed using $p_A$ and the inverse depth) in the first camera image, after projecting it into the second camera and back.\n", + "It requires point correspondences $(p_A, p_B)$, which can be provided in either calibrated or pixel coordinates (if a calibration object `K` is provided).\n", + "The error is a 2D reprojection error in the first image plane (typically in pixels if K is provided)." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 2: keys = { e0 d0 }\n", + " noise model: unit (2) \n", + " EssentialMatrixFactor2 with measurements\n", + " (480 288 1)' and (464 312)'\n", + "\n", + "Error for Factor 2: 412.82000000000016\n" + ] + } + ], + "source": [ + "# Assume pixel coordinates and known calibration\n", + "K_cal = Cal3_S2(500, 500, 0, 320, 240)\n", + "pA_pixels = Point2(480, 288) # Corresponds to (0.5, 0.2) calibrated\n", + "pB_pixels = Point2(464, 312) # Corresponds to (0.4, 0.25) calibrated\n", + "\n", + "# Noise model on the 2D reprojection error (pixels)\n", + "reprojection_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "\n", + "# Key for inverse depth\n", + "keyD = D(0)\n", + "\n", + "factor2 = EssentialMatrixFactor2(keyE, keyD, pA_pixels, pB_pixels, reprojection_noise)\n", + "factor2.print(\"\\nFactor 2: \")\n", + "\n", + "# Evaluate error (requires E and inverse depth d)\n", + "values.insert(keyD, 0.2) # Assume inverse depth d = 1/Z = 1/5 = 0.2\n", + "error2 = factor2.error(values)\n", + "print(f\"\\nError for Factor 2: {error2}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_header_md" + }, + "source": [ + "## 3. `EssentialMatrixFactor3`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_desc_md" + }, + "source": [ + "This is identical to `EssentialMatrixFactor2` but includes an additional fixed `Rot3` representing the rotation from the 'body' frame (where the Essential matrix is defined) to the 'camera' frame (where the measurements are made).\n", + "`iRc`: Rotation from camera frame to body frame (inverse of body-to-camera).\n", + "The Essential matrix $E_{body}$ is transformed to the camera frame before use: $E_{camera} = R_{cRb} \\cdot E_{body}$." + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": { + "id": "factor3_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 3: keys = { e0 d0 }\n", + " noise model: unit (2) \n", + " EssentialMatrixFactor2 with measurements\n", + " (480 288 1)' and (464 312)'\n", + " EssentialMatrixFactor3 with rotation [\n", + "\t0.99875, -0.0499792, 0;\n", + "\t0.0499792, 0.99875, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n", + "Error for Factor 3: 413.0638991792357\n" + ] + } + ], + "source": [ + "body_R_cam = Rot3.Yaw(0.05) # Example fixed rotation\n", + "\n", + "factor3 = EssentialMatrixFactor3(keyE, keyD, pA_pixels, pB_pixels, body_R_cam, reprojection_noise)\n", + "factor3.print(\"\\nFactor 3: \")\n", + "\n", + "# Evaluate error (uses same E and d from values)\n", + "error3 = factor3.error(values)\n", + "print(f\"\\nError for Factor 3: {error3}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor4_header_md" + }, + "source": [ + "## 4. `EssentialMatrixFactor4`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor4_desc_md" + }, + "source": [ + "This factor involves an `EssentialMatrix` variable and a single unknown `CALIBRATION` variable (e.g., `Cal3_S2`) that is assumed to be **shared** by both cameras.\n", + "It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n", + "The error is the algebraic epipolar error $ (K^{-1} p_B)^T E (K^{-1} p_A) $.\n", + "\n", + "**Note:** Recovering calibration from 2D correspondences alone is often ill-posed. This factor typically requires strong priors on the calibration." + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": { + "id": "factor4_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 4: keys = { e0 k0 }\n", + "isotropic dim=1 sigma=0.01\n", + " EssentialMatrixFactor4 with measurements\n", + " (480 288)' and (464 312)'\n", + "\n", + "Error for Factor 4: 11.520000000000007\n" + ] + } + ], + "source": [ + "# Key for the unknown shared Calibration\n", + "keyK = K(0)\n", + "\n", + "factor4 = EssentialMatrixFactor4Cal3_S2(keyE, keyK, pA_pixels, pB_pixels, epipolar_noise)\n", + "factor4.print(\"\\nFactor 4: \")\n", + "\n", + "# Evaluate error (requires E and K)\n", + "values.insert(keyK, K_cal) # Use the known K for this example\n", + "error4 = factor4.error(values)\n", + "print(f\"\\nError for Factor 4: {error4}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor5_header_md" + }, + "source": [ + "## 5. `EssentialMatrixFactor5`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor5_desc_md" + }, + "source": [ + "Similar to Factor4, but allows for **two different** unknown `CALIBRATION` variables, one for each camera ($K_A$ and $K_B$).\n", + "It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n", + "The error is the algebraic epipolar error $ (K_B^{-1} p_B)^T E (K_A^{-1} p_A) $.\n", + "\n", + "**Note:** Like Factor4, this is often ill-posed without strong priors." + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "id": "factor5_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 5: keys = { e0 k0 k1 }\n", + "isotropic dim=1 sigma=0.01\n", + " EssentialMatrixFactor5 with measurements\n", + " (480 288)' and (464 312)'\n", + "\n", + "Error for Factor 5: 11.520000000000007\n" + ] + } + ], + "source": [ + "# Keys for potentially different calibrations\n", + "keyKA = K(0) # Can reuse keyK if they are actually the same\n", + "keyKB = K(1)\n", + "\n", + "factor5 = EssentialMatrixFactor5Cal3_S2(keyE, keyKA, keyKB, pA_pixels, pB_pixels, epipolar_noise)\n", + "factor5.print(\"\\nFactor 5: \")\n", + "\n", + "# Evaluate error (requires E, KA, KB)\n", + "# values already contains E(0) and K(0)\n", + "values.insert(keyKB, K_cal) # Assume KB is also the same known K\n", + "error5 = factor5.error(values)\n", + "print(f\"\\nError for Factor 5: {error5}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/FrobeniusFactor.ipynb b/gtsam/slam/doc/FrobeniusFactor.ipynb new file mode 100644 index 000000000..fdf317da8 --- /dev/null +++ b/gtsam/slam/doc/FrobeniusFactor.ipynb @@ -0,0 +1,269 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Frobenius Norm Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors that operate directly on the entries of rotation matrices (`Rot3` or generally `SO(n)`) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.\n", + "\n", + "These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard `BetweenFactor` or `PriorFactor` on rotations.\n", + "\n", + "* `FrobeniusPrior`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n", + "* `FrobeniusFactor`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T1 - T2||_F^2$.\n", + "* `FrobeniusBetweenFactor`: Penalizes the Frobenius norm difference between the predicted rotation `T2` and the expected rotation `T1 * T12_measured`. Error is $||T1 \\cdot T_{12} - T2||_F^2$.\n", + "**Note:** The noise models for these factors operate on the vectorized rotation matrix (e.g., 9 elements for `Rot3`). The helper function `ConvertNoiseModel` attempts to convert standard rotation noise models (like those for `BetweenFactor`) into an appropriate isotropic noise model for the Frobenius factor. It expects the input noise model to be isotropic." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3, Pose3, Values\n", + "from gtsam import FrobeniusPriorRot3, FrobeniusFactorRot3, FrobeniusBetweenFactorRot3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "R = symbol_shorthand.R # Using 'R' for Rot3 keys" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fprior_header_md" + }, + "source": [ + "## 1. `FrobeniusPrior`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fprior_desc_md" + }, + "source": [ + "Constrains a `Rot3` variable `R(0)` to be close to a target matrix `M` in the Frobenius norm sense." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "fprior_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "FrobeniusPriorRot3: FrobeniusPriorFactor on R0\n", + "Error model: diagonal sigmas [0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01];\n", + "\n", + "FrobeniusPrior error (vectorized matrix diff): (9,)\n", + "[ 0.00054931 -0.00997917 0. -0.00997917 -0.00054931 0.\n", + " 0. 0. 0. ]\n" + ] + } + ], + "source": [ + "target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)\n", + "key = R(0)\n", + "\n", + "# Create a standard isotropic noise model for rotation (3 dimensional)\n", + "rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)\n", + "\n", + "# Convert it for the Frobenius factor (9 dimensional)\n", + "frobenius_noise_model_prior = gtsam.noiseModel.Isotropic.Sigma(9, 0.01) # Or use ConvertNoiseModel\n", + "\n", + "prior_fro = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model_prior)\n", + "prior_fro.print(\"FrobeniusPriorRot3: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n", + "error_prior = prior_fro.error(values)\n", + "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior.shape}\\n{error_prior}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ffactor_header_md" + }, + "source": [ + "## 1. `FrobeniusFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ffactor_desc_md" + }, + "source": [ + "Constrains two `Rot3` variables `R(0)` and `R(1)` to be close to each other in the Frobenius norm sense." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "ffactor_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "FrobeniusFactorRot3: keys = { r0 r1 }\n", + " noise model: unit (9) \n" + ] + }, + { + "ename": "RuntimeError", + "evalue": "Attempting to at the key \"r0\", which does not exist in the Values.", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[6], line 13\u001b[0m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;66;03m# Evaluate error\u001b[39;00m\n\u001b[0;32m 12\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(key2, Rot3\u001b[38;5;241m.\u001b[39mYaw(\u001b[38;5;241m0.115\u001b[39m)) \u001b[38;5;66;03m# R1 slightly different from R0\u001b[39;00m\n\u001b[1;32m---> 13\u001b[0m error_factor \u001b[38;5;241m=\u001b[39m \u001b[43mfactor_fro\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mFrobeniusFactor error (vectorized matrix diff): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;241m.\u001b[39mshape\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"r0\", which does not exist in the Values." + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "key1 = R(0)\n", + "key2 = R(1)\n", + "# Use same noise model dimension (9)\n", + "frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)\n", + "\n", + "factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)\n", + "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n", + "\n", + "# Evaluate error\n", + "values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n", + "error_factor = factor_fro.error(values)\n", + "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor.shape}\\n{error_factor}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fbetween_header_md" + }, + "source": [ + "## 3. `FrobeniusBetweenFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "fbetween_desc_md" + }, + "source": [ + "Acts like `BetweenFactor` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "fbetween_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor(R0,R1)\n", + " T12: R: [\n", + "\t0.999875, -0.0149991, 0;\n", + "\t0.0149991, 0.999875, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n", + " noise model: diagonal sigmas [0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005];\n", + "\n", + "FrobeniusBetweenFactor error: (9,)\n", + "[-0. 0. 0. 0. -0. 0. 0. 0. 0.]\n" + ] + } + ], + "source": [ + "measured_R12 = Rot3.Yaw(0.005)\n", + "# Use same noise model dimension (9)\n", + "frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)\n", + "\n", + "between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)\n", + "between_fro.print(\"\\nFrobeniusBetweenFactorRot3: \")\n", + "\n", + "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n", + "error_between = between_fro.error(values)\n", + "print(f\"\\nFrobeniusBetweenFactor error: {error_between.shape}\\n{error_between}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.9" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/GeneralSFMFactor.ipynb b/gtsam/slam/doc/GeneralSFMFactor.ipynb new file mode 100644 index 000000000..366254d1a --- /dev/null +++ b/gtsam/slam/doc/GeneralSFMFactor.ipynb @@ -0,0 +1,223 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# GeneralSFMFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors for Structure from Motion (SfM) problems where camera calibration might be unknown or optimized alongside pose and structure.\n", + "\n", + "`GeneralSFMFactor`:\n", + "- A binary factor connecting a `CAMERA` variable and a `LANDMARK` variable.\n", + "- Represents the reprojection error of the `LANDMARK` into the `CAMERA` view, compared to a 2D `measured_` pixel coordinate.\n", + "- The `CAMERA` type encapsulates both pose and calibration (e.g., `PinholeCamera`).\n", + "- Error: `camera.project(landmark) - measured`\n", + "\n", + "`GeneralSFMFactor2`:\n", + "- A ternary factor connecting a `Pose3` variable, a `Point3` landmark variable, and a `CALIBRATION` variable.\n", + "- This explicitly separates the camera pose and calibration into different variables.\n", + "- Error: `PinholeCamera(pose, calibration).project(landmark) - measured`\n", + "\n", + "These factors are core components for visual SLAM or SfM systems where calibration is refined or initially unknown." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (GeneralSFMFactorCal3_S2, GeneralSFMFactor2Cal3_S2,\n", + " PinholeCameraCal3_S2, Pose3, Point3, Point2, Cal3_S2, Values)\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L\n", + "K = symbol_shorthand.K\n", + "C = symbol_shorthand.C # For Camera variable" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `GeneralSFMFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "Connects a combined Camera variable (pose+calibration) and a Landmark.\n", + "Requires the `Values` object to contain instances of the specific `CAMERA` type (e.g., `PinholeCameraCal3_S2`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "GeneralSFMFactor: keys = { c0 l0 }\n", + " noise model: unit (2) \n", + "GeneralSFMFactor: .z[\n", + "\t320;\n", + "\t240\n", + "]\n", + "\n", + "Error for GeneralSFMFactor: 0.0\n" + ] + } + ], + "source": [ + "measured_pt = Point2(320, 240) # Measurement in pixels\n", + "sfm_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "camera_key = C(0)\n", + "landmark_key = L(0)\n", + "\n", + "# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactorCal3_S2\n", + "factor1 = GeneralSFMFactorCal3_S2(measured_pt, sfm_noise, camera_key, landmark_key)\n", + "factor1.print(\"GeneralSFMFactor: \")\n", + "\n", + "# Evaluate error - requires a Camera object in Values\n", + "values = Values()\n", + "camera_pose = Pose3() # Identity pose\n", + "calibration = Cal3_S2(500, 500, 0, 320, 240) # fx, fy, s, u0, v0\n", + "camera = PinholeCameraCal3_S2(camera_pose, calibration)\n", + "landmark = Point3(0, 0, 5) # Point 5m in front of camera\n", + "\n", + "values.insert(camera_key, camera)\n", + "values.insert(landmark_key, landmark)\n", + "\n", + "error1 = factor1.error(values)\n", + "print(f\"\\nError for GeneralSFMFactor: {error1}\") # Should be [0, 0] if landmark projects to measured_pt" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `GeneralSFMFactor2`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "Connects separate Pose3, Point3 (Landmark), and Calibration variables." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "GeneralSFMFactor2: keys = { x0 l0 k0 }\n", + " noise model: unit (2) \n", + "GeneralSFMFactor2: .z[\n", + "\t320;\n", + "\t240\n", + "]\n", + "\n", + "Error for GeneralSFMFactor2: 0.0\n" + ] + } + ], + "source": [ + "pose_key = X(0)\n", + "calib_key = K(0)\n", + "# landmark_key = L(0) # Re-use from above\n", + "\n", + "# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactor2Cal3_S2\n", + "factor2 = GeneralSFMFactor2Cal3_S2(measured_pt, sfm_noise, pose_key, landmark_key, calib_key)\n", + "factor2.print(\"GeneralSFMFactor2: \")\n", + "\n", + "# Evaluate error - requires Pose3, Point3, Cal3_S2 objects in Values\n", + "values2 = Values()\n", + "values2.insert(pose_key, camera_pose)\n", + "values2.insert(landmark_key, landmark)\n", + "values2.insert(calib_key, calibration)\n", + "\n", + "error2 = factor2.error(values2)\n", + "print(f\"\\nError for GeneralSFMFactor2: {error2}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/InitializePose3.ipynb b/gtsam/slam/doc/InitializePose3.ipynb new file mode 100644 index 000000000..2e65b906b --- /dev/null +++ b/gtsam/slam/doc/InitializePose3.ipynb @@ -0,0 +1,256 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# InitializePose3" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `InitializePose3` structure provides static methods for computing an initial estimate for 3D poses (`Pose3`) in a factor graph, particularly useful for Structure from Motion (SfM) or SLAM problems.\n", + "The core idea is to first estimate the orientations (`Rot3`) independently and then use these estimates to initialize a linear system for the translations.\n", + "\n", + "Key static methods:\n", + "* `buildPose3graph(graph)`: Extracts the subgraph containing only `Pose3` `BetweenFactor` and `PriorFactor` constraints from a larger `NonlinearFactorGraph`.\n", + "* `computeOrientationsChordal(pose3Graph)`: Estimates rotations using chordal relaxation on the rotation constraints.\n", + "* `computeOrientationsGradient(pose3Graph, initialGuess)`: Estimates rotations using gradient descent on the manifold.\n", + "* `initializeOrientations(graph)`: Convenience function combining `buildPose3graph` and `computeOrientationsChordal`.\n", + "* `computePoses(initialRot, poseGraph)`: Computes translations given estimated rotations by solving a linear system (performing one Gauss-Newton iteration on poses).\n", + "* `initialize(graph)`: Performs the full initialization pipeline (extract graph, estimate rotations via chordal, compute translations).\n", + "* `initialize(graph, givenGuess, useGradient)`: Full pipeline allowing specification of an initial guess and choosing between chordal or gradient descent for rotations." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import NonlinearFactorGraph, Values, Pose3, Rot3, Point3, PriorFactorPose3, BetweenFactorPose3\n", + "from gtsam import InitializePose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_header_md" + }, + "source": [ + "## Example Initialization Pipeline" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_desc_md" + }, + "source": [ + "We'll create a simple 3D pose graph and use the `InitializePose3.initialize` method to get an initial estimate." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "example_pipeline_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph:\n", + "size: 4\n", + "\n", + "Factor 0: PriorFactor on x0\n", + " prior mean: R: [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + " noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.05; 0.05; 0.05];\n", + "\n", + "Factor 1: BetweenFactor(x0,x1)\n", + " measured: R: [\n", + "\t0.877582562, -0.479425539, 0;\n", + "\t0.479425539, 0.877582562, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 1 0.1 0\n", + " noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n", + "\n", + "Factor 2: BetweenFactor(x1,x2)\n", + " measured: R: [\n", + "\t0.921060994, -0.389418342, 0;\n", + "\t0.389418342, 0.921060994, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.9 -0.1 0\n", + " noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n", + "\n", + "Factor 3: BetweenFactor(x2,x0)\n", + " measured: R: [\n", + "\t0.621609968, 0.78332691, 0;\n", + "\t-0.78332691, 0.621609968, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: -1.8 0.05 0\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.1; 0.4; 0.4; 0.4];\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "\n", + "Initial Estimate (using InitializePose3.initialize):\n", + "\n", + "Values with 3 values:\n", + "Value x0: (class gtsam::Pose3)\n", + "R: [\n", + "\t0.995004165, -0.0998334166, 0;\n", + "\t0.0998334166, 0.995004165, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 -7.63278329e-15 0\n", + "\n", + "Value x1: (class gtsam::Pose3)\n", + "R: [\n", + "\t0.825335615, -0.564642473, 0;\n", + "\t0.564642473, 0.825335615, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.956742586 0.343109526 0\n", + "\n", + "Value x2: (class gtsam::Pose3)\n", + "R: [\n", + "\t0.540302306, -0.841470985, 0;\n", + "\t0.841470985, 0.540302306, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 1.62773065 0.912529884 0\n", + "\n" + ] + } + ], + "source": [ + "# 1. Create a NonlinearFactorGraph with Pose3 factors\n", + "graph = NonlinearFactorGraph()\n", + "\n", + "# Add a prior on the first pose\n", + "prior_mean = Pose3(Rot3.Yaw(0.1), Point3(0.1, 0, 0))\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.05]*3))\n", + "graph.add(PriorFactorPose3(X(0), prior_mean, prior_noise))\n", + "\n", + "# Add odometry factors\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05]*3 + [0.2]*3))\n", + "odometry1 = Pose3(Rot3.Yaw(0.5), Point3(1.0, 0.1, 0))\n", + "odometry2 = Pose3(Rot3.Yaw(0.4), Point3(0.9, -0.1, 0))\n", + "graph.add(BetweenFactorPose3(X(0), X(1), odometry1, odometry_noise))\n", + "graph.add(BetweenFactorPose3(X(1), X(2), odometry2, odometry_noise))\n", + "\n", + "# Add a loop closure factor (less certain)\n", + "loop_mean = Pose3(Rot3.Yaw(-0.9), Point3(-1.8, 0.05, 0))\n", + "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1]*3 + [0.4]*3))\n", + "graph.add(BetweenFactorPose3(X(2), X(0), loop_mean, loop_noise))\n", + "\n", + "graph.print(\"Original Factor Graph:\\n\")\n", + "\n", + "# 2. Perform initialization using the default chordal relaxation method\n", + "initial_estimate = InitializePose3.initialize(graph)\n", + "\n", + "print(\"\\nInitial Estimate (using InitializePose3.initialize):\\n\")\n", + "initial_estimate.print()\n", + "\n", + "# 3. (Optional) Perform initialization step-by-step\n", + "# pose3graph = InitializePose3.buildPose3graph(graph)\n", + "# initial_orientations = InitializePose3.initializeOrientations(graph)\n", + "# initial_estimate_manual = InitializePose3.computePoses(initial_orientations, pose3graph)\n", + "# print(\"\\nInitial Estimate (Manual Steps):\\n\")\n", + "# initial_estimate_manual.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_header_md" + }, + "source": [ + "## Notes" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_desc_md" + }, + "source": [ + "- The quality of the initial estimate depends heavily on the quality and connectivity of the pose graph factors.\n", + "- Chordal relaxation (`computeOrientationsChordal`) is generally faster and often sufficient.\n", + "- Gradient descent (`computeOrientationsGradient`) might provide slightly more accurate orientations but is slower and requires a good initial guess.\n", + "- The final `computePoses` step performs only a *single* Gauss-Newton iteration, assuming the rotations are fixed. The resulting estimate is meant as an initialization for a full nonlinear optimization (e.g., using `GaussNewtonOptimizer` or `LevenbergMarquardtOptimizer`)." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb new file mode 100644 index 000000000..a1ca7a51e --- /dev/null +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -0,0 +1,245 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Karcher Mean Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", + "The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n", + "$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n", + "\n", + "Functions/Classes:\n", + "* `FindKarcherMean(rotations)`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", + "* `KarcherMeanFactor`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3, FindKarcherMean, KarcherMeanFactorRot3, NonlinearFactorGraph, Values\n", + "from gtsam import symbol_shorthand\n", + "\n", + "R = symbol_shorthand.R" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "find_header_md" + }, + "source": [ + "## 1. `FindKarcherMean`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "find_desc_md" + }, + "source": [ + "Computes the Karcher mean of a list of rotations." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "find_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Input Rotations (Yaw angles):\n", + " 0.100\n", + " 0.150\n", + " 0.050\n", + " 0.120\n", + "\n", + "Computed Karcher Mean (Yaw angle): 0.105\n" + ] + } + ], + "source": [ + "# Create a list of Rot3 objects\n", + "rotations = gtsam.Rot3Vector()\n", + "rotations.append(Rot3.Yaw(0.1))\n", + "rotations.append(Rot3.Yaw(0.15))\n", + "rotations.append(Rot3.Yaw(0.05))\n", + "rotations.append(Rot3.Yaw(0.12))\n", + "\n", + "# Compute the Karcher mean\n", + "karcher_mean = FindKarcherMean(rotations)\n", + "\n", + "print(\"Input Rotations (Yaw angles):\")\n", + "for r in rotations: print(f\" {r.yaw():.3f}\")\n", + "\n", + "print(f\"\\nComputed Karcher Mean (Yaw angle): {karcher_mean.yaw():.3f}\")\n", + "# Note: For yaw rotations, the Karcher mean yaw is close to the arithmetic mean (0.105)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_header_md" + }, + "source": [ + "## 2. `KarcherMeanFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_desc_md" + }, + "source": [ + "Creates a factor that constrains the rotational average of a set of `Rot3` variables.\n", + "It acts as a soft gauge constraint. When linearized, it yields a Jacobian factor where each block corresponding to a variable is $\\sqrt{\\beta} I_{3x3}$, and the error vector is zero. The `beta` parameter (optional, defaults to 1) controls the strength (precision) of the constraint." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "factor_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n", + "\n", + "Linearized Factor (JacobianFactor):\n", + " A[r0] = [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " A[r1] = [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " A[r2] = [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " b = [ 0 0 0 ]\n", + " No noise model\n" + ] + }, + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.JacobianFactor' object has no attribute 'find'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[9], line 20\u001b[0m\n\u001b[0;32m 18\u001b[0m sqrt_beta \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39msqrt(beta)\n\u001b[0;32m 19\u001b[0m expected_jacobian \u001b[38;5;241m=\u001b[39m sqrt_beta \u001b[38;5;241m*\u001b[39m np\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m3\u001b[39m)\n\u001b[1;32m---> 20\u001b[0m A0 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(\u001b[43mlinearized_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mfind\u001b[49m(R(\u001b[38;5;241m0\u001b[39m)))\n\u001b[0;32m 21\u001b[0m A1 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m1\u001b[39m)))\n\u001b[0;32m 22\u001b[0m A2 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m2\u001b[39m)))\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.JacobianFactor' object has no attribute 'find'" + ] + } + ], + "source": [ + "keys = [R(0), R(1), R(2)]\n", + "beta = 100.0 # Strength of the constraint\n", + "\n", + "k_factor = KarcherMeanFactorRot3(keys)\n", + "k_factor.print(\"KarcherMeanFactorRot3: \")\n", + "\n", + "# Linearization example\n", + "values = Values()\n", + "values.insert(R(0), Rot3.Yaw(0.1))\n", + "values.insert(R(1), Rot3.Yaw(0.2))\n", + "values.insert(R(2), Rot3.Yaw(0.3))\n", + "\n", + "linearized_factor = k_factor.linearize(values)\n", + "print(\"\\nLinearized Factor (JacobianFactor):\")\n", + "linearized_factor.print()\n", + "\n", + "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n", + "sqrt_beta = np.sqrt(beta)\n", + "expected_jacobian = sqrt_beta * np.eye(3)\n", + "A0 = linearized_factor.getA(linearized_factor.find(R(0)))\n", + "A1 = linearized_factor.getA(linearized_factor.find(R(1)))\n", + "A2 = linearized_factor.getA(linearized_factor.find(R(2)))\n", + "b = linearized_factor.getb()\n", + "\n", + "print(f\"\\nJacobian for R(0):\\n{A0}\")\n", + "print(f\"Jacobian for R(1):\\n{A1}\")\n", + "print(f\"Jacobian for R(2):\\n{A2}\")\n", + "print(f\"Error vector b:\\n{b}\")\n", + "print(f\"sqrt(beta): {sqrt_beta}\")\n", + "assert np.allclose(A0, expected_jacobian)\n", + "assert np.allclose(A1, expected_jacobian)\n", + "assert np.allclose(A2, expected_jacobian)\n", + "assert np.allclose(b, np.zeros(3))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb new file mode 100644 index 000000000..e140dbd71 --- /dev/null +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -0,0 +1,230 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# OrientedPlane3 Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors for working with planar landmarks represented by `gtsam.OrientedPlane3`.\n", + "`OrientedPlane3` represents a plane using normalized coefficients $(n_x, n_y, n_z, d)$, where $(n_x, n_y, n_z)$ is the unit normal vector and $d$ is the distance from the origin along the normal.\n", + "\n", + "Factors defined:\n", + "* `OrientedPlane3Factor`: A binary factor connecting a `Pose3` and an `OrientedPlane3`. It measures the difference between the plane parameters as observed from the given pose and a measured plane.\n", + "* `OrientedPlane3DirectionPrior`: A unary factor on an `OrientedPlane3`. It penalizes the difference between the plane's normal direction and a measured direction (represented by the normal of a measured `OrientedPlane3`). **Note:** The factor error is 3D, but only constrains 2 degrees of freedom (direction). Consider using a more specific direction factor if only direction is measured." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, OrientedPlane3, Point3, Rot3, Values\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m OrientedPlane3Factor, OrientedPlane3DirectionPrior\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n", + "from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "P = symbol_shorthand.P # For Plane" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_header_md" + }, + "source": [ + "## 1. `OrientedPlane3Factor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor_desc_md" + }, + "source": [ + "Connects a `Pose3` (camera/robot pose) and an `OrientedPlane3` (landmark). The measurement is the plane as observed *from the sensor frame*.\n", + "The error is calculated by transforming the global plane landmark into the sensor frame defined by the pose, and then computing the difference (`localCoordinates`) with the measured plane." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "factor_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "OrientedPlane3Factor: OrientedPlane3Factor Factor with keys: x0 P0\n", + "\n", + "Error model: diagonal sigmas [0.05; 0.05; 0.05];\n", + "\n", + "Error at ground truth: [-0. -0. -0.]\n", + "\n", + "Error with noisy plane: [ -0.0150837 -0.00503354 -49.83283361]\n" + ] + } + ], + "source": [ + "# Ground truth plane (e.g., z=1 in world frame)\n", + "gt_plane_world = OrientedPlane3(0, 0, 1, 1)\n", + "\n", + "# Ground truth pose\n", + "gt_pose = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0))\n", + "\n", + "# Measurement: transform the world plane into the camera frame\n", + "# measured_plane = gt_plane_world.transform(gt_pose)\n", + "# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n", + "measured_plane_coeffs = gt_plane_world.coeffs()\n", + "plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n", + "\n", + "pose_key = X(0)\n", + "plane_key = P(0)\n", + "\n", + "plane_factor = OrientedPlane3Factor(measured_plane_coeffs, plane_noise, pose_key, plane_key)\n", + "plane_factor.print(\"OrientedPlane3Factor: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose)\n", + "values.insert(plane_key, gt_plane_world)\n", + "error1 = plane_factor.error(values)\n", + "print(f\"\\nError at ground truth: {error1}\")\n", + "\n", + "# Evaluate with slightly different plane estimate\n", + "noisy_plane = OrientedPlane3(0.01, 0.01, 0.99, 1.05)\n", + "values.update(plane_key, noisy_plane)\n", + "error2 = plane_factor.error(values)\n", + "print(f\"\\nError with noisy plane: {error2}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "prior_header_md" + }, + "source": [ + "## 2. `OrientedPlane3DirectionPrior`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "prior_desc_md" + }, + "source": [ + "A unary factor that puts a prior on the *direction* (normal vector) of an `OrientedPlane3`. The distance component (`d`) of the measured plane is ignored.\n", + "The error is the 3D difference between the estimated plane's normal and the measured plane's normal." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "prior_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "OrientedPlane3DirectionPrior: Factor OrientedPlane3DirectionPrior on P0\n", + "Noise model: diagonal sigmas [0.02; 0.02; 0.02];\n", + "\n", + "Error for prior on noisy_plane: [-0.5 -0.5 0.5]\n", + "Error for prior on gt_plane_world: [0. 0. 0.]\n" + ] + } + ], + "source": [ + "# Measured direction prior (e.g., plane normal is close to world Z axis)\n", + "measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n", + "direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\n", + "\n", + "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.coeffs(), direction_noise)\n", + "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n", + "\n", + "# Evaluate error using the 'noisy_plane' from the previous step\n", + "error_prior = prior_factor.error(values) # values still contains plane_key -> noisy_plane\n", + "print(f\"\\nError for prior on noisy_plane: {error_prior}\")\n", + "\n", + "# Evaluate error for ground truth plane\n", + "values.update(plane_key, gt_plane_world)\n", + "error_prior_gt = prior_factor.error(values)\n", + "print(f\"Error for prior on gt_plane_world: {error_prior_gt}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb new file mode 100644 index 000000000..ce7c37a28 --- /dev/null +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -0,0 +1,293 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# PlanarProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `PlanarProjectionFactor` variants provide camera projection constraints specifically designed for scenarios where the robot or camera moves primarily on a 2D plane (e.g., ground robots with cameras).\n", + "They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n", + "* The robot's 2D pose (`Pose2` `wTb`: world-to-body) in the ground plane.\n", + "* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n", + "* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n", + "* The 3D landmark position in the world frame.\n", + "\n", + "The core projection logic involves converting the `Pose2` `wTb` to a `Pose3` assuming z=0 and yaw=theta, composing it with `bTc` to get the world-to-camera pose `wTc`, and then using a standard `PinholeCamera` model to project the landmark.\n", + "\n", + "Variants:\n", + "* `PlanarProjectionFactor1`: Unknown is robot `Pose2` (`wTb`). Landmark, `bTc`, and calibration are fixed. Useful for localization.\n", + "* `PlanarProjectionFactor2`: Unknowns are robot `Pose2` (`wTb`) and `Point3` landmark. `bTc` and calibration are fixed. Useful for planar SLAM.\n", + "* `PlanarProjectionFactor3`: Unknowns are robot `Pose2` (`wTb`), camera offset `Pose3` (`bTc`), and `Cal3DS2` calibration. Landmark is fixed. Useful for calibration." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", + " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L\n", + "C = symbol_shorthand.C # Calibration\n", + "O = symbol_shorthand.O # Offset" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `PlanarProjectionFactor1` (Localization)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "Used when the landmark, camera offset (`bTc`), and calibration (`calib`) are known, and we want to estimate the robot's `Pose2` (`wTb`)." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Ground Truth Pose2: (1, 0, 0.785398)\n", + "\n", + "Calculated Measurement: [ 909.25565099 1841.1002863 ]\n", + "Factor 1: keys = { x0 }\n", + "isotropic dim=2 sigma=1.5\n", + "\n", + "Error at ground truth: 0.0\n", + "Error at noisy pose: 3317.647263749095\n" + ] + } + ], + "source": [ + "# Known parameters\n", + "landmark_pt = Point3(2.0, 0.5, 0.5)\n", + "body_T_cam = Pose3(Rot3.Yaw(-np.pi/2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", + "calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n", + "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", + "\n", + "# Assume ground truth pose and calculate expected measurement\n", + "gt_pose2 = Pose2(1.0, 0.0, np.pi/4)\n", + "gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n", + "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", + "measured_pt2 = gt_camera.project(landmark_pt)\n", + "print(f\"Ground Truth Pose2: {gt_pose2}\")\n", + "print(f\"Calculated Measurement: {measured_pt2}\")\n", + "\n", + "# Create the factor\n", + "pose_key = X(0)\n", + "factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor1.print(\"Factor 1: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose2) # Error should be zero here\n", + "error1_gt = factor1.error(values)\n", + "print(f\"\\nError at ground truth: {error1_gt}\")\n", + "\n", + "noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", + "values.update(pose_key, noisy_pose2)\n", + "error1_noisy = factor1.error(values)\n", + "print(f\"Error at noisy pose: {error1_noisy}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `PlanarProjectionFactor2` (Planar SLAM)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "Used when the camera offset (`bTc`) and calibration (`calib`) are known, but both the robot `Pose2` (`wTb`) and the `Point3` landmark position are unknown." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor 2: keys = { l0 x0 }\n", + "isotropic dim=2 sigma=1.5\n" + ] + }, + { + "ename": "RuntimeError", + "evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 10\u001b[0m\n\u001b[0;32m 8\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose2)\n\u001b[0;32m 9\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(landmark_key, landmark_pt) \u001b[38;5;66;03m# Error should be zero\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m error2_gt \u001b[38;5;241m=\u001b[39m \u001b[43mfactor2\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror2_gt\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 13\u001b[0m noisy_landmark \u001b[38;5;241m=\u001b[39m Point3(\u001b[38;5;241m2.1\u001b[39m, \u001b[38;5;241m0.45\u001b[39m, \u001b[38;5;241m0.55\u001b[39m)\n", + "\u001b[1;31mRuntimeError\u001b[0m: Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix" + ] + } + ], + "source": [ + "landmark_key = L(0)\n", + "\n", + "factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor2.print(\"Factor 2: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose2)\n", + "values.insert(landmark_key, landmark_pt) # Error should be zero\n", + "error2_gt = factor2.error(values)\n", + "print(f\"\\nError at ground truth: {error2_gt}\")\n", + "\n", + "noisy_landmark = Point3(2.1, 0.45, 0.55)\n", + "values.update(landmark_key, noisy_landmark)\n", + "error2_noisy = factor2.error(values)\n", + "print(f\"Error with noisy landmark: {error2_noisy}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_header_md" + }, + "source": [ + "## 3. `PlanarProjectionFactor3` (Calibration)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor3_desc_md" + }, + "source": [ + "Used when the landmark position is known, but the robot `Pose2` (`wTb`), the camera offset `Pose3` (`bTc`), and the `Cal3DS2` calibration are unknown." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "factor3_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", + "Noise model: diagonal sigmas [1.5; 1.5];\n", + "\n", + "Error at ground truth: [-0. -0.]\n", + "Error with noisy calibration: [8.38867847 0.63659684]\n" + ] + } + ], + "source": [ + "offset_key = O(0)\n", + "calib_key = C(0)\n", + "\n", + "factor3 = PlanarProjectionFactor3(pose_key, offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", + "factor3.print(\"Factor 3: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "values.insert(pose_key, gt_pose2)\n", + "values.insert(offset_key, body_T_cam)\n", + "values.insert(calib_key, calib) # Error should be zero\n", + "error3_gt = factor3.error(values)\n", + "print(f\"\\nError at ground truth: {error3_gt}\")\n", + "\n", + "noisy_calib = Cal3DS2(fx=510, fy=495, s=0, u0=322, v0=241, k1=0, k2=0, p1=0, p2=0)\n", + "values.update(calib_key, noisy_calib)\n", + "error3_noisy = factor3.error(values)\n", + "print(f\"Error with noisy calibration: {error3_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/PoseRotationPrior.ipynb b/gtsam/slam/doc/PoseRotationPrior.ipynb new file mode 100644 index 000000000..cb7d09a75 --- /dev/null +++ b/gtsam/slam/doc/PoseRotationPrior.ipynb @@ -0,0 +1,215 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# PoseRotationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`PoseRotationPrior` is a unary factor that applies a prior constraint only to the **rotation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n", + "It ignores the translation component of the pose variable during error calculation.\n", + "The error is calculated as the difference between the rotation component of the pose variable and the measured prior rotation, expressed in the tangent space of the rotation group.\n", + "\n", + "Error: $ \\text{Log}(\\text{measured}^{-1} \\cdot \\text{pose.rotation}()) $\n", + "\n", + "This is useful when you have information about the absolute orientation of a pose but little or no information about its translation." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[2], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseRotationPriorPose\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPriorPose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a PoseRotationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Provide the key of the pose variable, the measured prior rotation (`Rot3` for `Pose3`, `Rot2` for `Pose2`), and a noise model defined on the rotation manifold's dimension (e.g., 3 for `Rot3`)." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "PoseRotationPrior: PoseRotationPriorFactor on x0\n", + "Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n", + "Measured Rotation R: [\n", + "\t0.707107, -0.707107, 0;\n", + "\t0.707107, 0.707107, 0;\n", + "\t0, 0, 1\n", + "]\n", + "\n" + ] + } + ], + "source": [ + "pose_key = X(0)\n", + "measured_rotation = Rot3.Yaw(np.pi / 4) # Prior belief about orientation\n", + "\n", + "# Noise model on rotation (3 dimensions for Rot3)\n", + "rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n", + "\n", + "# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n", + "factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n", + "factor.print(\"PoseRotationPrior: \")\n", + "\n", + "# Alternative constructor: extract rotation from a full Pose3 prior\n", + "full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n", + "factor_from_pose = PoseRotationPriorPose3(pose_key, full_pose_prior, rotation_noise)\n", + "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error depends only on the rotation part of the `Pose3` value in the `Values` object." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error with correct rotation: [0. 0. 0.] (Should be near zero)\n", + "Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n", + "Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Pose with correct rotation but different translation\n", + "pose_val1 = Pose3(measured_rotation, Point3(1, 2, 3))\n", + "values.insert(pose_key, pose_val1)\n", + "error1 = factor.error(values)\n", + "print(f\"Error with correct rotation: {error1} (Should be near zero)\")\n", + "\n", + "# Pose with incorrect rotation\n", + "pose_val2 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(1, 2, 3))\n", + "values.update(pose_key, pose_val2)\n", + "error2 = factor.error(values)\n", + "print(f\"Error with incorrect rotation: {error2} (Should be non-zero)\")\n", + "\n", + "# Check that translation change doesn't affect error\n", + "pose_val3 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(100, 200, 300))\n", + "values.update(pose_key, pose_val3)\n", + "error3 = factor.error(values)\n", + "print(f\"Error with different translation: {error3} (Should be same as error2)\")\n", + "assert np.allclose(error2, error3)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/PoseTranslationPrior.ipynb b/gtsam/slam/doc/PoseTranslationPrior.ipynb new file mode 100644 index 000000000..047bf5e59 --- /dev/null +++ b/gtsam/slam/doc/PoseTranslationPrior.ipynb @@ -0,0 +1,214 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# PoseTranslationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`PoseTranslationPrior` is a unary factor that applies a prior constraint only to the **translation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n", + "It ignores the rotation component of the pose variable during error calculation.\n", + "The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for `Point2` or `Point3`).\n", + "\n", + "Error: $ \\text{pose.translation}() - \\text{measured} $ (potentially rotated into world frame if translation is in body frame - check `evaluateError` implementation details in C++). *Correction based on C++ code*: The error is `traits::Local(measured_, pose.translation())`, calculated in the world frame, and the Jacobian involves the pose's rotation.\n", + "\n", + "This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a PoseTranslationPrior" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Provide the key of the pose variable, the measured prior translation (`Point3` for `Pose3`, `Point2` for `Pose2`), and a noise model defined on the translation space dimension (e.g., 3 for `Point3`)." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "PoseTranslationPrior: Factor PoseTranslationPrior on x0\n", + "Noise model: diagonal sigmas [0.5; 0.5; 0.5];\n", + "Measured Translation10 20 5\n" + ] + } + ], + "source": [ + "pose_key = X(0)\n", + "measured_translation = Point3(10.0, 20.0, 5.0) # Prior belief about position\n", + "\n", + "# Noise model on translation (3 dimensions for Point3)\n", + "translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n", + "\n", + "# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n", + "factor = PoseTranslationPriorPose3(pose_key, measured_translation, translation_noise)\n", + "factor.print(\"PoseTranslationPrior: \")\n", + "\n", + "# Alternative constructor: extract translation from a full Pose3 prior\n", + "full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored\n", + "factor_from_pose = PoseTranslationPriorPose3(pose_key, full_pose_prior, translation_noise)\n", + "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error depends only on the translation part of the `Pose3` value in the `Values` object." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error with correct translation: [0. 0. 0.] (Should be near zero)\n", + "Error with incorrect translation: [ 0.39999999 -0.19999999 0.19999999] (Should be non-zero)\n", + "Error with different rotation: [ 0.39999999 -0.19999999 0.19999999] (Should reflect Jacobian change)\n", + "Unwhitened error with different rotation: [ 0.2 -0.1 0.1] (Should be [0.2, -0.1, 0.1])\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Pose with correct translation but different rotation\n", + "pose_val1 = Pose3(Rot3.Roll(0.5), measured_translation)\n", + "values.insert(pose_key, pose_val1)\n", + "error1 = factor.error(values)\n", + "print(f\"Error with correct translation: {error1} (Should be near zero)\")\n", + "\n", + "# Pose with incorrect translation\n", + "pose_val2 = Pose3(Rot3.Roll(0.5), Point3(10.2, 19.9, 5.1))\n", + "values.update(pose_key, pose_val2)\n", + "error2 = factor.error(values)\n", + "print(f\"Error with incorrect translation: {error2} (Should be non-zero)\")\n", + "\n", + "# Check that rotation change doesn't affect unwhitened error\n", + "pose_val3 = Pose3(Rot3.Yaw(1.0), Point3(10.2, 19.9, 5.1))\n", + "values.update(pose_key, pose_val3)\n", + "error3 = factor.error(values)\n", + "unwhitened2 = factor.unwhitenedError(values)\n", + "print(f\"Error with different rotation: {error3} (Should reflect Jacobian change)\")\n", + "print(f\"Unwhitened error with different rotation: {unwhitened2} (Should be [0.2, -0.1, 0.1])\")\n", + "# assert np.allclose(error2, error3) # Whitened error WILL change due to Jacobian\n", + "assert np.allclose(unwhitened2, np.array([0.2, -0.1, 0.1]))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/ProjectionFactor.ipynb b/gtsam/slam/doc/ProjectionFactor.ipynb new file mode 100644 index 000000000..50bc5d71d --- /dev/null +++ b/gtsam/slam/doc/ProjectionFactor.ipynb @@ -0,0 +1,238 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# GenericProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`GenericProjectionFactor` is a versatile factor for monocular camera measurements.\n", + "It models the reprojection error between the predicted projection of a 3D `LANDMARK` (usually `Point3`) onto the image plane of a camera defined by `POSE` (usually `Pose3`) and `CALIBRATION` (e.g., `Cal3_S2`, `Cal3Bundler`, `Cal3DS2`), and a measured 2D pixel coordinate `measured_`.\n", + "\n", + "Key features:\n", + "- **Templated:** Works with different pose, landmark, and calibration types.\n", + "- **Fixed Calibration:** Assumes the `CALIBRATION` object (`K_`) is known and fixed (passed as a shared pointer).\n", + "- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform between the pose variable's frame (body) and the camera's sensor frame.\n", + "- **Cheirality Handling:** Can be configured to throw an exception or return a large error if the landmark projects behind the camera.\n", + "\n", + "The error is the 2D vector difference:\n", + "$$ \\text{error}(P, L) = \\text{project}(P \\cdot S, L) - z $$\n", + "where $P$ is the pose variable, $L$ is the landmark variable, $S$ is the optional `body_P_sensor` transform, `project` is the camera projection function including calibration, and $z$ is the `measured_` point." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Point3, Point2, Rot3, Cal3_S2, Values\n", + "# The Python wrapper often creates specific instantiations\n", + "from gtsam import GenericProjectionFactorCal3_S2\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a GenericProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing:\n", + "1. The 2D measurement (`Point2`).\n", + "2. The noise model (typically 2D isotropic).\n", + "3. The key for the pose variable.\n", + "4. The key for the landmark variable.\n", + "5. A `shared_ptr` to the fixed calibration object.\n", + "6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n", + "7. (Optional) Cheirality handling flags." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor with offset: GenericProjectionFactor, z = [\n", + "\t330;\n", + "\t250\n", + "]\n", + " sensor pose in body frame: R: [\n", + "\t6.12323e-17, 6.12323e-17, 1;\n", + "\t-1, 3.7494e-33, 6.12323e-17;\n", + "\t-0, -1, 6.12323e-17\n", + "]\n", + "t: 0.1 0 0.2\n", + " keys = { x0 l1 }\n", + " noise model: unit (2) \n", + "\n", + "Factor without offset: GenericProjectionFactor, z = [\n", + "\t330;\n", + "\t250\n", + "]\n", + " keys = { x0 l1 }\n", + " noise model: unit (2) \n" + ] + } + ], + "source": [ + "measured_pt2 = Point2(330, 250)\n", + "pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # 1 pixel std dev\n", + "pose_key = X(0)\n", + "landmark_key = L(1)\n", + "\n", + "# Shared pointer to calibration\n", + "K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n", + "\n", + "# Optional sensor pose offset\n", + "body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n", + "\n", + "# Create factor with sensor offset\n", + "factor_with_offset = GenericProjectionFactorCal3_S2(\n", + " measured_pt2, pixel_noise, pose_key, landmark_key, K, body_P_sensor)\n", + "factor_with_offset.print(\"Factor with offset: \")\n", + "\n", + "# Create factor without sensor offset (implicitly identity)\n", + "factor_no_offset = GenericProjectionFactorCal3_S2(\n", + " measured_pt2, pixel_noise, pose_key, landmark_key, K)\n", + "factor_no_offset.print(\"\\nFactor without offset: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error is the difference between the predicted projection and the measurement." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error (no offset): 1175689.2145311693\n", + "Error (with offset): 50751.576015003826\n", + "Manual error calculation (no offset): [1370.63962025 687.55033305]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Example values\n", + "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n", + "landmark = Point3(4.0, 2.0, 3.0)\n", + "\n", + "values.insert(pose_key, pose)\n", + "values.insert(landmark_key, landmark)\n", + "\n", + "# Evaluate factor without offset\n", + "error_no_offset = factor_no_offset.error(values)\n", + "print(f\"Error (no offset): {error_no_offset}\")\n", + "\n", + "# Evaluate factor with offset\n", + "error_with_offset = factor_with_offset.error(values)\n", + "print(f\"Error (with offset): {error_with_offset}\")\n", + "\n", + "# Manually verify projection (no offset case)\n", + "cam_no_offset = gtsam.PinholeCameraCal3_S2(pose, K)\n", + "predicted_no_offset = cam_no_offset.project(landmark)\n", + "manual_error = predicted_no_offset - measured_pt2\n", + "print(f\"Manual error calculation (no offset): {manual_error}\")\n", + "assert np.allclose(factor_no_offset.unwhitenedError(values), manual_error)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb new file mode 100644 index 000000000..5ea646d19 --- /dev/null +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -0,0 +1,219 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# ReferenceFrameFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`ReferenceFrameFactor` is a ternary factor used to relate landmark observations made in two different reference frames (e.g., from two different robots or two distinct SLAM sessions).\n", + "It connects:\n", + "1. A landmark (`POINT`) expressed in a 'global' or common reference frame (`globalKey`).\n", + "2. A transform (`TRANSFORM`) representing the pose of a 'local' frame relative to the 'global' frame (`transKey`). Typically `TRANSFORM = global_T_local`.\n", + "3. The *same* landmark (`POINT`) expressed in the 'local' reference frame (`localKey`).\n", + "\n", + "The factor enforces the constraint that the globally expressed landmark, when transformed by the `global_T_local` transform, should match the locally expressed landmark.\n", + "The transformation logic depends on the specific `POINT` and `TRANSFORM` types and is handled by the `transform_point` helper function (which usually calls `Transform::transformFrom`).\n", + "\n", + "Error: $ \\text{Log}(\\text{local}^{-1} \\cdot \\text{transform\\_point}(\\text{trans}, \\text{global})) $\n", + "\n", + "This factor is crucial for multi-robot map merging or combining results from different SLAM sessions where common landmarks have been observed." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 5\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# The Python wrapper creates specific instantiations\u001b[39;00m\n\u001b[1;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m ReferenceFrameFactorPoint3Pose3\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 8\u001b[0m L \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mL \u001b[38;5;66;03m# Global landmark\u001b[39;00m\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n", + "# The Python wrapper creates specific instantiations\n", + "from gtsam import ReferenceFrameFactorPoint3Pose3\n", + "from gtsam import symbol_shorthand\n", + "\n", + "L = symbol_shorthand.L # Global landmark\n", + "T = symbol_shorthand.T # Transform global_T_local\n", + "l = symbol_shorthand.l # Local landmark" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a ReferenceFrameFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing the keys for the global landmark, the transform, the local landmark, and a noise model.\n", + "The noise model dimensionality should match the dimension of the `POINT` type." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ReferenceFrameFactor: : ReferenceFrameFactor(Global: L0, Transform: T0, Local: l0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.1];\n" + ] + } + ], + "source": [ + "global_landmark_key = L(0)\n", + "transform_key = T(0)\n", + "local_landmark_key = l(0)\n", + "\n", + "# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n", + "noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n", + "\n", + "# Factor type includes Point and Transform types\n", + "factor = ReferenceFrameFactorPoint3Pose3(global_landmark_key,\n", + " transform_key,\n", + " local_landmark_key,\n", + " noise)\n", + "factor.print(\"ReferenceFrameFactor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error represents how much the transformed global landmark deviates from the local landmark estimate." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Expected local landmark: [ 2. -4. 1.]\n", + "\n", + "Error at ground truth: [-0. 0. 0.] (Should be zero)\n", + "Error with noisy local landmark: [-1. 1. -0.5]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Example values\n", + "global_landmark = Point3(5.0, 2.0, 1.0)\n", + "global_T_local = Pose3(Rot3.Yaw(np.pi/2), Point3(1, 0, 0))\n", + "expected_local_landmark = global_T_local.transformTo(global_landmark)\n", + "print(f\"Expected local landmark: {expected_local_landmark}\")\n", + "\n", + "values.insert(global_landmark_key, global_landmark)\n", + "values.insert(transform_key, global_T_local)\n", + "values.insert(local_landmark_key, expected_local_landmark)\n", + "\n", + "error_gt = factor.error(values)\n", + "print(f\"\\nError at ground truth: {error_gt} (Should be zero)\")\n", + "\n", + "# Introduce error in the local landmark estimate\n", + "noisy_local_landmark = expected_local_landmark + Point3(0.1, -0.1, 0.05)\n", + "values.update(local_landmark_key, noisy_local_landmark)\n", + "error_noisy = factor.error(values)\n", + "print(f\"Error with noisy local landmark: {error_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/RotateFactor.ipynb b/gtsam/slam/doc/RotateFactor.ipynb new file mode 100644 index 000000000..a62e986ad --- /dev/null +++ b/gtsam/slam/doc/RotateFactor.ipynb @@ -0,0 +1,229 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Rotate Factors" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "This header defines factors that constrain an unknown rotation (`Rot3`) based on how it transforms either full rotations or directions.\n", + "\n", + "* `RotateFactor`: Relates two *incremental* rotations measured in different frames using an unknown rotation relating the frames. If $Z = R \\cdot P \\cdot R^T$, where $P = e^{[p]}$ and $Z = e^{[z]}$ are measured incremental rotations (expressed as angular velocity vectors $p$ and $z$), this factor constrains the unknown rotation $R$ such that $p = R z$. The error is $Rz - p$.\n", + "* `RotateDirectionsFactor`: Relates two *directions* (unit vectors, `Unit3`) measured in different frames using an unknown rotation $R$ relating the frames. If $p_{world} = R \\cdot z_{body}$, this factor constrains $R$. The error is the angular difference between the predicted $R z_{body}$ and the measured $p_{world}$." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 4\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Rot3, Point3, Unit3, Values\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m RotateFactor, RotateDirectionsFactor\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m R \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mR \u001b[38;5;66;03m# For Rotation key\u001b[39;00m\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Rot3, Point3, Unit3, Values\n", + "from gtsam import RotateFactor, RotateDirectionsFactor\n", + "from gtsam import symbol_shorthand\n", + "\n", + "R = symbol_shorthand.R # For Rotation key" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_header_md" + }, + "source": [ + "## 1. `RotateFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor1_desc_md" + }, + "source": [ + "Constraints an unknown `Rot3` based on corresponding incremental rotation measurements $P$ (predicted/world) and $Z$ (measured/body)." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "factor1_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "RotateFactor: Factor RotateFactor on R0\n", + "Noise model: diagonal sigmas [0.001; 0.001; 0.001];\n", + "RotateFactor:]\n", + "p: [0.01 0.02 0.03]\n", + "z: [ 0.03 -0.01 0.02]\n", + "\n", + "Error at ground truth R: [-0. -0. 0.]\n", + "Error at noisy R: [ 0.039987 0.00999887 -0.00998887]\n" + ] + } + ], + "source": [ + "# Assume P and Z are small incremental rotations\n", + "P = Rot3.Expmap(np.array([0.01, 0.02, 0.03]))\n", + "Z = Rot3.Expmap(np.array([0.03, -0.01, 0.02]))\n", + "rot_key = R(0)\n", + "noise1 = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)\n", + "\n", + "factor1 = RotateFactor(rot_key, P, Z, noise1)\n", + "factor1.print(\"RotateFactor: \")\n", + "\n", + "# Evaluate error\n", + "values = Values()\n", + "# Ground truth R would satisfy P = R*Z\n", + "# R = P * Z.inverse()\n", + "gt_R = P * (Z.inverse())\n", + "values.insert(rot_key, gt_R)\n", + "error1_gt = factor1.error(values)\n", + "print(f\"\\nError at ground truth R: {error1_gt}\")\n", + "\n", + "noisy_R = gt_R * Rot3.Expmap(np.array([0.001, -0.001, 0.001]))\n", + "values.update(rot_key, noisy_R)\n", + "error1_noisy = factor1.error(values)\n", + "print(f\"Error at noisy R: {error1_noisy}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_header_md" + }, + "source": [ + "## 2. `RotateDirectionsFactor`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "factor2_desc_md" + }, + "source": [ + "Constraints an unknown `Rot3` based on corresponding direction measurements $p_{world}$ (predicted/world) and $z_{body}$ (measured/body)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "factor2_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "RotateDirectionsFactor: Factor RotateDirectionsFactor on R0\n", + "Noise model: diagonal sigmas [0.01; 0.01];\n", + "RotateDirectionsFactor:\n", + "pUnit3: (1, 0, 0)\n", + "zUnit3: (0, 1, 0)\n", + "\n", + "Check: gt_R * z_body = [ 1.0000000e+00 -6.1232340e-17 0.0000000e+00]\n", + "Error at ground truth R: [-0. 0.]\n", + "Error at noisy R: [-0.99995 -0.99995]\n" + ] + } + ], + "source": [ + "p_world = Unit3(Point3(1, 0, 0)) # Direction in world frame\n", + "z_body = Unit3(Point3(0, 1, 0)) # Corresponding direction in body frame\n", + "noise2 = gtsam.noiseModel.Isotropic.Sigma(2, 0.01) # Noise on 2D tangent space\n", + "\n", + "factor2 = RotateDirectionsFactor(rot_key, p_world, z_body, noise2)\n", + "factor2.print(\"RotateDirectionsFactor: \")\n", + "\n", + "# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)\n", + "# This corresponds to a -90 deg yaw\n", + "gt_R_dir = Rot3.Yaw(-np.pi/2)\n", + "print(f\"\\nCheck: gt_R * z_body = {gt_R_dir * z_body.point3()}\")\n", + "\n", + "# Evaluate error\n", + "values_dir = Values()\n", + "values_dir.insert(rot_key, gt_R_dir)\n", + "error2_gt = factor2.error(values_dir)\n", + "print(f\"Error at ground truth R: {error2_gt}\")\n", + "\n", + "noisy_R_dir = gt_R_dir * Rot3.Expmap(np.array([0.01, 0, 0.01]))\n", + "values_dir.update(rot_key, noisy_R_dir)\n", + "error2_noisy = factor2.error(values_dir)\n", + "print(f\"Error at noisy R: {error2_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartFactorParams.ipynb b/gtsam/slam/doc/SmartFactorParams.ipynb new file mode 100644 index 000000000..b19218538 --- /dev/null +++ b/gtsam/slam/doc/SmartFactorParams.ipynb @@ -0,0 +1,221 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionParams" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionParams` is a structure used to configure the behavior of \"smart\" factors like `SmartProjectionFactor`, `SmartProjectionPoseFactor`, and `SmartStereoFactor`.\n", + "These factors implicitly manage the triangulation of a 3D point observed by multiple cameras.\n", + "\n", + "Parameters include:\n", + "- **`linearizationMode`**: Determines the type of linear factor produced when `.linearize()` is called.\n", + " - `HESSIAN` (Default): Creates a `RegularHessianFactor` by marginalizing out the point.\n", + " - `IMPLICIT_SCHUR`: Creates a `RegularImplicitSchurFactor`.\n", + " - `JACOBIAN_Q`: Creates a `JacobianFactorQ`.\n", + " - `JACOBIAN_SVD`: Creates a `JacobianFactorSVD`.\n", + "- **`degeneracyMode`**: How to handle cases where triangulation fails or is ill-conditioned.\n", + " - `IGNORE_DEGENERACY` (Default): Factor might become ill-conditioned.\n", + " - `ZERO_ON_DEGENERACY`: Return a zero-information factor (Hessian/Jacobian) if triangulation fails.\n", + " - `HANDLE_INFINITY`: Treat the point as being at infinity (uses `Unit3` representation).\n", + "- **`triangulation`**: A `gtsam.TriangulationParameters` struct containing parameters for the `triangulateSafe` function:\n", + " - `rankTolerance`: Rank tolerance threshold for SVD during triangulation.\n", + " - `enableEPI`: Use Essential matrix check for epipolar inconsistency (only for Point3 landmarks).\n", + " - `landmarkDistanceThreshold`: If point distance is greater than this, use point-at-infinity.\n", + " - `dynamicOutlierRejectionThreshold`: If non-zero, dynamically rejects measurements based on reprojection error (threshold in sigmas).\n", + "- **`retriangulationThreshold`**: If the change in camera poses between linearizations exceeds this threshold (Frobenius norm difference), the point is re-triangulated.\n", + "- **`throwCheirality` / `verboseCheirality`**: Flags inherited from projection factors to control exception handling when a point is behind a camera." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "from gtsam import SmartProjectionParams, LinearizationMode, DegeneracyMode" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating and Modifying Params" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Default Parameters:\n" + ] + }, + { + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mDefault Parameters:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# default_params.print() # TODO\u001b[39;00m\n\u001b[0;32m 5\u001b[0m \n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Custom parameters\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m custom_params \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionParams\u001b[49m\u001b[43m(\u001b[49m\n\u001b[0;32m 8\u001b[0m \u001b[43m \u001b[49m\u001b[43mlinearizationMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mLinearizationMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mJACOBIAN_Q\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 9\u001b[0m \u001b[43m \u001b[49m\u001b[43mdegeneracyMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mDegeneracyMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mZERO_ON_DEGENERACY\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 10\u001b[0m \u001b[43m \u001b[49m\u001b[43mthrowCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[43m \u001b[49m\u001b[43mverboseCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 12\u001b[0m \u001b[43m \u001b[49m\u001b[43mretriangulationTh\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1e-3\u001b[39;49m\n\u001b[0;32m 13\u001b[0m \u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;66;03m# Modify triangulation parameters directly\u001b[39;00m\n\u001b[0;32m 15\u001b[0m custom_params\u001b[38;5;241m.\u001b[39mtriangulation\u001b[38;5;241m.\u001b[39mrankTolerance \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m1e-8\u001b[39m\n", + "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001" + ] + } + ], + "source": [ + "# Default parameters\n", + "default_params = SmartProjectionParams()\n", + "print(\"Default Parameters:\")\n", + "default_params.print()\n", + "\n", + "# Custom parameters\n", + "custom_params = SmartProjectionParams(\n", + " linearizationMode = LinearizationMode.JACOBIAN_Q,\n", + " degeneracyMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", + " throwCheirality = False,\n", + " verboseCheirality = True,\n", + " retriangulationTh = 1e-3\n", + ")\n", + "# Modify triangulation parameters directly\n", + "custom_params.triangulation.rankTolerance = 1e-8\n", + "custom_params.triangulation.enableEPI = False\n", + "custom_params.triangulation.dynamicOutlierRejectionThreshold = 3.0 # Reject points with reproj error > 3*sigma\n", + "\n", + "print(\"\\nCustom Parameters:\")\n", + "custom_params.print()\n", + "\n", + "# Accessing parameters\n", + "print(f\"\\nAccessing Custom Params:\")\n", + "print(f\" Linearization Mode: {custom_params.getLinearizationMode()}\")\n", + "print(f\" Degeneracy Mode: {custom_params.getDegeneracyMode()}\")\n", + "print(f\" Rank Tolerance: {custom_params.getTriangulationParameters().rankTolerance}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_header_md" + }, + "source": [ + "## Usage" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_desc_md" + }, + "source": [ + "These `SmartProjectionParams` objects are passed to the constructors of smart factors, like `SmartProjectionPoseFactor`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "usage_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart Factor created with custom params:\n", + "SmartProjectionPoseFactor, z = \n", + " SmartProjectionFactor\n", + "linearizationMode: 2\n", + "triangulationParameters:\n", + "rankTolerance = 1e-08\n", + "enableEPI = false\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = 3\n", + "\n", + "\n", + "result:\n", + "Degenerate\n", + "\n", + "SmartFactorBase, z = \n", + "Factor\n" + ] + } + ], + "source": [ + "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n", + "\n", + "# Example: Using custom params with a smart factor\n", + "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240)\n", + "\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n", + "print(\"Smart Factor created with custom params:\")\n", + "smart_factor.print()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb new file mode 100644 index 000000000..afb0daa1e --- /dev/null +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -0,0 +1,343 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionFactor` is a \"smart\" factor designed for Structure from Motion (SfM) or visual SLAM problems where **both camera poses and calibration are being optimized**.\n", + "It implicitly represents a 3D point landmark that has been observed by multiple cameras.\n", + "\n", + "Key characteristics:\n", + "- **Implicit Point:** The 3D point is not an explicit variable in the factor graph. Instead, the factor internally triangulates the point based on the current camera estimates whenever needed (e.g., during linearization or error calculation).\n", + "- **Marginalization:** When linearized (e.g., to a Hessian factor), it effectively marginalizes out the 3D point, creating a dense factor connecting only the cameras that observed the point.\n", + "- **`CAMERA` Template:** This template parameter must represent a camera model that includes *both* pose and calibration (e.g., `PinholeCameraCal3_S2`, `PinholeCameraBundler`).\n", + "- **`Values` Requirement:** When using this factor, the `Values` object passed to methods like `error` or `linearize` must contain `CAMERA` objects (not separate `Pose3` and `Calib` objects) associated with the factor's keys.\n", + "- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n", + "\n", + "**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n", + "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", + " SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n", + " PinholeCameraCal3_S2, Cal3_S2)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "C = symbol_shorthand.C # Key for Camera object" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating and Adding Measurements" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "1. Create the factor with a noise model and parameters.\n", + "2. Add measurements (2D points) and the corresponding camera keys one by one or in batches." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 3 measurements.\n", + "SmartFactor: SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1e-09\n", + "enableEPI = false\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "\n", + "\n", + "result:\n", + "Degenerate\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "[150; 505]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 1, px = \n", + "[470; 495]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 2, px = \n", + "[480; 150]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "Factor on C0 C1 C2\n" + ] + } + ], + "source": [ + "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "smart_params = SmartProjectionParams() # Use default params (HESSIAN, IGNORE_DEGENERACY)\n", + "\n", + "# Factor type includes the Camera type, e.g., SmartProjectionFactorPinholeCameraCal3_S2\n", + "smart_factor = SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n", + "\n", + "# Add measurements and keys\n", + "smart_factor.add(Point2(150, 505), C(0))\n", + "smart_factor.add(Point2(470, 495), C(1))\n", + "smart_factor.add(Point2(480, 150), C(2))\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n", + "smart_factor.print(\"SmartFactor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method implicitly triangulates the point based on the `CAMERA` objects in `values` and computes the sum of squared reprojection errors." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulated point result:\n", + "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" + ] + } + ], + "source": [ + "# Create Values containing CAMERA objects\n", + "values = Values()\n", + "K = Cal3_S2(500, 500, 0, 320, 240)\n", + "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", + "pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", + "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n", + "\n", + "values.insert(C(0), PinholeCameraCal3_S2(pose0, K))\n", + "values.insert(C(1), PinholeCameraCal3_S2(pose1, K))\n", + "values.insert(C(2), PinholeCameraCal3_S2(pose2, K))\n", + "\n", + "# Triangulate first to see the implicit point\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulated point result:\\n{point_result}\")\n", + "\n", + "if point_result.valid():\n", + " # Calculate error\n", + " total_error = smart_factor.error(values)\n", + " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "else:\n", + " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_header_md" + }, + "source": [ + "## Linearization" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_desc_md" + }, + "source": [ + "The `.linearize(values)` method creates a linear factor (e.g., `RegularHessianFactor`) connecting the camera variables, effectively marginalizing the implicit point." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "linearize_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized Factor (showing HessianFactor structure):\n", + "RegularHessianFactor(9): density=100% keys={ C0 C1 C2 }\n", + "Augmented information matrix: (dimensions: 9, 9, 9) : \n", + "{\n", + "\t[ 4.383e+04, 2.596e+04, -1.466e+04, -6.732e+03, 1.357e+04, -4.327e+03, -2.004e+04, -1.143e+03, -3.459e+03; ]\n", + "\t[ 2.596e+04, 6.879e+04, 5.416e+03, 3.589e+03, -1.075e+04, 2.158e+04, 3.107e+04, -8.167e+03, -1.237e+04; ]\n", + "\t[ -1.466e+04, 5.416e+03, 2.336e+04, -1.026e+03, -3.572e+03, -1.124e+04, -1.497e+04, -5.631e+03, 4.149e+03; ]\n", + "\t[ -6.732e+03, 3.589e+03, -1.026e+03, 7.027e+03, 1.434e+03, 3.742e+03, 2.527e+03, 2.454e+03, -6.619e+03; ]\n", + "\t[ 1.357e+04, -1.075e+04, -3.572e+03, 1.434e+03, 1.511e+04, -8.935e+02, -1.484e+04, 3.811e+03, -1.993e+03; ]\n", + "\t[ -4.327e+03, 2.158e+04, -1.124e+04, 3.742e+03, -8.935e+02, 2.587e+04, 2.085e+04, -1.193e+04, -5.818e+03; ]\n", + "\t[ -2.004e+04, 3.107e+04, -1.497e+04, 2.527e+03, -1.484e+04, 2.085e+04, 3.128e+04, -5.349e+03, -6.557e+03; ]\n", + "\t[ -1.143e+03, -8.167e+03, -5.631e+03, 2.454e+03, 3.811e+03, -1.193e+04, -5.349e+03, 1.537e+04, -1.987e+03; ]\n", + "\t[ -3.459e+03, -1.237e+04, 4.149e+03, -6.619e+03, -1.993e+03, -5.818e+03, -6.557e+03, -1.987e+03, 1.043e+04; ]\n", + "}\n", + "Augmented Diagonal Block [0,0]:\n", + "[ 4.383e+04, 2.596e+04, -1.466e+04; ]\n", + "[ 2.596e+04, 6.879e+04, 5.416e+03; ]\n", + "[ -1.466e+04, 5.416e+03, 2.336e+04; ]\n", + "\n", + "Augmented Diagonal Block [1,1]:\n", + "[ 7.027e+03, 1.434e+03, 3.742e+03; ]\n", + "[ 1.434e+03, 1.511e+04, -8.935e+02; ]\n", + "[ 3.742e+03, -8.935e+02, 2.587e+04; ]\n", + "\n", + "Augmented Diagonal Block [2,2]:\n", + "[ 3.128e+04, -5.349e+03, -6.557e+03; ]\n", + "[ -5.349e+03, 1.537e+04, -1.987e+03; ]\n", + "[ -6.557e+03, -1.987e+03, 1.043e+04; ]\n", + "\n", + "Off-Diagonal Block [0,1]:\n", + "[ -6.732e+03, 1.357e+04, -4.327e+03; ]\n", + "[ 3.589e+03, -1.075e+04, 2.158e+04; ]\n", + "[ -1.026e+03, -3.572e+03, -1.124e+04; ]\n", + "\n", + "Off-Diagonal Block [0,2]:\n", + "[ -2.004e+04, -1.143e+03, -3.459e+03; ]\n", + "[ 3.107e+04, -8.167e+03, -1.237e+04; ]\n", + "[ -1.497e+04, -5.631e+03, 4.149e+03; ]\n", + "\n", + "Off-Diagonal Block [1,2]:\n", + "[ 2.527e+03, 2.454e+03, -6.619e+03; ]\n", + "[ -1.484e+04, 3.811e+03, -1.993e+03; ]\n", + "[ 2.085e+04, -1.193e+04, -5.818e+03; ]\n", + "\n", + "Error vector:\n", + "[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n", + "Constant error term: 103876\n" + ] + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "graph.add(smart_factor)\n", + "\n", + "# Linearize (default mode is HESSIAN)\n", + "linear_factor = smart_factor.linearize(values)\n", + "\n", + "if linear_factor:\n", + " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", + " # Cast to HessianFactor to print its details\n", + " hessian_factor = gtsam.RegularHessianFactorCal3_S2.Downcast(linear_factor)\n", + " if hessian_factor:\n", + " hessian_factor.print()\n", + " else:\n", + " print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n", + "else:\n", + " print(\"Linearization failed (likely due to triangulation degeneracy)\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb new file mode 100644 index 000000000..9f56fc8c9 --- /dev/null +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -0,0 +1,354 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionPoseFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionPoseFactor` is a \"smart\" factor optimized for visual SLAM problems where the **camera calibration is known and fixed**, and only the camera **poses** are being optimized.\n", + "It inherits from `SmartProjectionFactor` but simplifies the setup by taking a single shared `CALIBRATION` object (e.g., `Cal3_S2`, `Cal3Bundler`) assumed to be used by all cameras observing the landmark.\n", + "\n", + "Key characteristics:\n", + "- **Implicit Point:** Like its base class, the 3D point is not an explicit variable.\n", + "- **Fixed Calibration:** Assumes a single, known calibration `K` for all measurements.\n", + "- **Pose Variables:** The factor connects `Pose3` variables directly.\n", + "- **`Values` Requirement:** Requires `Pose3` objects in the `Values` container for its keys.\n", + "- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n", + "\n", + "**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n", + "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n\u001b[0;32m 5\u001b[0m Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", + " SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n", + " Cal3_S2)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X # Key for Pose3 variable" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating and Adding Measurements" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "1. Create the factor with a noise model, the shared calibration `K`, and parameters.\n", + "2. Add measurements (2D points) and the corresponding pose keys." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 3 measurements.\n", + "SmartFactorPose: SmartProjectionPoseFactor, z = \n", + " SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1e-09\n", + "enableEPI = false\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "\n", + "\n", + "result:\n", + "Degenerate\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "[150; 505]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 1, px = \n", + "[470; 495]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 2, px = \n", + "[480; 150]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "Factor on x0 x1 x2\n" + ] + } + ], + "source": [ + "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "smart_params = SmartProjectionParams() # Use default params\n", + "K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", + "\n", + "# Factor type includes the Calibration type\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n", + "\n", + "# Add measurements and keys (Pose keys)\n", + "smart_factor.add(Point2(150, 505), X(0))\n", + "smart_factor.add(Point2(470, 495), X(1))\n", + "smart_factor.add(Point2(480, 150), X(2))\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n", + "smart_factor.print(\"SmartFactorPose: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method uses the `Pose3` objects from `values` and the fixed `K` to triangulate the point and compute the error." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulated point result:\n", + "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" + ] + } + ], + "source": [ + "# Create Values containing Pose3 objects\n", + "values = Values()\n", + "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", + "pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", + "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n", + "\n", + "values.insert(X(0), pose0)\n", + "values.insert(X(1), pose1)\n", + "values.insert(X(2), pose2)\n", + "\n", + "# Triangulate first to see the implicit point\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulated point result:\\n{point_result}\")\n", + "\n", + "if point_result.valid():\n", + " # Calculate error\n", + " total_error = smart_factor.error(values)\n", + " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "else:\n", + " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_header_md" + }, + "source": [ + "## Linearization" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_desc_md" + }, + "source": [ + "Linearization works similarly to `SmartProjectionFactor`, producing a linear factor (default: Hessian) connecting the `Pose3` variables." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "linearize_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized Factor (showing HessianFactor structure):\n", + "RegularHessianFactor(6): density=100% keys={ x0 x1 x2 }\n", + "Augmented information matrix: (dimensions: 6, 6, 6) : \n", + "{\n", + "\t[ 1900, 4864, -698, -5014, -4686, -552; ]\n", + "\t[ 4864, 1.337e+04, 1473, -9.522e+03, -1.623e+04, 447; ]\n", + "\t[ -698, 1473, 4803, 3085, -3660, -1.118e+04; ]\n", + "\t[ -5014, -9.522e+03, 3085, 1318, 2669, -1843; ]\n", + "\t[ -4686, -1.623e+04, -3660, 2669, 4137, -1067; ]\n", + "\t[ -552, 447, -1.118e+04, -1843, -1067, 4035; ]\n", + "\t[ 1318, 2669, -1843, 768, -2084, 849; ]\n", + "\t[ 2669, 4137, -1067, -2084, 6054, -1412; ]\n", + "\t[ -1843, -1067, 4035, 849, -1412, 4392; ]\n", + "\t[ 6273, -1.411e+04, -2161, 11515, 16951, 996; ]\n", + "\t[ -141, -1903, -1.299e+04, 230, -2148, 7017; ]\n", + "\t[ -104, 5192, -1500, -373, -620, -7830; ]\n", + "\t[ -830, 6917, -785, -395, -1398, -708; ]\n", + "\t[ -969, 3198, -1569, 587, 1292, -3268; ]\n", + "\t[ 268, -1.286e+04, 1865, -122, -2205, 2048; ]\n", + "\t[ -708, -1569, -7830, 23563, 2137, 1132; ]\n", + "\t[ -321, 3767, -302, 2137, 15054, -1172; ]\n", + "\t[ -1196, 5036, 1563, 1132, -1172, 8384; ]\n", + "}\n", + "Augmented Diagonal Block [0,0]:\n", + "[ 1900, 4864, -698; ]\n", + "[ 4864, 1.337e+04, 1473; ]\n", + "[ -698, 1473, 4803; ]\n", + "\n", + "Augmented Diagonal Block [1,1]:\n", + "[ 768, -2084, 849; ]\n", + "[ -2084, 6054, -1412; ]\n", + "[ 849, -1412, 4392; ]\n", + "\n", + "Augmented Diagonal Block [2,2]:\n", + "[ 23563, 2137, 1132; ]\n", + "[ 2137, 15054, -1172; ]\n", + "[ 1132, -1172, 8384; ]\n", + "\n", + "Off-Diagonal Block [0,1]:\n", + "[ -5014, -4686, -552; ]\n", + "[ -9.522e+03, -1.623e+04, 447; ]\n", + "[ 3085, -3660, -1.118e+04; ]\n", + "\n", + "Off-Diagonal Block [0,2]:\n", + "[ 11515, 16951, 996; ]\n", + "[ 230, -2148, 7017; ]\n", + "[ -373, -620, -7830; ]\n", + "\n", + "Off-Diagonal Block [1,2]:\n", + "[ 6273, -830, -969; ]\n", + "[ -1.411e+04, 6917, 3198; ]\n", + "[ -2161, -785, -1569; ]\n", + "\n", + "Error vector:\n", + "[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n", + "Constant error term: 103876\n" + ] + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "graph.add(smart_factor)\n", + "\n", + "# Linearize (default mode is HESSIAN)\n", + "linear_factor = smart_factor.linearize(values)\n", + "\n", + "if linear_factor:\n", + " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", + " # Cast to HessianFactor to print its details\n", + " # Note: The factor dimension is Pose3::dimension (6)\n", + " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", + " if hessian_factor:\n", + " hessian_factor.print()\n", + " else:\n", + " print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n", + "else:\n", + " print(\"Linearization failed (likely due to triangulation degeneracy)\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb new file mode 100644 index 000000000..ad3009001 --- /dev/null +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -0,0 +1,418 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# SmartProjectionRigFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`SmartProjectionRigFactor` is a generalization of `SmartProjectionPoseFactor` designed for multi-camera systems (rigs).\n", + "Like other smart factors, it implicitly represents a 3D point landmark observed by multiple cameras.\n", + "\n", + "Key differences/features:\n", + "- **Multi-Camera Rig:** Assumes a fixed rig configuration, where multiple cameras (`CAMERA` instances, which include fixed intrinsics and fixed extrinsics *relative to the rig's body frame*) are defined.\n", + "- **Pose Variables:** Connects `Pose3` variables representing the pose of the **rig's body frame** in the world.\n", + "- **Multiple Observations per Pose:** Allows multiple measurements associated with the *same* body pose key, but originating from different cameras within the rig.\n", + "- **Camera Indexing:** Each measurement must be associated with both a body pose key and a `cameraId` (index) specifying which camera in the rig took the measurement.\n", + "- **Fixed Calibration/Extrinsics:** The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.\n", + "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholeCamera`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts.\n", + "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n", + "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", + "\n", + "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n", + "\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", + " SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n", + " CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n", + "from gtsam import symbol_shorthand\n", + "import graphviz\n", + "\n", + "X = symbol_shorthand.X # Key for Pose3 variable (Body Pose)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating the Rig and Factor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "1. Define the camera rig configuration: Create a `CameraSet` containing the `CAMERA` objects (with fixed intrinsics and rig-relative extrinsics).\n", + "2. Create the `SmartProjectionRigFactor` with noise, the rig, and parameters.\n", + "3. Add measurements, specifying the 2D point, the corresponding **body pose key**, and the **camera ID** within the rig." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 4 measurements from 2 unique poses.\n", + "SmartFactorRig: SmartProjectionRigFactor: \n", + " -- Measurement nr 0\n", + "key: x0\n", + "cameraId: 0\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 0 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "-- Measurement nr 1\n", + "key: x0\n", + "cameraId: 1\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 -0.1 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "-- Measurement nr 2\n", + "key: x1\n", + "cameraId: 0\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 0 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "-- Measurement nr 3\n", + "key: x1\n", + "cameraId: 1\n", + "camera in rig:\n", + "Pose3:\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + " t: 0.1 -0.1 0\n", + "\n", + "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", + "SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1e-09\n", + "enableEPI = false\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "\n", + "\n", + "result:\n", + "Degenerate\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "[300; 200]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 1, px = \n", + "[250; 201]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 2, px = \n", + "[310; 210]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "measurement 3, px = \n", + "[260; 211]\n", + "\n", + "noise model = diagonal sigmas [1; 1];\n", + "Factor on x0 x1\n" + ] + } + ], + "source": [ + "# 1. Define the Camera Rig\n", + "K = Cal3_S2(500, 500, 0, 320, 240)\n", + "# Camera 0: Forward facing, slightly offset\n", + "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n", + "cam0 = PinholeCameraCal3_S2(body_P_cam0, K)\n", + "# Camera 1: Stereo camera, right of cam0\n", + "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", + "cam1 = PinholeCameraCal3_S2(body_P_cam1, K)\n", + "\n", + "rig_cameras = CameraSetPinholeCameraCal3_S2()\n", + "rig_cameras.append(cam0)\n", + "rig_cameras.append(cam1)\n", + "rig_cameras_ptr = gtsam.make_shared_CameraSetPinholeCameraCal3_S2(rig_cameras)\n", + "\n", + "# 2. Create the Factor\n", + "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n", + "smart_params = SmartProjectionParams(linearizationMode=gtsam.LinearizationMode.HESSIAN,\n", + " degeneracyMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", + "\n", + "# Factor type includes the Camera type\n", + "smart_factor = SmartProjectionRigFactorPinholeCameraCal3_S2(smart_noise, rig_cameras_ptr, smart_params)\n", + "\n", + "# 3. Add measurements\n", + "# Observation from Body Pose X(0), Camera 0\n", + "smart_factor.add(Point2(300, 200), X(0), 0)\n", + "# Observation from Body Pose X(0), Camera 1 (stereo pair?)\n", + "smart_factor.add(Point2(250, 201), X(0), 1)\n", + "# Observation from Body Pose X(1), Camera 0\n", + "smart_factor.add(Point2(310, 210), X(1), 0)\n", + "# Observation from Body Pose X(1), Camera 1\n", + "smart_factor.add(Point2(260, 211), X(1), 1)\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n", + "smart_factor.print(\"SmartFactorRig: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The `.error(values)` method uses the `Pose3` objects (body poses) from `values` and the fixed rig configuration to triangulate the point and compute the error." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulated point result:\n", + "Valid triangulation with point [0.70307883 0.20615766 5.18676602]\n", + "\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n" + ] + } + ], + "source": [ + "# Create Values containing Body Pose3 objects\n", + "values = Values()\n", + "pose0 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))\n", + "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n", + "values.insert(X(0), pose0)\n", + "values.insert(X(1), pose1)\n", + "\n", + "# Triangulate first to see the implicit point\n", + "# The 'cameras' method internally combines body poses with rig extrinsics\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulated point result:\\n{point_result}\")\n", + "\n", + "if point_result.valid():\n", + " # Calculate error\n", + " total_error = smart_factor.error(values)\n", + " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "else:\n", + " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")\n", + " # Since mode is ZERO_ON_DEGENERACY, error should be 0\n", + " total_error = smart_factor.error(values)\n", + " print(f\"Error when degenerate: {total_error}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_header_md" + }, + "source": [ + "## Linearization" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_desc_md" + }, + "source": [ + "Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "linearize_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized Factor (HessianFactor structure):\n", + "RegularHessianFactor(6): density=100% keys={ x0 x1 }\n", + "Augmented information matrix: (dimensions: 6, 6) : \n", + "{\n", + "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", + "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", + "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", + "\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\n", + "\t[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\n", + "\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\n", + "\t[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\n", + "\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n", + "\t[ 2.81, 18.91, 6.302, -2.81, -18.91, -6.302; ]\n", + "\t[ -15.09, -101.6, -33.87, 15.09, 101.6, 33.87; ]\n", + "\t[ 3.829, 25.77, 8.589, -3.829, -25.77, -8.589; ]\n", + "\t[ -3.448, -23.21, -7.737, 3.448, 23.21, 7.737; ]\n", + "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", + "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", + "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", + "\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\n", + "\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\n", + "\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\n", + "}\n", + "Augmented Diagonal Block [0,0]:\n", + "[ 1.257, 8.427, 2.81; ]\n", + "[ 8.427, 56.73, 18.91; ]\n", + "[ 2.81, 18.91, 6.302; ]\n", + "\n", + "Augmented Diagonal Block [1,1]:\n", + "[ 1.257, 8.427, 2.81; ]\n", + "[ 8.427, 56.73, 18.91; ]\n", + "[ 2.81, 18.91, 6.302; ]\n", + "\n", + "Off-Diagonal Block [0,1]:\n", + "[ -1.257, -8.427, -2.81; ]\n", + "[ -8.427, -56.73, -18.91; ]\n", + "[ -2.81, -18.91, -6.302; ]\n", + "\n", + "Error vector:\n", + "[-15.087; -101.588; -33.8672; 181.19; -46.1294; 41.5391; 15.087; 101.588; 33.8672; -181.19; 46.1294; -41.5391]\n", + "Constant error term: 181.19\n" + ] + } + ], + "source": [ + "graph = NonlinearFactorGraph()\n", + "graph.add(smart_factor)\n", + "\n", + "# Linearize (HESSIAN mode)\n", + "linear_factor = smart_factor.linearize(values)\n", + "\n", + "if linear_factor:\n", + " print(\"\\nLinearized Factor (HessianFactor structure):\")\n", + " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", + " if hessian_factor:\n", + " hessian_factor.print()\n", + " else:\n", + " print(\"Linearized factor is not a HessianFactor\")\n", + "else:\n", + " print(\"Linearization failed (likely due to triangulation degeneracy)\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/StereoFactor.ipynb b/gtsam/slam/doc/StereoFactor.ipynb new file mode 100644 index 000000000..98b597a9f --- /dev/null +++ b/gtsam/slam/doc/StereoFactor.ipynb @@ -0,0 +1,246 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# GenericStereoFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`GenericStereoFactor` is a factor for handling measurements from a **calibrated stereo camera**.\n", + "It relates a 3D `LANDMARK` (usually `Point3`) to a `StereoPoint2` measurement observed by a stereo camera system defined by a `POSE` (usually `Pose3`) and a fixed stereo calibration `Cal3_S2Stereo`.\n", + "\n", + "`StereoPoint2` contains $(u_L, u_R, v)$, the horizontal pixel coordinates in the left ($u_L$) and right ($u_R$) images, and the vertical pixel coordinate ($v$), which is assumed the same for both images in a rectified stereo setup.\n", + "`Cal3_S2Stereo` holds the intrinsic parameters (focal length, principal point) common to both cameras and the stereo baseline (distance between camera centers).\n", + "\n", + "Key features:\n", + "- **Templated:** Works with different pose and landmark types.\n", + "- **Fixed Calibration:** Assumes the `Cal3_S2Stereo` object (`K_`) is known and fixed.\n", + "- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform.\n", + "- **Cheirality Handling:** Can be configured for points behind the camera.\n", + "\n", + "The error is the 3D vector difference:\n", + "$$ \\text{error}(P, L) = \\text{projectStereo}(P \\cdot S, L) - z $$\n", + "where `projectStereo` uses the `StereoCamera` model, $P$ is the pose, $L$ the landmark, $S$ the optional offset, and $z$ is the `measured_` `StereoPoint2`." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n", + "# The Python wrapper often creates specific instantiations\n", + "from gtsam import GenericStereoFactor3D\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a GenericStereoFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing:\n", + "1. The measurement (`StereoPoint2`).\n", + "2. The noise model (typically 3D).\n", + "3. The key for the pose variable.\n", + "4. The key for the landmark variable.\n", + "5. A `shared_ptr` to the fixed stereo calibration object (`Cal3_S2Stereo`).\n", + "6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n", + "7. (Optional) Cheirality handling flags." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor with offset: keys = { x0 l1 }\n", + " noise model: unit (3) \n", + "Factor with offset: .z(330, 305, 250)\n", + " sensor pose in body frame: R: [\n", + "\t6.12323e-17, 6.12323e-17, 1;\n", + "\t-1, 3.7494e-33, 6.12323e-17;\n", + "\t-0, -1, 6.12323e-17\n", + "]\n", + "t: 0.1 0 0.2\n", + "\n", + "Factor without offset: keys = { x0 l1 }\n", + " noise model: unit (3) \n", + "\n", + "Factor without offset: .z(330, 305, 250)\n" + ] + } + ], + "source": [ + "measured_stereo = StereoPoint2(330, 305, 250) # uL, uR, v\n", + "stereo_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) # 1 pixel std dev (ul, ur, v)\n", + "pose_key = X(0)\n", + "landmark_key = L(1)\n", + "\n", + "# Shared pointer to stereo calibration\n", + "K_stereo = Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.1) # fx, fy, s, u0, v0, baseline\n", + "\n", + "# Optional sensor pose offset\n", + "body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n", + "\n", + "# Create factor with sensor offset\n", + "factor_with_offset = GenericStereoFactor3D(\n", + " measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo, body_P_sensor=body_P_sensor)\n", + "factor_with_offset.print(\"Factor with offset: \")\n", + "\n", + "# Create factor without sensor offset\n", + "factor_no_offset = GenericStereoFactor3D(\n", + " measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo)\n", + "factor_no_offset.print(\"\\nFactor without offset: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error is the 3D difference between the predicted stereo projection and the measurement." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 4\u001b[0m pose \u001b[38;5;241m=\u001b[39m Pose3(Rot3\u001b[38;5;241m.\u001b[39mRodrigues(\u001b[38;5;241m0.1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m0.2\u001b[39m, \u001b[38;5;241m0.3\u001b[39m), Point3(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m0.5\u001b[39m))\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Triangulate a point that *should* project to measured_stereo\u001b[39;00m\n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m expected_point_camera \u001b[38;5;241m=\u001b[39m \u001b[43mK_stereo\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mbackproject\u001b[49m(measured_stereo)\n\u001b[0;32m 8\u001b[0m landmark \u001b[38;5;241m=\u001b[39m pose\u001b[38;5;241m.\u001b[39mtransformFrom(expected_point_camera)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mExpected landmark point: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlandmark\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Example values\n", + "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n", + "# Triangulate a point that *should* project to measured_stereo\n", + "# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n", + "expected_point_camera = K_stereo.backproject(measured_stereo)\n", + "landmark = pose.transformFrom(expected_point_camera)\n", + "print(f\"Expected landmark point: {landmark}\")\n", + "\n", + "values.insert(pose_key, pose)\n", + "values.insert(landmark_key, landmark)\n", + "\n", + "# Evaluate factor without offset\n", + "error_no_offset = factor_no_offset.error(values)\n", + "print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be zero)\")\n", + "\n", + "# Evaluate factor with offset\n", + "# Need to recompute landmark based on offset pose\n", + "pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n", + "expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n", + "landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n", + "values.update(landmark_key, landmark_offset)\n", + "error_with_offset = factor_with_offset.error(values)\n", + "print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be zero)\")\n", + "\n", + "# Evaluate with noisy landmark\n", + "noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n", + "values.update(landmark_key, noisy_landmark)\n", + "error_no_offset_noisy = factor_no_offset.error(values)\n", + "print(f\"Error (no offset) at noisy landmark: {error_no_offset_noisy}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/TriangulationFactor.ipynb b/gtsam/slam/doc/TriangulationFactor.ipynb new file mode 100644 index 000000000..0bf9ea75a --- /dev/null +++ b/gtsam/slam/doc/TriangulationFactor.ipynb @@ -0,0 +1,283 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# TriangulationFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "`TriangulationFactor` is a unary factor that constrains a single unknown 3D point (`Point3`) based on a 2D measurement from a **known** camera.\n", + "It's essentially the inverse of a projection factor where the camera pose and calibration are fixed, and only the 3D point is variable.\n", + "\n", + "Key characteristics:\n", + "- **Unary Factor:** Connects only to a `Point3` variable key.\n", + "- **Known Camera:** The `CAMERA` object (e.g., `PinholeCameraCal3_S2`, `StereoCamera`) containing the fixed pose and calibration is provided during factor construction.\n", + "- **Measurement:** Takes a 2D measurement (`Point2` for monocular, `StereoPoint2` for stereo).\n", + "- **Error:** Calculates the reprojection error: $ \\text{error}(L) = \\text{camera.project}(L) - z $\n", + "\n", + "**Use Case:** Useful in triangulation scenarios where multiple camera views with known poses observe an unknown landmark. A `NonlinearFactorGraph` containing only `TriangulationFactor`s (one for each view) can be optimized to find the maximum likelihood estimate of the 3D landmark position." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam import Point3, Point2, Pose3, Rot3, Cal3_S2, PinholeCameraCal3_S2, Values, NonlinearFactorGraph\n", + "# The Python wrapper often creates specific instantiations\n", + "from gtsam import TriangulationFactorCal3_S2\n", + "from gtsam import symbol_shorthand\n", + "\n", + "L = symbol_shorthand.L" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_header_md" + }, + "source": [ + "## Creating a TriangulationFactor" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "create_desc_md" + }, + "source": [ + "Instantiate by providing:\n", + "1. The known `CAMERA` object.\n", + "2. The 2D measurement.\n", + "3. The noise model for the measurement.\n", + "4. The key for the unknown `Point3` landmark.\n", + "5. (Optional) Cheirality handling flags." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "create_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "TriangulationFactor: TriangulationFactor,camera.pose R: [\n", + "\t0.990033, -0.117578, -0.0775207;\n", + "\t0.0993347, 0.97319, -0.207445;\n", + "\t0.0998334, 0.197677, 0.97517\n", + "]\n", + "t: 1 0 0.5\n", + "camera.calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "z[\n", + "\t450.5;\n", + "\t300.2\n", + "]\n", + " keys = { l0 }\n", + " noise model: unit (2) \n" + ] + } + ], + "source": [ + "# Known camera parameters\n", + "K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n", + "pose = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(1, 0, 0.5))\n", + "camera = PinholeCameraCal3_S2(pose, K)\n", + "\n", + "# Measurement observed by this camera\n", + "measured_pt2 = Point2(450.5, 300.2)\n", + "pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "landmark_key = L(0)\n", + "\n", + "# Factor type includes the Camera type\n", + "factor = TriangulationFactorCal3_S2(camera, measured_pt2, pixel_noise, landmark_key)\n", + "factor.print(\"TriangulationFactor: \")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_header_md" + }, + "source": [ + "## Evaluating the Error" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eval_desc_md" + }, + "source": [ + "The error is the reprojection error given an estimate of the landmark's `Point3` position." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "eval_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Reprojection error for estimate [3. 0.5 2. ]: 314012.75623020524\n", + "Expected projection: [1225.10768109 467.55726116]\n", + "Manual error calculation: [774.60768109 167.35726116]\n" + ] + } + ], + "source": [ + "values = Values()\n", + "\n", + "# Estimate for the landmark position\n", + "landmark_estimate = Point3(3.0, 0.5, 2.0)\n", + "values.insert(landmark_key, landmark_estimate)\n", + "\n", + "error = factor.error(values)\n", + "print(f\"Reprojection error for estimate {landmark_estimate}: {error}\")\n", + "\n", + "# Calculate expected projection\n", + "expected_projection = camera.project(landmark_estimate)\n", + "manual_error = expected_projection - measured_pt2\n", + "print(f\"Expected projection: {expected_projection}\")\n", + "print(f\"Manual error calculation: {manual_error}\")\n", + "assert np.allclose(factor.unwhitenedError(values), manual_error)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_header_md" + }, + "source": [ + "## Usage in Triangulation" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "usage_desc_md" + }, + "source": [ + "Multiple `TriangulationFactor`s, one for each known camera view, can be added to a graph to solve for the landmark position." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "usage_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Optimized Landmark Position:\n", + "Values with 1 values:\n", + "Value l0: (class Eigen::Matrix)\n", + "[\n", + "\t-12446.1;\n", + "\t-55075.8;\n", + "\t2.39319e+06\n", + "]\n", + "\n", + "Final Error: 7855.8598\n" + ] + } + ], + "source": [ + "# Create a second camera and measurement\n", + "pose2 = Pose3(Rot3.Ypr(-0.1, 0.1, -0.1), Point3(-1, 0, 0.5))\n", + "camera2 = PinholeCameraCal3_S2(pose2, K)\n", + "measured_pt2_cam2 = Point2(180.0, 190.0)\n", + "factor2 = TriangulationFactorCal3_S2(camera2, measured_pt2_cam2, pixel_noise, landmark_key)\n", + "\n", + "# Create graph and add factors\n", + "triangulation_graph = NonlinearFactorGraph()\n", + "triangulation_graph.add(factor)\n", + "triangulation_graph.add(factor2)\n", + "\n", + "# Optimize (requires an initial estimate)\n", + "initial_estimate = Values()\n", + "initial_estimate.insert(landmark_key, Point3(2, 0, 5)) # Initial guess\n", + "\n", + "optimizer = gtsam.LevenbergMarquardtOptimizer(triangulation_graph, initial_estimate)\n", + "result = optimizer.optimize()\n", + "\n", + "print(\"\\nOptimized Landmark Position:\")\n", + "result.print()\n", + "print(f\"Final Error: {triangulation_graph.error(result):.4f}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/dataset.ipynb b/gtsam/slam/doc/dataset.ipynb new file mode 100644 index 000000000..2c2130cf3 --- /dev/null +++ b/gtsam/slam/doc/dataset.ipynb @@ -0,0 +1,252 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# Dataset Utilities" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `gtsam/slam/dataset.h` header provides utility functions for loading and saving factor graph data, particularly in formats commonly used in the SLAM community like TORO ('.graph') and g2o ('.g2o').\n", + "\n", + "Key functions include:\n", + "* `findExampleDataFile(name)`: Locates example dataset files distributed with GTSAM.\n", + "* `load2D(filename, ...)`: Loads a 2D pose graph (Poses and BetweenFactors) from TORO or g2o format.\n", + "* `load3D(filename)`: Loads a 3D pose graph (currently simpler than `load2D`, assumes specific format).\n", + "* `readG2o(filename, is3D)`: A more general function to read g2o files containing various factor and variable types (2D/3D poses, landmarks, measurements).\n", + "* `writeG2o(graph, initialEstimate, filename)`: Saves a factor graph and values to the g2o format.\n", + "* `parseVariables`/`parseMeasurements`/`parseFactors`: Lower-level parsing functions (less commonly used directly)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import gtsam.utils.plot as gtsam_plot\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "import os\n", + "\n", + "# Ensure the example datasets are available\n", + "# In Colab/pip install, they are usually included.\n", + "# If running locally from build, findExampleDataFile works.\n", + "# If running locally from install without examples, this might fail.\n", + "try:\n", + " # Check for a common example file\n", + " gtsam.findExampleDataFile(\"w100.graph\")\n", + " HAVE_EXAMPLES = True\n", + "except RuntimeError:\n", + " print(\"Warning: Example datasets not found.\")\n", + " print(\"Try running from build directory or installing examples.\")\n", + " HAVE_EXAMPLES = False\n", + "\n", + "# Create dummy files for writing examples if needed\n", + "if not os.path.exists(\"output\"):\n", + " os.makedirs(\"output\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "find_header_md" + }, + "source": [ + "## Finding Example Datasets" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "find_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Found w100.graph at: c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\Data\\w100.graph\n", + "Found dlr.g2o at: None\n" + ] + } + ], + "source": [ + "if HAVE_EXAMPLES:\n", + " try:\n", + " w100_path = gtsam.findExampleDataFile(\"w100.graph\")\n", + " print(f\"Found w100.graph at: {w100_path}\")\n", + " dlr_path = gtsam.findExampleDataFile(\"dlr.g2o\")\n", + " print(f\"Found dlr.g2o at: {dlr_path}\")\n", + " except RuntimeError as e:\n", + " print(f\"Error finding example file: {e}\")\n", + "else:\n", + " print(\"Skipping findExampleDataFile test as examples are not available.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "load2d_header_md" + }, + "source": [ + "## Loading 2D Datasets (`load2D`)" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": { + "id": "load2d_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Loaded c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\Data\\w100.graph:\n", + " Graph size: 300\n", + " Initial estimate keys: 100\n" + ] + } + ], + "source": [ + "if HAVE_EXAMPLES:\n", + " # Load TORO 2D file\n", + " graph_2d, initial_2d = gtsam.load2D(w100_path)\n", + " print(f\"\\nLoaded {w100_path}:\")\n", + " print(f\" Graph size: {graph_2d.size()}\")\n", + " print(f\" Initial estimate keys: {len(initial_2d.keys())}\")\n", + "\n", + " # Plot initial estimate (optional)\n", + " for key in initial_2d.keys():\n", + " gtsam_plot.plot_pose2(0, initial_2d.atPose2(key))\n", + " plt.title(\"Initial Estimate from w100.graph\")\n", + " plt.axis('equal')\n", + " # plt.show() # Uncomment to display plot\n", + " plt.close() # Close plot to prevent display in output\n", + "else:\n", + " print(\"\\nSkipping load2D test.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "g2o_header_md" + }, + "source": [ + "## Loading/Saving G2O Files (`readG2o`, `writeG2o`)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "g2o_desc_md" + }, + "source": [ + "`readG2o` can handle both 2D and 3D datasets and various factor types defined in the g2o format.\n", + "`writeG2o` saves a `NonlinearFactorGraph` and `Values` into a g2o file." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "id": "g2o_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Error processing None: readG2o(): incompatible function arguments. The following argument types are supported:\n", + " 1. (filename: str, is3D: bool = False, kernelFunctionType: gtsam.gtsam.KernelFunctionType = ) -> tuple[gtsam.gtsam.NonlinearFactorGraph, gtsam.gtsam.Values]\n", + "\n", + "Invoked with: None; kwargs: is3D=True\n" + ] + } + ], + "source": [ + "if HAVE_EXAMPLES:\n", + " # Load a 3D g2o file\n", + " try:\n", + " graph_3d, initial_3d = gtsam.readG2o(dlr_path, is3D=True)\n", + " print(f\"\\nLoaded {dlr_path} (3D):\")\n", + " print(f\" Graph size: {graph_3d.size()}\")\n", + " print(f\" Initial estimate keys: {len(initial_3d.keys())}\")\n", + " # You could optimize graph_3d with initial_3d here...\n", + " print(\"Optimization skipped for brevity.\")\n", + " optimized_values = initial_3d # Use initial for demo write\n", + "\n", + " # Save the graph and values back to a g2o file\n", + " output_g2o_file = os.path.join(\"output\", \"dlr_rewrite.g2o\")\n", + " gtsam.writeG2o(graph_3d, optimized_values, output_g2o_file)\n", + " print(f\" Saved graph and values to {output_g2o_file}\")\n", + " # Clean up dummy file\n", + " # os.remove(output_g2o_file)\n", + " # os.rmdir(\"output\")\n", + " except (RuntimeError, TypeError) as e:\n", + " print(f\"Error processing {dlr_path}: {e}\")\n", + "else:\n", + " print(\"\\nSkipping readG2o/writeG2o test.\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb new file mode 100644 index 000000000..3bd5de668 --- /dev/null +++ b/gtsam/slam/doc/lago.ipynb @@ -0,0 +1,242 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "intro_md" + }, + "source": [ + "# LAGO (Linear Approximation for Graph Optimization)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "desc_md" + }, + "source": [ + "The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n", + "LAGO stands for Linear Approximation for Graph Optimization. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", + "\n", + "The core idea is:\n", + "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", + "2. **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n", + "\n", + "Key functions:\n", + "* `initializeOrientations(graph)`: Computes initial estimates for the `Rot2` (orientation) components of the poses in the graph.\n", + "* `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n", + "* `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n", + "\n", + "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "colab_badge_md" + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "pip_code", + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "id": "imports_code" + }, + "outputs": [], + "source": [ + "import gtsam\n", + "import gtsam.utils.plot as gtsam_plot\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "from gtsam import NonlinearFactorGraph, Values, Pose2, Point3, PriorFactorPose2, BetweenFactorPose2\n", + "from gtsam import lago\n", + "from gtsam import symbol_shorthand\n", + "\n", + "X = symbol_shorthand.X" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_header_md" + }, + "source": [ + "## Example Initialization" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "example_desc_md" + }, + "source": [ + "We'll create a simple 2D pose graph with odometry and a loop closure, then use `lago.initialize`." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": { + "id": "example_pipeline_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Original Factor Graph:\n", + "size: 6\n", + "\n", + "Factor 0: PriorFactor on x0\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "\n", + "Factor 1: BetweenFactor(x0,x1)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 2: BetweenFactor(x1,x2)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 3: BetweenFactor(x2,x3)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor 4: BetweenFactor(x3,x4)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 5: BetweenFactor(x4,x0)\n", + " measured: (2.1, 0.1, 1.62079633)\n", + " noise model: diagonal sigmas [0.25; 0.25; 0.15];\n", + "\n" + ] + }, + { + "ename": "IndexError", + "evalue": "invalid map key", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mIndexError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 25\u001b[0m\n\u001b[0;32m 22\u001b[0m graph\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOriginal Factor Graph:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;66;03m# 2. Perform LAGO initialization\u001b[39;00m\n\u001b[1;32m---> 25\u001b[0m initial_estimate_lago \u001b[38;5;241m=\u001b[39m \u001b[43mlago\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitialize\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mInitial Estimate from LAGO:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 28\u001b[0m initial_estimate_lago\u001b[38;5;241m.\u001b[39mprint()\n", + "\u001b[1;31mIndexError\u001b[0m: invalid map key" + ] + } + ], + "source": [ + "# 1. Create a NonlinearFactorGraph with Pose2 factors\n", + "graph = NonlinearFactorGraph()\n", + "\n", + "# Add a prior on the first pose\n", + "prior_mean = Pose2(0.0, 0.0, 0.0)\n", + "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n", + "graph.add(PriorFactorPose2(X(0), prior_mean, prior_noise))\n", + "\n", + "# Add odometry factors (simulating moving in a square)\n", + "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", + "graph.add(BetweenFactorPose2(X(0), X(1), Pose2(2.0, 0.0, 0.0), odometry_noise))\n", + "graph.add(BetweenFactorPose2(X(1), X(2), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(X(2), X(3), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(X(3), X(4), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "\n", + "# Add a loop closure factor\n", + "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n", + "# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n", + "measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n", + "graph.add(BetweenFactorPose2(X(4), X(0), measured_loop, loop_noise))\n", + "\n", + "graph.print(\"Original Factor Graph:\\n\")\n", + "\n", + "# 2. Perform LAGO initialization\n", + "initial_estimate_lago = lago.initialize(graph)\n", + "\n", + "print(\"\\nInitial Estimate from LAGO:\\n\")\n", + "initial_estimate_lago.print()\n", + "\n", + "# 3. Visualize the LAGO estimate (optional)\n", + "plt.figure(1)\n", + "for key in initial_estimate_lago.keys():\n", + " gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n", + "plt.title(\"LAGO Initial Estimate\")\n", + "plt.axis('equal')\n", + "# plt.show()\n", + "plt.close() # Close plot to prevent display in output\n", + "\n", + "# 4. Compare orientation initialization\n", + "initial_orientations = lago.initializeOrientations(graph)\n", + "print(\"\\nLAGO Orientations (VectorValues):\")\n", + "initial_orientations.print()\n", + "\n", + "print(\"Orientations from full LAGO Values:\")\n", + "for i in range(5):\n", + " print(f\" X({i}): {initial_estimate_lago.atPose2(X(i)).theta():.4f}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_header_md" + }, + "source": [ + "## Notes" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "notes_desc_md" + }, + "source": [ + "- LAGO provides a good initial guess, especially for orientations, which can significantly help the convergence of nonlinear optimizers.\n", + "- It assumes the graph structure allows for the orientation estimation (typically requires a spanning tree and a prior).\n", + "- The translation estimates are computed based on the fixed, estimated orientations." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "gtsam", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.1" + } + }, + "nbformat": 4, + "nbformat_minor": 0 +} From e4cf0a2cf90101e92ec4eea8ec79b29c98a010ee Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 24 Apr 2025 10:55:19 -0400 Subject: [PATCH 16/38] pip install gtsam-develop --- gtsam/slam/doc/BetweenFactor.ipynb | 2 +- gtsam/slam/doc/EssentialMatrixConstraint.ipynb | 2 +- gtsam/slam/doc/EssentialMatrixFactor.ipynb | 2 +- gtsam/slam/doc/FrobeniusFactor.ipynb | 2 +- gtsam/slam/doc/GeneralSFMFactor.ipynb | 2 +- gtsam/slam/doc/InitializePose3.ipynb | 2 +- gtsam/slam/doc/KarcherMeanFactor.ipynb | 2 +- gtsam/slam/doc/OrientedPlane3Factor.ipynb | 2 +- gtsam/slam/doc/PlanarProjectionFactor.ipynb | 2 +- gtsam/slam/doc/PoseRotationPrior.ipynb | 2 +- gtsam/slam/doc/PoseTranslationPrior.ipynb | 2 +- gtsam/slam/doc/ProjectionFactor.ipynb | 2 +- gtsam/slam/doc/ReferenceFrameFactor.ipynb | 2 +- gtsam/slam/doc/RotateFactor.ipynb | 2 +- gtsam/slam/doc/SmartFactorParams.ipynb | 2 +- gtsam/slam/doc/SmartProjectionFactor.ipynb | 2 +- gtsam/slam/doc/SmartProjectionPoseFactor.ipynb | 2 +- gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 2 +- gtsam/slam/doc/StereoFactor.ipynb | 2 +- gtsam/slam/doc/TriangulationFactor.ipynb | 2 +- gtsam/slam/doc/dataset.ipynb | 2 +- gtsam/slam/doc/lago.ipynb | 2 +- 22 files changed, 22 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/doc/BetweenFactor.ipynb b/gtsam/slam/doc/BetweenFactor.ipynb index 4eef414bb..da2d98e63 100644 --- a/gtsam/slam/doc/BetweenFactor.ipynb +++ b/gtsam/slam/doc/BetweenFactor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/EssentialMatrixConstraint.ipynb b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb index fe54c0515..b8d23505b 100644 --- a/gtsam/slam/doc/EssentialMatrixConstraint.ipynb +++ b/gtsam/slam/doc/EssentialMatrixConstraint.ipynb @@ -47,7 +47,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/EssentialMatrixFactor.ipynb b/gtsam/slam/doc/EssentialMatrixFactor.ipynb index cac43612c..9291b2ec0 100644 --- a/gtsam/slam/doc/EssentialMatrixFactor.ipynb +++ b/gtsam/slam/doc/EssentialMatrixFactor.ipynb @@ -48,7 +48,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/FrobeniusFactor.ipynb b/gtsam/slam/doc/FrobeniusFactor.ipynb index fdf317da8..cba959dc6 100644 --- a/gtsam/slam/doc/FrobeniusFactor.ipynb +++ b/gtsam/slam/doc/FrobeniusFactor.ipynb @@ -45,7 +45,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/GeneralSFMFactor.ipynb b/gtsam/slam/doc/GeneralSFMFactor.ipynb index 366254d1a..3fdfb6408 100644 --- a/gtsam/slam/doc/GeneralSFMFactor.ipynb +++ b/gtsam/slam/doc/GeneralSFMFactor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/InitializePose3.ipynb b/gtsam/slam/doc/InitializePose3.ipynb index 2e65b906b..b132697b2 100644 --- a/gtsam/slam/doc/InitializePose3.ipynb +++ b/gtsam/slam/doc/InitializePose3.ipynb @@ -48,7 +48,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb index a1ca7a51e..973987286 100644 --- a/gtsam/slam/doc/KarcherMeanFactor.ipynb +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb index e140dbd71..1600674b2 100644 --- a/gtsam/slam/doc/OrientedPlane3Factor.ipynb +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -43,7 +43,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb index ce7c37a28..55d76252d 100644 --- a/gtsam/slam/doc/PlanarProjectionFactor.ipynb +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -50,7 +50,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/PoseRotationPrior.ipynb b/gtsam/slam/doc/PoseRotationPrior.ipynb index cb7d09a75..444658711 100644 --- a/gtsam/slam/doc/PoseRotationPrior.ipynb +++ b/gtsam/slam/doc/PoseRotationPrior.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/PoseTranslationPrior.ipynb b/gtsam/slam/doc/PoseTranslationPrior.ipynb index 047bf5e59..a71c61c02 100644 --- a/gtsam/slam/doc/PoseTranslationPrior.ipynb +++ b/gtsam/slam/doc/PoseTranslationPrior.ipynb @@ -44,7 +44,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/ProjectionFactor.ipynb b/gtsam/slam/doc/ProjectionFactor.ipynb index 50bc5d71d..43d7e0f07 100644 --- a/gtsam/slam/doc/ProjectionFactor.ipynb +++ b/gtsam/slam/doc/ProjectionFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb index 5ea646d19..13ca2adef 100644 --- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/RotateFactor.ipynb b/gtsam/slam/doc/RotateFactor.ipynb index a62e986ad..edb658d05 100644 --- a/gtsam/slam/doc/RotateFactor.ipynb +++ b/gtsam/slam/doc/RotateFactor.ipynb @@ -41,7 +41,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartFactorParams.ipynb b/gtsam/slam/doc/SmartFactorParams.ipynb index b19218538..8ff32bdc1 100644 --- a/gtsam/slam/doc/SmartFactorParams.ipynb +++ b/gtsam/slam/doc/SmartFactorParams.ipynb @@ -57,7 +57,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index afb0daa1e..8dd7606ff 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index 9f56fc8c9..be34d6b44 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -49,7 +49,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index ad3009001..09d23b3ca 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -51,7 +51,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/StereoFactor.ipynb b/gtsam/slam/doc/StereoFactor.ipynb index 98b597a9f..08c1b6ab4 100644 --- a/gtsam/slam/doc/StereoFactor.ipynb +++ b/gtsam/slam/doc/StereoFactor.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/TriangulationFactor.ipynb b/gtsam/slam/doc/TriangulationFactor.ipynb index 0bf9ea75a..df324ad95 100644 --- a/gtsam/slam/doc/TriangulationFactor.ipynb +++ b/gtsam/slam/doc/TriangulationFactor.ipynb @@ -47,7 +47,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/dataset.ipynb b/gtsam/slam/doc/dataset.ipynb index 2c2130cf3..cb9c40741 100644 --- a/gtsam/slam/doc/dataset.ipynb +++ b/gtsam/slam/doc/dataset.ipynb @@ -46,7 +46,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb index 3bd5de668..aa6d0e98b 100644 --- a/gtsam/slam/doc/lago.ipynb +++ b/gtsam/slam/doc/lago.ipynb @@ -50,7 +50,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam" + "%pip install --quiet gtsam-develop" ] }, { From e226cdde9f8c12c48556170cb5c9ce9dd3fb0b89 Mon Sep 17 00:00:00 2001 From: p-zach Date: Thu, 24 Apr 2025 10:59:02 -0400 Subject: [PATCH 17/38] yml and summary --- gtsam/slam/slam.md | 54 ++++++++++++++++++++++++++++++++++++++++++++++ myst.yml | 3 +++ 2 files changed, 57 insertions(+) create mode 100644 gtsam/slam/slam.md diff --git a/gtsam/slam/slam.md b/gtsam/slam/slam.md new file mode 100644 index 000000000..4ae7875c7 --- /dev/null +++ b/gtsam/slam/slam.md @@ -0,0 +1,54 @@ +# SLAM Factors and Algorithms + +The `slam` module provides a collection of factors, constraints, utilities, and initialization algorithms commonly used in Simultaneous Localization and Mapping (SLAM) and Structure from Motion (SfM) applications. It builds upon the core GTSAM inference engine (`gtsam/inference`) and geometric types (`gtsam/geometry`). + +## Core Factors + +These are fundamental factor types often used as building blocks in SLAM. + +- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., odometry). +- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable. +- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable. + +## Visual SLAM/SfM Factors + +Factors specifically designed for visual data (camera measurements). + +- [GenericProjectionFactor](doc/GenericProjectionFactor.ipynb) : Standard monocular projection factor relating a 3D landmark, camera pose, and fixed calibration to a 2D measurement. +- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement. +- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane. +- [GeneralSFMFactor](doc/GeneralSFMFactor.ipynb) : Projection factors used when camera calibration is unknown or optimized alongside poses and landmarks. +- [EssentialMatrixFactor](doc/EssentialMatrixFactor.ipynb) : Factors constraining poses or calibration based on the Essential matrix derived from calibrated cameras. +- [EssentialMatrixConstraint](doc/EssentialMatrixConstraint.ipynb) : Factor constraining the relative pose between two cameras based on a measured Essential matrix. +- [TriangulationFactor](doc/TriangulationFactor.ipynb) : Factor constraining a 3D point based on a measurement from a single known camera view, useful for triangulation. + +## Smart Factors + +Factors that implicitly manage landmark variables, marginalizing them out during optimization. + +- [SmartFactorParams](doc/SmartFactorParams.ipynb) : Configuration parameters controlling the behavior of smart factors (linearization, degeneracy handling, etc.). +- [SmartProjectionFactor](doc/SmartProjectionFactor.ipynb) : Smart factor for monocular measurements where both camera pose and calibration are optimized. +- [SmartProjectionPoseFactor](doc/SmartProjectionPoseFactor.ipynb) : Smart factor for monocular measurements where camera calibration is fixed, optimizing only poses. +- [SmartProjectionRigFactor](doc/SmartProjectionRigFactor.ipynb) : Smart factor for calibrated multi-camera rigs, optimizing only the rig's body pose. +- [SmartFactorBase](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorBase.h) : Abstract base class for smart factors (internal use). + +## Other Geometric Factors & Constraints + +Factors representing various geometric relationships or constraints. + +- [OrientedPlane3Factor](doc/OrientedPlane3Factor.ipynb) : Factors for estimating and constraining 3D planar landmarks (`OrientedPlane3`). +- [RotateFactor](doc/RotateFactor.ipynb) : Factors constraining an unknown rotation based on how it transforms measured rotations or directions. +- [KarcherMeanFactor](doc/KarcherMeanFactor.ipynb) : Factor for constraining the Karcher mean (geometric average) of a set of rotations or other manifold values. +- [FrobeniusFactor](doc/FrobeniusFactor.ipynb) : Factors operating directly on rotation matrix entries using the Frobenius norm, an alternative to Lie algebra-based factors. +- [ReferenceFrameFactor](doc/ReferenceFrameFactor.ipynb) : Factor relating the same landmark observed in two different coordinate frames via an unknown transformation, useful for map merging. +- [BoundingConstraint](doc/BoundingConstraint.ipynb) : Abstract base class for creating inequality constraints (e.g., keeping a variable within certain bounds). Requires C++ derivation. +- [AntiFactor](doc/AntiFactor.ipynb) : A factor designed to negate the effect of another factor, useful for dynamically removing constraints. + +## Initialization & Utilities + +Helper functions and classes for SLAM tasks. + +- [lago](doc/lago.ipynb) : Linear Approximation for Graph Optimization (LAGO) for initializing `Pose2` graphs. +- [InitializePose3](doc/InitializePose3.ipynb) : Methods for initializing `Pose3` graphs by first solving for rotations, then translations. +- [dataset](doc/dataset.ipynb) : Utility functions for loading/saving common SLAM dataset formats (g2o, TORO). +- [expressions](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/expressions.h) : Pre-defined Expression trees for common SLAM factor types (internal use for Expression-based factors). \ No newline at end of file diff --git a/myst.yml b/myst.yml index 5fbe34010..f0b62c88f 100644 --- a/myst.yml +++ b/myst.yml @@ -25,6 +25,9 @@ project: - file: ./gtsam/navigation/navigation.md children: - pattern: ./gtsam/navigation/doc/* + - file: ./gtsam/slam/slam.md + children: + - pattern: ./gtsam/slam/doc/* - file: ./doc/examples.md children: - pattern: ./python/gtsam/examples/*.ipynb From 423e447de0f559e3f3a85bc8c60b85a4ee2e0fa0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 15:49:20 -0400 Subject: [PATCH 18/38] Two new modules --- python/gtsam/examples/EKF_SLAM.ipynb | 404 +++++++------------------- python/gtsam/examples/gtsam_plotly.py | 290 ++++++++++++++++++ python/gtsam/examples/simulation.py | 137 +++++++++ 3 files changed, 527 insertions(+), 304 deletions(-) create mode 100644 python/gtsam/examples/gtsam_plotly.py create mode 100644 python/gtsam/examples/simulation.py diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 9422204f1..84e69e1a1 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -62,12 +62,17 @@ " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", "4. **Iteration:** Repeat prediction and update for each time step.\n", "\n", + "**Enhancements in this version:**\n", + "* Simulation code is moved to `simulation.py`.\n", + "* Plotting code is moved to `gtsam_plotly.py`.\n", + "* The evolving factor graph is displayed at each step using `graphviz`.\n", + "\n", "**Advantages:**\n", "* Leverages GTSAM's robust factor graph machinery.\n", "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", "* Avoids explicit management of large, dense covariance matrices.\n", "\n", - "**Output:** The process will be visualized using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + "**Output:** The process will be visualized with the evolving factor graph, and finally using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." ] }, { @@ -101,13 +106,22 @@ "outputs": [], "source": [ "import numpy as np\n", - "import matplotlib.pyplot as plt # For initial plot if desired\n", + "import matplotlib.pyplot as plt # For initial plot if desired (optional)\n", "import plotly.graph_objects as go\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", + "import time # To slow down graphviz display if needed\n", "\n", "import gtsam\n", - "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks" + "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", + "\n", + "# Imports for new modules\n", + "import simulation\n", + "import gtsam_plotly \n", + "\n", + "# Imports for graph visualization\n", + "import graphviz\n", + "from IPython.display import display, clear_output" ] }, { @@ -132,7 +146,7 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", + "NUM_STEPS = 20 # Reduced steps for faster animation generation\n", "DT = 1.0 # Time step duration (simplified)\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", @@ -143,13 +157,12 @@ "# Measurement noise (bearing, range)\n", "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", - "# Create samplers for noise models\n", - "prior_noise_sampler = gtsam.Sampler(PRIOR_NOISE, 42)\n", - "odometry_noise_sampler = gtsam.Sampler(ODOMETRY_NOISE, 42)\n", - "measurement_noise_sampler = gtsam.Sampler(MEASUREMENT_NOISE, 42)\n", - "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0" + "MAX_SENSOR_RANGE = 5.0\n", + "\n", + "# Visualization parameters\n", + "SHOW_GRAPHVIZ_EACH_STEP = True # Set to False to only show final graph\n", + "GRAPHVIZ_PAUSE_SECONDS = 1 # Pause briefly to allow viewing the graph" ] }, { @@ -159,79 +172,30 @@ "source": [ "## 3. Generate Ground Truth Data\n", "\n", - "Create true landmark positions, robot path, and simulate noisy measurements." + "Use the `simulation` module to create true landmark positions, robot path, and simulate noisy measurements." ] }, { "cell_type": "code", "execution_count": null, - "id": "ground-truth-code-srif", + "id": "ground-truth-call-srif", "metadata": {}, "outputs": [], "source": [ - "# 3.1 Ground Truth Landmarks\n", - "landmarks_gt = (np.random.rand(2, NUM_LANDMARKS) - 0.5) * WORLD_SIZE\n", - "landmark_ids = list(range(NUM_LANDMARKS))\n", - "landmarks_gt_dict = {L(i): gtsam.Point2(landmarks_gt[:, i]) for i in range(NUM_LANDMARKS)}\n", - "\n", - "# 3.2 Ground Truth Robot Path\n", - "poses_gt = []\n", - "current_pose_gt = gtsam.Pose2(ROBOT_RADIUS, 0, np.pi / 2) # Start on circle edge\n", - "poses_gt.append(current_pose_gt)\n", - "\n", - "for k in range(NUM_STEPS):\n", - " delta_theta = ROBOT_ANGULAR_VEL * DT\n", - " arc_length = ROBOT_ANGULAR_VEL * ROBOT_RADIUS * DT\n", - " motion_command = gtsam.Pose2(arc_length, 0, delta_theta)\n", - " current_pose_gt = current_pose_gt.compose(motion_command)\n", - " poses_gt.append(current_pose_gt)\n", - "\n", - "# 3.3 Simulate Noisy Odometry Measurements (as Pose2 objects)\n", - "odometry_measurements = []\n", - "for k in range(NUM_STEPS):\n", - " pose_k = poses_gt[k]\n", - " pose_k1 = poses_gt[k+1]\n", - " true_odom = pose_k.between(pose_k1)\n", - " \n", - " # Sample noise directly for Pose2 composition (approximate)\n", - " # A more rigorous approach samples from the tangent space\n", - " odom_noise_vec = odometry_noise_sampler.sample()\n", - " noisy_odom = true_odom.compose(gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]))\n", - " odometry_measurements.append(noisy_odom)\n", - "\n", - "# 3.4 Simulate Noisy Bearing-Range Measurements\n", - "# measurements_sim[k] = list of (L(lm_id), range, bearing) for measurements *at* pose k\n", - "measurements_sim = [[] for _ in range(NUM_STEPS + 1)]\n", - "for k in range(NUM_STEPS + 1):\n", - " robot_pose = poses_gt[k]\n", - " for lm_id in range(NUM_LANDMARKS):\n", - " lm_gt_pt = landmarks_gt_dict[L(lm_id)]\n", - " try:\n", - " # Use BearingRangeFactor2D to simulate the measurement model\n", - " measurement_factor = gtsam.BearingRangeFactor2D(X(k), L(lm_id), \n", - " robot_pose.bearing(lm_gt_pt),\n", - " robot_pose.range(lm_gt_pt), \n", - " MEASUREMENT_NOISE)\n", - " true_range = measurement_factor.measured().range()\n", - " true_bearing = measurement_factor.measured().bearing()\n", - "\n", - " if true_range <= MAX_SENSOR_RANGE and abs(true_bearing.theta()) 0: # Ensure range is positive\n", - " measurements_sim[k].append((L(lm_id), noisy_bearing, noisy_range))\n", - " except Exception as e:\n", - " # Catch potential errors like point being too close to the pose\n", - " print(f\"Sim Warning at step {k}, landmark {lm_id}: {e}\")\n", - " pass\n", - "\n", - "print(f\"Generated {NUM_LANDMARKS} landmarks.\")\n", - "print(f\"Generated {NUM_STEPS} ground truth poses and odometry measurements.\")\n", - "num_meas_total = sum(len(m_list) for m_list in measurements_sim)\n", - "print(f\"Generated {num_meas_total} bearing-range measurements across all steps.\")" + "landmarks_gt_dict, poses_gt, odometry_measurements, measurements_sim, landmarks_gt_array = \\\n", + " simulation.generate_simulation_data(\n", + " num_landmarks=NUM_LANDMARKS,\n", + " world_size=WORLD_SIZE,\n", + " robot_radius=ROBOT_RADIUS,\n", + " robot_angular_vel=ROBOT_ANGULAR_VEL,\n", + " num_steps=NUM_STEPS,\n", + " dt=DT,\n", + " odometry_noise_model=ODOMETRY_NOISE,\n", + " measurement_noise_model=MEASUREMENT_NOISE,\n", + " max_sensor_range=MAX_SENSOR_RANGE,\n", + " X=X, # Pass symbol functions\n", + " L=L\n", + " )" ] }, { @@ -241,7 +205,25 @@ "source": [ "## 4. Iterative Factor Graph SLAM Implementation\n", "\n", - "Here we perform the step-by-step prediction and update using factor graphs." + "Here we perform the step-by-step prediction and update using factor graphs. The evolving graph will be displayed." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "90bf9320", + "metadata": {}, + "outputs": [], + "source": [ + "writer = gtsam.GraphvizFormatting()\n", + "writer.binaryEdges = True\n", + "\n", + "def maybe_show(graph, estimate, message):\n", + " if SHOW_GRAPHVIZ_EACH_STEP:\n", + " print(message)\n", + " graph_dot = graph.dot(estimate, writer=writer)\n", + " display(graphviz.Source(graph_dot, engine='neato'))\n", + " time.sleep(GRAPHVIZ_PAUSE_SECONDS)" ] }, { @@ -258,11 +240,15 @@ "\n", "# Add prior on the initial pose X(0)\n", "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", - "current_estimate.insert(current_pose_key, initial_pose)\n", + "# Add a bit of noise to the initial guess if desired:\n", + "# initial_pose_noisy = initial_pose.compose(gtsam.Pose2(0.1, 0.1, 0.01))\n", + "# current_estimate.insert(current_pose_key, initial_pose_noisy)\n", + "current_estimate.insert(current_pose_key, initial_pose) \n", + "\n", "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", "\n", "# Variables to store results for animation\n", - "results_history = [current_estimate] # Store Values object at each step\n", + "results_history = [gtsam.Values(current_estimate)] # Store Values object at each step\n", "marginals_history = [] # Store Marginals object at each step\n", "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", "\n", @@ -273,7 +259,9 @@ " marginals_history.append(initial_marginals)\n", "except Exception as e:\n", " print(f\"Error computing initial marginals: {e}\")\n", - " marginals_history.append(None) # Append placeholder if fails\n" + " marginals_history.append(None) # Append placeholder if fails\n", + "\n", + "maybe_show(current_graph, current_estimate, \"--- Step 0 --- Initial Graph with Prior ---\")" ] }, { @@ -300,18 +288,15 @@ " odom_k = odometry_measurements[k]\n", " \n", " # Predict next pose and add to Values (linearization point)\n", + " # Ensure previous pose exists before accessing\n", " predicted_pose = current_estimate.atPose2(prev_pose_key).compose(odom_k)\n", - " current_estimate.insert(current_pose_key, predicted_pose)\n", + " \n", + " # Insert or update the current pose prediction\n", + " current_estimate.insert_or_assign(current_pose_key, predicted_pose)\n", " \n", " # Add odometry factor to the graph\n", " current_graph.add(gtsam.BetweenFactorPose2(prev_pose_key, current_pose_key, odom_k, ODOMETRY_NOISE))\n", "\n", - " # --- At this point, the full graph contains the history --- \n", - " # --- To mimic SRIF/EKF, we *could* eliminate prev_pose_key, but --- \n", - " # --- it's simpler here to keep the full graph and optimize it --- \n", - " # --- then extract the current belief for the next step's factors --- \n", - " # --- Let's proceed with adding measurements first ----\n", - "\n", " # === Update Step ===\n", " measurements_k1 = measurements_sim[k + 1] # Measurements taken AT pose k+1\n", " landmarks_observed_this_step = set()\n", @@ -329,42 +314,29 @@ " lm_initial_guess = current_pose_val.transformFrom(gtsam.Point2(delta_x, delta_y))\n", " current_estimate.insert(lm_key, lm_initial_guess)\n", " known_landmarks.add(lm_key)\n", - " # print(f\"Step {k+1}: Initialized Landmark {lm_key.index()}\")\n", "\n", " # Add measurement factor\n", " current_graph.add(gtsam.BearingRangeFactor2D(current_pose_key, lm_key, \n", - " measured_bearing, measured_range,\n", - " MEASUREMENT_NOISE))\n", + " measured_bearing, measured_range,\n", + " MEASUREMENT_NOISE))\n", " \n", " # --- Optimization (Incremental Smoothing) ---\n", - " # In a true filter, we'd eliminate previous states.\n", - " # Here, we re-optimize the growing graph. For efficiency in large problems, \n", - " # iSAM2 would be used, but Levenberg-Marquardt on the growing graph \n", - " # demonstrates the concept for this smaller example.\n", - " \n", " optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n", - " try:\n", - " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", - " except Exception as e:\n", - " print(f\"Optimization failed at step {k+1}: {e}\")\n", - " # Potentially revert estimate update or handle error\n", - " # For simplicity, we continue with the potentially unoptimized estimate\n", - " pass \n", + " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", + " \n", + " # --- Display Factor Graph --- \n", + " maybe_show(current_graph, current_estimate, f\"--- Step {k+1} --- Factor Graph ---\")\n", "\n", " # Store results for animation\n", " results_history.append(gtsam.Values(current_estimate)) # Store a copy\n", " \n", " # Calculate marginals for visualization (can be slow for large graphs)\n", - " try:\n", - " current_gaussian_graph = current_graph.linearize(current_estimate)\n", - " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", - " marginals_history.append(current_marginals)\n", - " except Exception as e:\n", - " print(f\"Marginals calculation failed at step {k+1}: {e}\")\n", - " marginals_history.append(None) # Append placeholder\n", + " current_gaussian_graph = current_graph.linearize(current_estimate)\n", + " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", + " marginals_history.append(current_marginals)\n", "\n", - "print(\"Iterative Factor Graph SLAM finished.\")\n", - "print(f\"Final number of poses: {k+2}\")\n", + "print(\"\\nIterative Factor Graph SLAM finished.\")\n", + "print(f\"Final number of poses estimated: {len([key for key in current_estimate.keys() if gtsam.Symbol(key).chr() == ord('x')])}\")\n", "print(f\"Number of landmarks mapped: {len(known_landmarks)}\")" ] }, @@ -373,208 +345,30 @@ "id": "plotly-animation", "metadata": {}, "source": [ - "## 5. Create Plotly Animation" + "## 5. Create Plotly Animation\n", + "\n", + "Use the `gtsam_plotly` module to visualize the results." ] }, { "cell_type": "code", "execution_count": null, - "id": "plotly-animation-code", + "id": "plotly-animation-call-code", "metadata": { "scrolled": false, - "tags": [ - "remove-input" - ] + "tags": [] }, "outputs": [], "source": [ - "print(\"Generating Plotly animation...\")\n", - "\n", - "def ellipse_path(cx, cy, sizex, sizey, angle, N=60):\n", - " \"\"\"SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.\"\"\"\n", - " angle_rad = np.radians(angle)\n", - " t = np.linspace(0, 2 * np.pi, N)\n", - " x = (sizex / 2) * np.cos(t)\n", - " y = (sizey / 2) * np.sin(t)\n", - "\n", - " x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad)\n", - " y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad)\n", - "\n", - " path = f\"M {x_rot[0]},{y_rot[0]} \" + \" \".join(\n", - " f\"L{x_},{y_}\" for x_, y_ in zip(x_rot[1:], y_rot[1:])\n", - " ) + \" Z\"\n", - " return path\n", - "\n", - "# Helper to convert GTSAM covariance to Plotly ellipse parameters\n", - "def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0):\n", - " \"\"\"Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.\"\"\"\n", - " # Ensure positive definite - add small epsilon if needed\n", - " cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 \n", - " try:\n", - " eigvals, eigvecs = np.linalg.eigh(cov)\n", - " # Ensure eigenvalues are positive for sqrt\n", - " eigvals = np.maximum(eigvals, 1e-9)\n", - " except np.linalg.LinAlgError:\n", - " print(\"Warning: Covariance matrix SVD failed, using default ellipse.\")\n", - " return 0, 0.1*scale, 0.1*scale # Default small ellipse\n", - " \n", - " # Width/Height are 2*scale*sqrt(eigenvalue)\n", - " width = 2 * scale * np.sqrt(eigvals[1])\n", - " height = 2 * scale * np.sqrt(eigvals[0])\n", - " \n", - " # Angle of the major axis (corresponding to largest eigenvalue)\n", - " angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1])\n", - " angle_deg = np.degrees(angle_rad)\n", - " \n", - " return angle_deg, width, height\n", - "\n", - "# --- Create Plotly Figure --- \n", - "fig = go.Figure()\n", - "\n", - "# Add Ground Truth Landmarks (static)\n", - "fig.add_trace(go.Scatter(\n", - " x=landmarks_gt[0, :], \n", - " y=landmarks_gt[1, :], \n", - " mode='markers', \n", - " marker=dict(color='black', size=8, symbol='star'),\n", - " name='Landmarks GT'\n", - "))\n", - "\n", - "# Add Ground Truth Path (static)\n", - "gt_path_x = [p.x() for p in poses_gt]\n", - "gt_path_y = [p.y() for p in poses_gt]\n", - "fig.add_trace(go.Scatter(\n", - " x=gt_path_x,\n", - " y=gt_path_y,\n", - " mode='lines',\n", - " line=dict(color='gray', width=1, dash='dash'),\n", - " name='Path GT'\n", - "))\n", - "\n", - "# --- Animation Frames --- \n", - "frames = []\n", - "steps = list(range(NUM_STEPS + 1))\n", - "\n", - "for k in tqdm(steps):\n", - " frame_data = []\n", - " step_results = results_history[k]\n", - " step_marginals = marginals_history[k]\n", - " \n", - " # Estimated Path up to step k\n", - " est_path_x = [results_history[i].atPose2(X(i)).x() for i in range(k + 1)]\n", - " est_path_y = [results_history[i].atPose2(X(i)).y() for i in range(k + 1)]\n", - " frame_data.append(go.Scatter(\n", - " x=est_path_x,\n", - " y=est_path_y,\n", - " mode='lines+markers',\n", - " line=dict(color='red', width=2),\n", - " marker=dict(size=4, color='red'),\n", - " name='Path Est'\n", - " ))\n", - " \n", - " # Estimated Landmarks known at step k\n", - " est_landmarks_x = []\n", - " est_landmarks_y = []\n", - " landmark_keys_in_frame = []\n", - " for lm_key in step_results.keys():\n", - " if gtsam.symbolChr(lm_key)==108: # Check if it's a landmark key\n", - " lm_pose = step_results.atPoint2(lm_key)\n", - " est_landmarks_x.append(lm_pose[0])\n", - " est_landmarks_y.append(lm_pose[1])\n", - " landmark_keys_in_frame.append(lm_key)\n", - " \n", - " if est_landmarks_x:\n", - " frame_data.append(go.Scatter(\n", - " x=est_landmarks_x, \n", - " y=est_landmarks_y, \n", - " mode='markers',\n", - " marker=dict(color='blue', size=6, symbol='x'),\n", - " name='Landmarks Est'\n", - " ))\n", - "\n", - " # Current Pose Covariance Ellipse\n", - " shapes = [] # List to hold ellipse shapes for this frame\n", - " if step_marginals is not None:\n", - " try:\n", - " current_pose_key = X(k)\n", - " pose_cov = step_marginals.marginalCovariance(current_pose_key)\n", - " pose_mean = step_results.atPose2(current_pose_key).translation()\n", - " angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov)\n", - " cx, cy = pose_mean[0], pose_mean[1]\n", - " shapes.append(dict(\n", - " type=\"path\",\n", - " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", - " xref=\"x\", yref=\"y\",\n", - " fillcolor=\"rgba(255,0,255,0.2)\",\n", - " line_color=\"rgba(255,0,255,0.5)\",\n", - " name=f'Pose {k} Cov'\n", - " ))\n", - " except Exception as e:\n", - " print(f\"Warning: Failed getting pose cov ellipse at step {k}: {e}\")\n", - "\n", - " # Landmark Covariance Ellipses\n", - " for lm_key in landmark_keys_in_frame:\n", - " index = gtsam.symbolIndex(lm_key)\n", - " try:\n", - " lm_cov = step_marginals.marginalCovariance(lm_key)\n", - " lm_mean = step_results.atPoint2(lm_key)\n", - " angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov)\n", - " cx, cy = lm_mean[0], lm_mean[1]\n", - " shapes.append(dict(\n", - " type=\"path\",\n", - " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", - " xref=\"x\", yref=\"y\",\n", - " fillcolor=\"rgba(0,0,255,0.1)\",\n", - " line_color=\"rgba(0,0,255,0.3)\",\n", - " name=f'LM {index} Cov'\n", - " ))\n", - " except Exception as e:\n", - " print(f\"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}\")\n", - "\n", - " frames.append(go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)))\n", - "\n", - "# --- Set Initial State and Layout --- \n", - "fig.update(frames=frames)\n", - "\n", - "# Set initial data to the first frame's data\n", - "if frames:\n", - " fig.add_traces(frames[0].data)\n", - " initial_shapes = frames[0].layout.shapes if frames[0].layout else []\n", - "else:\n", - " initial_shapes = []\n", - "\n", - "# Define slider\n", - "sliders = [dict(\n", - " active=0,\n", - " currentvalue={\"prefix\": \"Step: \"},\n", - " pad={\"t\": 50},\n", - " steps=[dict(label=str(k), \n", - " method='animate', \n", - " args=[[str(k)], \n", - " dict(mode='immediate', \n", - " frame=dict(duration=100, redraw=True), \n", - " transition=dict(duration=0))])\n", - " for k in steps]\n", - ")]\n", - "\n", - "# Update layout\n", - "fig.update_layout(\n", - " title='Iterative Factor Graph SLAM Animation',\n", - " xaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], constrain='domain'),\n", - " yaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], scaleanchor='x', scaleratio=1),\n", - " width=800, height=800,\n", - " hovermode='closest',\n", - " updatemenus=[dict(type='buttons', \n", - " showactive=False,\n", - " buttons=[dict(label='Play',\n", - " method='animate', \n", - " args=[None, \n", - " dict(mode='immediate', \n", - " frame=dict(duration=100, redraw=True), \n", - " transition=dict(duration=0), \n", - " fromcurrent=True)])])],\n", - " sliders=sliders,\n", - " shapes=initial_shapes # Set initial shapes\n", + "fig = gtsam_plotly.create_slam_animation(\n", + " results_history=results_history,\n", + " marginals_history=marginals_history,\n", + " landmarks_gt_array=landmarks_gt_array,\n", + " poses_gt=poses_gt,\n", + " num_steps=NUM_STEPS,\n", + " world_size=WORLD_SIZE,\n", + " X=X, # Pass symbol functions\n", + " L=L\n", ")\n", "\n", "print(\"Displaying animation...\")\n", @@ -589,6 +383,8 @@ "## 6. Discussion\n", "\n", "* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n", + "* **Modularity:** Simulation and Plotly visualization code have been moved into separate `simulation.py` and `gtsam_plotly.py` files for better organization.\n", + "* **Graph Visualization:** The `graphviz` library was used to render the factor graph at each step (or only at the end, depending on `SHOW_GRAPHVIZ_EACH_STEP`). This helps visualize how the graph structure grows and connects poses and landmarks.\n", "* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n", "* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n", "* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)." @@ -597,7 +393,7 @@ ], "metadata": { "kernelspec": { - "display_name": "Python 3 (ipykernel)", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -611,7 +407,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.12" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py new file mode 100644 index 000000000..398156263 --- /dev/null +++ b/python/gtsam/examples/gtsam_plotly.py @@ -0,0 +1,290 @@ +# gtsam_plotly.py +import numpy as np +import plotly.graph_objects as go +from tqdm.notebook import tqdm # Progress bar + +import gtsam + + +def ellipse_path(cx, cy, sizex, sizey, angle, N=60): + """SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.""" + angle_rad = np.radians(angle) + t = np.linspace(0, 2 * np.pi, N) + x = (sizex / 2) * np.cos(t) + y = (sizey / 2) * np.sin(t) + + x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad) + y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad) + + path = ( + f"M {x_rot[0]},{y_rot[0]} " + + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_rot[1:], y_rot[1:])) + + " Z" + ) + return path + + +# Helper to convert GTSAM covariance to Plotly ellipse parameters +def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0): + """Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.""" + # Ensure positive definite - add small epsilon if needed + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 + try: + eigvals, eigvecs = np.linalg.eigh(cov) + # Ensure eigenvalues are positive for sqrt + eigvals = np.maximum(eigvals, 1e-9) + except np.linalg.LinAlgError: + # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Can be verbose + return 0, 0.1 * scale, 0.1 * scale # Default small ellipse + + # Width/Height are 2*scale*sqrt(eigenvalue) + width = 2 * scale * np.sqrt(eigvals[1]) + height = 2 * scale * np.sqrt(eigvals[0]) + + # Angle of the major axis (corresponding to largest eigenvalue) + angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1]) + angle_deg = np.degrees(angle_rad) + + return angle_deg, width, height + + +def create_slam_animation( + results_history, # List of gtsam.Values + marginals_history, # List of gtsam.Marginals or None + landmarks_gt_array, # Nx2 numpy array + poses_gt, # List of gtsam.Pose2 + num_steps, + world_size, + X, # Symbol func + L, # Symbol func +): + """Creates a Plotly animation of the SLAM results.""" + print("Generating Plotly animation...") + + fig = go.Figure() + + # Add Ground Truth Landmarks (static) + fig.add_trace( + go.Scatter( + x=landmarks_gt_array[0, :], + y=landmarks_gt_array[1, :], + mode="markers", + marker=dict(color="black", size=8, symbol="star"), + name="Landmarks GT", + ) + ) + + # Add Ground Truth Path (static) + gt_path_x = [p.x() for p in poses_gt] + gt_path_y = [p.y() for p in poses_gt] + fig.add_trace( + go.Scatter( + x=gt_path_x, + y=gt_path_y, + mode="lines", + line=dict(color="gray", width=1, dash="dash"), + name="Path GT", + ) + ) + + # --- Animation Frames --- + frames = [] + steps = list(range(num_steps + 1)) + + for k in tqdm(steps): + frame_data = [] + step_results = results_history[k] + step_marginals = marginals_history[k] + + # Estimated Path up to step k + est_path_x = [] + est_path_y = [] + for i in range(k + 1): + pose_key = X(i) + if step_results.exists( + pose_key + ): # Check if pose exists in results for this step + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + + frame_data.append( + go.Scatter( + x=est_path_x, + y=est_path_y, + mode="lines+markers", + line=dict(color="red", width=2), + marker=dict(size=4, color="red"), + name="Path Est", + ) + ) + + # Estimated Landmarks known at step k + est_landmarks_x = [] + est_landmarks_y = [] + landmark_keys_in_frame = [] + # Iterate through all possible landmark keys seen so far, check if present in current step's results + all_keys = step_results.keys() + for lm_key_val in all_keys: # Use keys() for integer keys + if gtsam.Symbol(lm_key_val).chr() == ord("l"): # Check if symbol chr is 'l' + lm_key = lm_key_val # Use the integer key directly + if step_results.exists(lm_key): + lm_pose = step_results.atPoint2(lm_key) + est_landmarks_x.append(lm_pose[0]) + est_landmarks_y.append(lm_pose[1]) + landmark_keys_in_frame.append(lm_key) + + if est_landmarks_x: + frame_data.append( + go.Scatter( + x=est_landmarks_x, + y=est_landmarks_y, + mode="markers", + marker=dict(color="blue", size=6, symbol="x"), + name="Landmarks Est", + ) + ) + + # Covariance Ellipses + shapes = [] # List to hold ellipse shapes for this frame + if step_marginals is not None: + # Current Pose Covariance Ellipse + try: + current_pose_key = X(k) + if step_results.exists(current_pose_key): + pose_cov = step_marginals.marginalCovariance(current_pose_key) + pose_mean = step_results.atPose2(current_pose_key).translation() + angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov) + cx, cy = pose_mean[0], pose_mean[1] + shapes.append( + dict( + type="path", + path=ellipse_path( + cx=cx, + cy=cy, + sizex=width, + sizey=height, + angle=angle, + N=60, + ), + xref="x", + yref="y", + fillcolor="rgba(255,0,255,0.2)", + line_color="rgba(255,0,255,0.5)", + name=f"Pose {k} Cov", + ) + ) + except Exception as e: + print( + f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" + ) # Can be verbose + pass + + # Landmark Covariance Ellipses + for lm_key in landmark_keys_in_frame: + try: + lm_cov = step_marginals.marginalCovariance(lm_key) + lm_mean = step_results.atPoint2(lm_key) + angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov) + cx, cy = lm_mean[0], lm_mean[1] + index = gtsam.Symbol(lm_key).index() + shapes.append( + dict( + type="path", + path=ellipse_path( + cx=cx, + cy=cy, + sizex=width, + sizey=height, + angle=angle, + N=60, + ), + xref="x", + yref="y", + fillcolor="rgba(0,0,255,0.1)", + line_color="rgba(0,0,255,0.3)", + name=f"LM {index} Cov", + ) + ) + except Exception as e: + index = gtsam.Symbol(lm_key).index() + print( + f"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}" + ) # Can be verbose + + frames.append( + go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)) + ) + + # --- Set Initial State and Layout --- + fig.update(frames=frames) + + # Set initial data to the first frame's data + if frames: + fig.add_traces(frames[0].data) + initial_shapes = frames[0].layout.shapes if frames[0].layout else [] + else: + initial_shapes = [] + + # Define slider + sliders = [ + dict( + active=0, + currentvalue={"prefix": "Step: "}, + pad={"t": 50}, + steps=[ + dict( + label=str(k), + method="animate", + args=[ + [str(k)], + dict( + mode="immediate", + frame=dict(duration=100, redraw=True), + transition=dict(duration=0), + ), + ], + ) + for k in steps + ], + ) + ] + + # Update layout + fig.update_layout( + title="Iterative Factor Graph SLAM Animation", + xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), + yaxis=dict( + range=[-world_size / 2 - 2, world_size / 2 + 2], + scaleanchor="x", + scaleratio=1, + ), + width=800, + height=800, + hovermode="closest", + updatemenus=[ + dict( + type="buttons", + showactive=False, + buttons=[ + dict( + label="Play", + method="animate", + args=[ + None, + dict( + mode="immediate", + frame=dict(duration=100, redraw=True), + transition=dict(duration=0), + fromcurrent=True, + ), + ], + ) + ], + ) + ], + sliders=sliders, + shapes=initial_shapes, # Set initial shapes + ) + print("Plotly animation generated.") + return fig diff --git a/python/gtsam/examples/simulation.py b/python/gtsam/examples/simulation.py new file mode 100644 index 000000000..a36433878 --- /dev/null +++ b/python/gtsam/examples/simulation.py @@ -0,0 +1,137 @@ +# simulation.py +import numpy as np + +import gtsam + + +def generate_simulation_data( + num_landmarks, + world_size, + robot_radius, + robot_angular_vel, + num_steps, + dt, + odometry_noise_model, + measurement_noise_model, + max_sensor_range, + X, # Symbol generator function + L, # Symbol generator function + odom_seed=42, + meas_seed=42, + landmark_seed=42, +): + """Generates ground truth and simulated measurements for SLAM. + + Args: + num_landmarks: Number of landmarks to generate. + world_size: Size of the square world environment. + robot_radius: Radius of the robot's circular path. + robot_angular_vel: Angular velocity of the robot (rad/step). + num_steps: Number of simulation steps. + dt: Time step duration. + odometry_noise_model: GTSAM noise model for odometry. + measurement_noise_model: GTSAM noise model for bearing-range. + max_sensor_range: Maximum range of the bearing-range sensor. + X: GTSAM symbol shorthand function for poses. + L: GTSAM symbol shorthand function for landmarks. + odom_seed: Random seed for odometry noise. + meas_seed: Random seed for measurement noise. + landmark_seed: Random seed for landmark placement. + + Returns: + tuple: Contains: + - landmarks_gt_dict (dict): L(i) -> gtsam.Point2 ground truth. + - poses_gt (list): List of gtsam.Pose2 ground truth poses. + - odometry_measurements (list): List of noisy gtsam.Pose2 odometry. + - measurements_sim (list): List of lists, measurements_sim[k] contains + tuples (L(lm_id), bearing, range) for step k. + - landmarks_gt_array (np.array): 2xN numpy array of landmark positions. + """ + np.random.seed(landmark_seed) + odometry_noise_sampler = gtsam.Sampler(odometry_noise_model, odom_seed) + measurement_noise_sampler = gtsam.Sampler(measurement_noise_model, meas_seed) + + # 1. Ground Truth Landmarks + landmarks_gt_array = (np.random.rand(2, num_landmarks) - 0.5) * world_size + landmarks_gt_dict = { + L(i): gtsam.Point2(landmarks_gt_array[:, i]) for i in range(num_landmarks) + } + + # 2. Ground Truth Robot Path + poses_gt = [] + current_pose_gt = gtsam.Pose2(robot_radius, 0, np.pi / 2) # Start on circle edge + poses_gt.append(current_pose_gt) + + for _ in range(num_steps): + delta_theta = robot_angular_vel * dt + arc_length = robot_angular_vel * robot_radius * dt + motion_command = gtsam.Pose2(arc_length, 0, delta_theta) + current_pose_gt = current_pose_gt.compose(motion_command) + poses_gt.append(current_pose_gt) + + # 3. Simulate Noisy Odometry Measurements + odometry_measurements = [] + for k in range(num_steps): + pose_k = poses_gt[k] + pose_k1 = poses_gt[k + 1] + true_odom = pose_k.between(pose_k1) + + # Sample noise directly for Pose2 composition (approximate) + odom_noise_vec = odometry_noise_sampler.sample() + noisy_odom = true_odom.compose( + gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]) + ) + odometry_measurements.append(noisy_odom) + + # 4. Simulate Noisy Bearing-Range Measurements + measurements_sim = [[] for _ in range(num_steps + 1)] + for k in range(num_steps + 1): + robot_pose = poses_gt[k] + for lm_id in range(num_landmarks): + lm_gt_pt = landmarks_gt_dict[L(lm_id)] + try: + measurement_factor = gtsam.BearingRangeFactor2D( + X(k), + L(lm_id), + robot_pose.bearing(lm_gt_pt), + robot_pose.range(lm_gt_pt), + measurement_noise_model, + ) + true_range = measurement_factor.measured().range() + true_bearing = measurement_factor.measured().bearing() + + # Check sensor limits (range and Field of View - e.g. +/- 45 degrees) + if ( + true_range <= max_sensor_range + and abs(true_bearing.theta()) < np.pi / 4 + ): + # Sample noise + noise_vec = measurement_noise_sampler.sample() + noisy_bearing = true_bearing.retract( + np.array([noise_vec[0]]) + ) # Retract on SO(2) + noisy_range = true_range + noise_vec[1] + + if noisy_range > 0: # Ensure range is positive + measurements_sim[k].append( + (L(lm_id), noisy_bearing, noisy_range) + ) + except Exception as e: + # Catch potential errors like point being too close to the pose + # print(f"Sim Warning at step {k}, landmark {lm_id}: {e}") # Can be verbose + pass + + print(f"Simulation Generated: {num_landmarks} landmarks.") + print( + f"Simulation Generated: {num_steps + 1} ground truth poses and {num_steps} odometry measurements." + ) + num_meas_total = sum(len(m_list) for m_list in measurements_sim) + print(f"Simulation Generated: {num_meas_total} bearing-range measurements.") + + return ( + landmarks_gt_dict, + poses_gt, + odometry_measurements, + measurements_sim, + landmarks_gt_array, + ) From 8148b78af621e2cda699cb2d8e26c27031993b33 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 15:51:56 -0400 Subject: [PATCH 19/38] More modular --- python/gtsam/examples/gtsam_plotly.py | 593 +++++++++++++++++--------- 1 file changed, 385 insertions(+), 208 deletions(-) diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index 398156263..5cf4f4462 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -1,13 +1,31 @@ -# gtsam_plotly.py +# gtsam_plotly_modular.py import numpy as np import plotly.graph_objects as go from tqdm.notebook import tqdm # Progress bar +from typing import List, Optional, Tuple, Dict, Any import gtsam +# --- Ellipse Calculation Helpers (Mostly unchanged) --- -def ellipse_path(cx, cy, sizex, sizey, angle, N=60): - """SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.""" + +def ellipse_path( + cx: float, cy: float, sizex: float, sizey: float, angle: float, N: int = 60 +) -> str: + """ + Generates an SVG path string for an ellipse. + + Args: + cx: Center x-coordinate. + cy: Center y-coordinate. + sizex: Full width of the ellipse along its major axis. + sizey: Full height of the ellipse along its minor axis. + angle: Rotation angle in degrees. + N: Number of points to approximate the ellipse. + + Returns: + SVG path string. + """ angle_rad = np.radians(angle) t = np.linspace(0, 2 * np.pi, N) x = (sizex / 2) * np.cos(t) @@ -24,9 +42,19 @@ def ellipse_path(cx, cy, sizex, sizey, angle, N=60): return path -# Helper to convert GTSAM covariance to Plotly ellipse parameters -def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0): - """Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.""" +def gtsam_cov_to_plotly_ellipse( + cov_matrix: np.ndarray, scale: float = 2.0 +) -> Tuple[float, float, float]: + """ + Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix. + + Args: + cov_matrix: The 2x2 covariance matrix (or larger, only top-left 2x2 used). + scale: Scaling factor for the ellipse size (e.g., 2.0 for 2-sigma). + + Returns: + Tuple containing (angle_degrees, width, height). + """ # Ensure positive definite - add small epsilon if needed cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 try: @@ -34,199 +62,225 @@ def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0): # Ensure eigenvalues are positive for sqrt eigvals = np.maximum(eigvals, 1e-9) except np.linalg.LinAlgError: - # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Can be verbose + # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Optional warning return 0, 0.1 * scale, 0.1 * scale # Default small ellipse - # Width/Height are 2*scale*sqrt(eigenvalue) - width = 2 * scale * np.sqrt(eigvals[1]) - height = 2 * scale * np.sqrt(eigvals[0]) + # Width/Height are 2*scale*sqrt(eigenvalue) (using full width/height) + width = ( + 2 * scale * np.sqrt(eigvals[1]) + ) # Major axis corresponds to largest eigenvalue + height = ( + 2 * scale * np.sqrt(eigvals[0]) + ) # Minor axis corresponds to smallest eigenvalue - # Angle of the major axis (corresponding to largest eigenvalue) + # Angle of the major axis (eigenvector corresponding to largest eigenvalue eigvals[1]) angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1]) angle_deg = np.degrees(angle_rad) return angle_deg, width, height -def create_slam_animation( - results_history, # List of gtsam.Values - marginals_history, # List of gtsam.Marginals or None - landmarks_gt_array, # Nx2 numpy array - poses_gt, # List of gtsam.Pose2 - num_steps, - world_size, - X, # Symbol func - L, # Symbol func -): - """Creates a Plotly animation of the SLAM results.""" - print("Generating Plotly animation...") +# --- Plotting Element Creation Helpers --- - fig = go.Figure() - # Add Ground Truth Landmarks (static) - fig.add_trace( +def _add_ground_truth_traces( + fig: go.Figure, landmarks_gt_array: np.ndarray, poses_gt: List[gtsam.Pose2] +) -> None: + """Adds static ground truth landmark and path traces to the figure.""" + # Ground Truth Landmarks + if landmarks_gt_array is not None and landmarks_gt_array.size > 0: + fig.add_trace( + go.Scatter( + x=landmarks_gt_array[0, :], + y=landmarks_gt_array[1, :], + mode="markers", + marker=dict(color="black", size=8, symbol="star"), + name="Landmarks GT", + ) + ) + + # Ground Truth Path + if poses_gt: + gt_path_x = [p.x() for p in poses_gt] + gt_path_y = [p.y() for p in poses_gt] + fig.add_trace( + go.Scatter( + x=gt_path_x, + y=gt_path_y, + mode="lines", + line=dict(color="gray", width=1, dash="dash"), + name="Path GT", + ) + ) + + +def _create_ellipse_shape_dict( + cx: float, + cy: float, + angle: float, + width: float, + height: float, + fillcolor: str, + line_color: str, + name: str, +) -> Dict[str, Any]: + """Creates the dictionary required for a Plotly ellipse shape.""" + return dict( + type="path", + path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60), + xref="x", + yref="y", + fillcolor=fillcolor, + line_color=line_color, + name=name, # Note: name isn't directly displayed for shapes, but good for metadata + ) + + +def _create_single_frame_data( + k: int, + step_results: gtsam.Values, + step_marginals: Optional[gtsam.Marginals], + X: callable, + L: callable, + ellipse_scale: float = 2.0, + verbose: bool = False, +) -> Tuple[List[go.Scatter], List[Dict[str, Any]]]: + """ + Creates the traces and shapes for a single animation frame. + + Args: + k: The current step index. + step_results: gtsam.Values for this step. + step_marginals: gtsam.Marginals for this step (or None). + X: Symbol function for poses. + L: Symbol function for landmarks. + ellipse_scale: Scaling factor for covariance ellipses. + verbose: If True, print warnings for covariance errors. + + Returns: + A tuple containing (list_of_traces, list_of_shapes). + """ + traces = [] + shapes = [] + + # 1. Estimated Path up to step k + est_path_x = [] + est_path_y = [] + for i in range(k + 1): + pose_key = X(i) + if step_results.exists(pose_key): + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + + traces.append( go.Scatter( - x=landmarks_gt_array[0, :], - y=landmarks_gt_array[1, :], - mode="markers", - marker=dict(color="black", size=8, symbol="star"), - name="Landmarks GT", + x=est_path_x, + y=est_path_y, + mode="lines+markers", + line=dict(color="red", width=2), + marker=dict(size=4, color="red"), + name="Path Est", # Legend entry for the whole path ) ) - # Add Ground Truth Path (static) - gt_path_x = [p.x() for p in poses_gt] - gt_path_y = [p.y() for p in poses_gt] - fig.add_trace( - go.Scatter( - x=gt_path_x, - y=gt_path_y, - mode="lines", - line=dict(color="gray", width=1, dash="dash"), - name="Path GT", - ) - ) + # 2. Estimated Landmarks known at step k + est_landmarks_x = [] + est_landmarks_y = [] + landmark_keys_in_frame = [] + all_keys = step_results.keys() + for key_val in all_keys: + symbol = gtsam.Symbol(key_val) + if symbol.chr() == ord("l"): # Check if it's a landmark symbol + # Check existence again (though keys() implies existence) + if step_results.exists(key_val): + lm_point = step_results.atPoint2(key_val) + est_landmarks_x.append(lm_point[0]) + est_landmarks_y.append(lm_point[1]) + landmark_keys_in_frame.append(key_val) + + if est_landmarks_x: + traces.append( + go.Scatter( + x=est_landmarks_x, + y=est_landmarks_y, + mode="markers", + marker=dict(color="blue", size=6, symbol="x"), + name="Landmarks Est", # Legend entry for all estimated landmarks + ) + ) + + # 3. Covariance Ellipses (if marginals available) + if step_marginals is not None: + # Current Pose Covariance Ellipse + current_pose_key = X(k) + if step_results.exists(current_pose_key): + try: + pose_cov = step_marginals.marginalCovariance(current_pose_key) + pose_mean = step_results.atPose2(current_pose_key).translation() + angle, width, height = gtsam_cov_to_plotly_ellipse( + pose_cov, scale=ellipse_scale + ) + shapes.append( + _create_ellipse_shape_dict( + cx=pose_mean[0], + cy=pose_mean[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(255,0,255,0.2)", + line_color="rgba(255,0,255,0.5)", + name=f"Pose {k} Cov", + ) + ) + except Exception as e: + if verbose: + print( + f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" + ) + + # Landmark Covariance Ellipses + for lm_key in landmark_keys_in_frame: + try: + lm_cov = step_marginals.marginalCovariance(lm_key) + lm_mean = step_results.atPoint2(lm_key) + angle, width, height = gtsam_cov_to_plotly_ellipse( + lm_cov, scale=ellipse_scale + ) + symbol = gtsam.Symbol(lm_key) + shapes.append( + _create_ellipse_shape_dict( + cx=lm_mean[0], + cy=lm_mean[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(0,0,255,0.1)", + line_color="rgba(0,0,255,0.3)", + name=f"LM {symbol.index()} Cov", + ) + ) + except Exception as e: + symbol = gtsam.Symbol(lm_key) + if verbose: + print( + f"Warning: Failed getting landmark {symbol.index()} cov ellipse at step {k}: {e}" + ) + + return traces, shapes + + +def _configure_figure_layout( + fig: go.Figure, + num_steps: int, + world_size: float, + initial_shapes: List[Dict[str, Any]], +) -> None: + """Configures the Plotly figure's layout, axes, slider, and buttons.""" - # --- Animation Frames --- - frames = [] steps = list(range(num_steps + 1)) - for k in tqdm(steps): - frame_data = [] - step_results = results_history[k] - step_marginals = marginals_history[k] - - # Estimated Path up to step k - est_path_x = [] - est_path_y = [] - for i in range(k + 1): - pose_key = X(i) - if step_results.exists( - pose_key - ): # Check if pose exists in results for this step - pose = step_results.atPose2(pose_key) - est_path_x.append(pose.x()) - est_path_y.append(pose.y()) - - frame_data.append( - go.Scatter( - x=est_path_x, - y=est_path_y, - mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), - name="Path Est", - ) - ) - - # Estimated Landmarks known at step k - est_landmarks_x = [] - est_landmarks_y = [] - landmark_keys_in_frame = [] - # Iterate through all possible landmark keys seen so far, check if present in current step's results - all_keys = step_results.keys() - for lm_key_val in all_keys: # Use keys() for integer keys - if gtsam.Symbol(lm_key_val).chr() == ord("l"): # Check if symbol chr is 'l' - lm_key = lm_key_val # Use the integer key directly - if step_results.exists(lm_key): - lm_pose = step_results.atPoint2(lm_key) - est_landmarks_x.append(lm_pose[0]) - est_landmarks_y.append(lm_pose[1]) - landmark_keys_in_frame.append(lm_key) - - if est_landmarks_x: - frame_data.append( - go.Scatter( - x=est_landmarks_x, - y=est_landmarks_y, - mode="markers", - marker=dict(color="blue", size=6, symbol="x"), - name="Landmarks Est", - ) - ) - - # Covariance Ellipses - shapes = [] # List to hold ellipse shapes for this frame - if step_marginals is not None: - # Current Pose Covariance Ellipse - try: - current_pose_key = X(k) - if step_results.exists(current_pose_key): - pose_cov = step_marginals.marginalCovariance(current_pose_key) - pose_mean = step_results.atPose2(current_pose_key).translation() - angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov) - cx, cy = pose_mean[0], pose_mean[1] - shapes.append( - dict( - type="path", - path=ellipse_path( - cx=cx, - cy=cy, - sizex=width, - sizey=height, - angle=angle, - N=60, - ), - xref="x", - yref="y", - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", - name=f"Pose {k} Cov", - ) - ) - except Exception as e: - print( - f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" - ) # Can be verbose - pass - - # Landmark Covariance Ellipses - for lm_key in landmark_keys_in_frame: - try: - lm_cov = step_marginals.marginalCovariance(lm_key) - lm_mean = step_results.atPoint2(lm_key) - angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov) - cx, cy = lm_mean[0], lm_mean[1] - index = gtsam.Symbol(lm_key).index() - shapes.append( - dict( - type="path", - path=ellipse_path( - cx=cx, - cy=cy, - sizex=width, - sizey=height, - angle=angle, - N=60, - ), - xref="x", - yref="y", - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", - name=f"LM {index} Cov", - ) - ) - except Exception as e: - index = gtsam.Symbol(lm_key).index() - print( - f"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}" - ) # Can be verbose - - frames.append( - go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)) - ) - - # --- Set Initial State and Layout --- - fig.update(frames=frames) - - # Set initial data to the first frame's data - if frames: - fig.add_traces(frames[0].data) - initial_shapes = frames[0].layout.shapes if frames[0].layout else [] - else: - initial_shapes = [] - - # Define slider + # Slider sliders = [ dict( active=0, @@ -237,10 +291,12 @@ def create_slam_animation( label=str(k), method="animate", args=[ - [str(k)], + [str(k)], # Frame name dict( mode="immediate", - frame=dict(duration=100, redraw=True), + frame=dict( + duration=100, redraw=True + ), # Redraw needed for shapes transition=dict(duration=0), ), ], @@ -250,41 +306,162 @@ def create_slam_animation( ) ] - # Update layout + # Buttons + updatemenus = [ + dict( + type="buttons", + showactive=False, + buttons=[ + dict( + label="Play", + method="animate", + args=[ + None, # Animate all frames + dict( + mode="immediate", + frame=dict(duration=100, redraw=True), + transition=dict(duration=0), + fromcurrent=True, + ), + ], + ), + dict( + label="Pause", + method="animate", + args=[ + [None], # Stop animation + dict( + mode="immediate", + frame=dict(duration=0, redraw=False), + transition=dict(duration=0), + ), + ], + ), + ], + direction="left", + pad={"r": 10, "t": 87}, + x=0.1, + xanchor="right", + y=0, + yanchor="top", + ) + ] + + # Layout settings fig.update_layout( title="Iterative Factor Graph SLAM Animation", - xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), + xaxis=dict( + range=[-world_size / 2 - 2, world_size / 2 + 2], + constrain="domain", # Keep aspect ratio when zooming + ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - scaleanchor="x", + scaleanchor="x", # Ensure square aspect ratio scaleratio=1, ), width=800, height=800, hovermode="closest", - updatemenus=[ - dict( - type="buttons", - showactive=False, - buttons=[ - dict( - label="Play", - method="animate", - args=[ - None, - dict( - mode="immediate", - frame=dict(duration=100, redraw=True), - transition=dict(duration=0), - fromcurrent=True, - ), - ], - ) - ], - ) - ], + updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Set initial shapes + shapes=initial_shapes, # Set initial shapes from frame 0 + # Add legend if desired + legend=dict( + traceorder="reversed", + title_text="Legend", + orientation="h", + yanchor="bottom", + y=1.02, + xanchor="right", + x=1, + ), ) + + +# --- Main Animation Function (Orchestrator) --- + + +def create_slam_animation( + results_history: List[gtsam.Values], + marginals_history: List[Optional[gtsam.Marginals]], + num_steps: int, + X: callable, + L: callable, + landmarks_gt_array: Optional[np.ndarray] = None, + poses_gt: Optional[List[gtsam.Pose2]] = None, + world_size: float = 20.0, + ellipse_scale: float = 2.0, + verbose_cov_errors: bool = False, +) -> go.Figure: + """ + Creates a Plotly animation of the SLAM results in a modular way. + + Args: + results_history: List of gtsam.Values, one per step. + marginals_history: List of gtsam.Marginals or None, one per step. + num_steps: The total number of steps (results_history should have length num_steps + 1). + X: Symbol function for poses (e.g., lambda i: gtsam.symbol('x', i)). + L: Symbol function for landmarks (e.g., lambda j: gtsam.symbol('l', j)). + landmarks_gt_array: Optional Nx2 numpy array of ground truth landmark positions. + poses_gt: Optional list of gtsam.Pose2 ground truth poses. + world_size: Approximate size of the world for axis scaling. + ellipse_scale: Scaling factor for covariance ellipses (e.g., 2.0 for 2-sigma). + verbose_cov_errors: If True, print warnings for covariance calculation errors. + + Returns: + A plotly.graph_objects.Figure containing the animation. + """ + print("Generating Plotly animation...") + + fig = go.Figure() + + # 1. Add static ground truth elements + _add_ground_truth_traces(fig, landmarks_gt_array, poses_gt) + + # 2. Create frames for animation + frames = [] + steps_iterable = range(num_steps + 1) + + # Use tqdm for progress bar if available + try: + steps_iterable = tqdm(steps_iterable, desc="Creating Frames") + except NameError: + pass # tqdm not installed or not in notebook env + + for k in steps_iterable: + step_results = results_history[k] + step_marginals = marginals_history[k] if marginals_history else None + + # Create traces and shapes for this specific frame + frame_traces, frame_shapes = _create_single_frame_data( + k, step_results, step_marginals, X, L, ellipse_scale, verbose_cov_errors + ) + + # Create the Plotly frame object + frames.append( + go.Frame( + data=frame_traces, + name=str(k), # Name used by slider/buttons + layout=go.Layout( + shapes=frame_shapes + ), # Shapes are part of layout per frame + ) + ) + + # 3. Set initial figure state (using data from frame 0) + if frames: + # Add traces from the first frame as the initial state + for trace in frames[0].data: + fig.add_trace(trace) + initial_shapes = frames[0].layout.shapes if frames[0].layout else [] + else: + initial_shapes = [] + + # 4. Assign frames to the figure + fig.update(frames=frames) + + # 5. Configure overall layout, slider, buttons + _configure_figure_layout(fig, num_steps, world_size, initial_shapes) + print("Plotly animation generated.") return fig From 1bf76be62b5480ad901fa7022937635e06470244 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 15:58:33 -0400 Subject: [PATCH 20/38] Even more modular --- python/gtsam/examples/gtsam_plotly.py | 458 ++++++++++++-------------- 1 file changed, 202 insertions(+), 256 deletions(-) diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index 5cf4f4462..d37e32dc9 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -1,39 +1,25 @@ -# gtsam_plotly_modular.py +# gtsam_plotly_modular_v2.py +from typing import Any, Callable, Dict, List, Optional, Tuple + import numpy as np import plotly.graph_objects as go -from tqdm.notebook import tqdm # Progress bar -from typing import List, Optional, Tuple, Dict, Any +from tqdm.notebook import tqdm import gtsam -# --- Ellipse Calculation Helpers (Mostly unchanged) --- +# --- Core Ellipse Calculations --- def ellipse_path( cx: float, cy: float, sizex: float, sizey: float, angle: float, N: int = 60 ) -> str: - """ - Generates an SVG path string for an ellipse. - - Args: - cx: Center x-coordinate. - cy: Center y-coordinate. - sizex: Full width of the ellipse along its major axis. - sizey: Full height of the ellipse along its minor axis. - angle: Rotation angle in degrees. - N: Number of points to approximate the ellipse. - - Returns: - SVG path string. - """ + """Generates SVG path string for a rotated ellipse.""" angle_rad = np.radians(angle) t = np.linspace(0, 2 * np.pi, N) - x = (sizex / 2) * np.cos(t) - y = (sizey / 2) * np.sin(t) - - x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad) - y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad) - + x_unit = (sizex / 2) * np.cos(t) + y_unit = (sizey / 2) * np.sin(t) + x_rot = cx + x_unit * np.cos(angle_rad) - y_unit * np.sin(angle_rad) + y_rot = cy + x_unit * np.sin(angle_rad) + y_unit * np.cos(angle_rad) path = ( f"M {x_rot[0]},{y_rot[0]} " + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_rot[1:], y_rot[1:])) @@ -45,125 +31,150 @@ def ellipse_path( def gtsam_cov_to_plotly_ellipse( cov_matrix: np.ndarray, scale: float = 2.0 ) -> Tuple[float, float, float]: - """ - Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix. - - Args: - cov_matrix: The 2x2 covariance matrix (or larger, only top-left 2x2 used). - scale: Scaling factor for the ellipse size (e.g., 2.0 for 2-sigma). - - Returns: - Tuple containing (angle_degrees, width, height). - """ - # Ensure positive definite - add small epsilon if needed - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 + """Calculates ellipse angle (deg), width, height from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite try: eigvals, eigvecs = np.linalg.eigh(cov) - # Ensure eigenvalues are positive for sqrt - eigvals = np.maximum(eigvals, 1e-9) + eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues except np.linalg.LinAlgError: - # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Optional warning - return 0, 0.1 * scale, 0.1 * scale # Default small ellipse + return 0, 0.1 * scale, 0.1 * scale # Default on failure - # Width/Height are 2*scale*sqrt(eigenvalue) (using full width/height) - width = ( - 2 * scale * np.sqrt(eigvals[1]) - ) # Major axis corresponds to largest eigenvalue - height = ( - 2 * scale * np.sqrt(eigvals[0]) - ) # Minor axis corresponds to smallest eigenvalue - - # Angle of the major axis (eigenvector corresponding to largest eigenvalue eigvals[1]) - angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1]) + width = 2 * scale * np.sqrt(eigvals[1]) # Major axis (largest eigenvalue) + height = 2 * scale * np.sqrt(eigvals[0]) # Minor axis (smallest eigenvalue) + angle_rad = np.arctan2( + eigvecs[1, 1], eigvecs[0, 1] + ) # Angle of major axis eigenvector angle_deg = np.degrees(angle_rad) - return angle_deg, width, height -# --- Plotting Element Creation Helpers --- +# --- Plotly Element Generators --- -def _add_ground_truth_traces( - fig: go.Figure, landmarks_gt_array: np.ndarray, poses_gt: List[gtsam.Pose2] -) -> None: - """Adds static ground truth landmark and path traces to the figure.""" - # Ground Truth Landmarks - if landmarks_gt_array is not None and landmarks_gt_array.size > 0: - fig.add_trace( - go.Scatter( - x=landmarks_gt_array[0, :], - y=landmarks_gt_array[1, :], - mode="markers", - marker=dict(color="black", size=8, symbol="star"), - name="Landmarks GT", - ) - ) +def create_gt_landmarks_trace(landmarks_gt_array: np.ndarray) -> Optional[go.Scatter]: + """Creates scatter trace for ground truth landmarks.""" + if landmarks_gt_array is None or landmarks_gt_array.size == 0: + return None + return go.Scatter( + x=landmarks_gt_array[0, :], + y=landmarks_gt_array[1, :], + mode="markers", + marker=dict(color="black", size=8, symbol="star"), + name="Landmarks GT", + ) - # Ground Truth Path - if poses_gt: - gt_path_x = [p.x() for p in poses_gt] - gt_path_y = [p.y() for p in poses_gt] - fig.add_trace( - go.Scatter( - x=gt_path_x, - y=gt_path_y, - mode="lines", - line=dict(color="gray", width=1, dash="dash"), - name="Path GT", - ) - ) + +def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: + """Creates line trace for ground truth path.""" + if not poses_gt: + return None + gt_path_x = [p.x() for p in poses_gt] + gt_path_y = [p.y() for p in poses_gt] + return go.Scatter( + x=gt_path_x, + y=gt_path_y, + mode="lines", + line=dict(color="gray", width=1, dash="dash"), + name="Path GT", + ) + + +def create_est_path_trace( + est_path_x: List[float], est_path_y: List[float] +) -> go.Scatter: + """Creates scatter/line trace for the estimated path up to current step.""" + return go.Scatter( + x=est_path_x, + y=est_path_y, + mode="lines+markers", + line=dict(color="red", width=2), + marker=dict(size=4, color="red"), + name="Path Est", # This name applies to the trace in the specific frame + ) + + +def create_est_landmarks_trace( + est_landmarks_x: List[float], est_landmarks_y: List[float] +) -> Optional[go.Scatter]: + """Creates scatter trace for currently estimated landmarks.""" + if not est_landmarks_x: + return None + return go.Scatter( + x=est_landmarks_x, + y=est_landmarks_y, + mode="markers", + marker=dict(color="blue", size=6, symbol="x"), + name="Landmarks Est", # Applies to landmarks in the specific frame + ) def _create_ellipse_shape_dict( - cx: float, - cy: float, - angle: float, - width: float, - height: float, - fillcolor: str, - line_color: str, - name: str, + cx, cy, angle, width, height, fillcolor, line_color, name_suffix ) -> Dict[str, Any]: - """Creates the dictionary required for a Plotly ellipse shape.""" + """Helper to create the dictionary for a Plotly ellipse shape.""" return dict( type="path", - path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60), + path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle), xref="x", yref="y", fillcolor=fillcolor, line_color=line_color, - name=name, # Note: name isn't directly displayed for shapes, but good for metadata + # name=f"{name_suffix} Cov", # Name isn't really used by Plotly for shapes ) -def _create_single_frame_data( +def create_pose_ellipse_shape( + pose_mean_xy: np.ndarray, pose_cov: np.ndarray, k: int, scale: float +) -> Dict[str, Any]: + """Creates shape dictionary for a pose covariance ellipse.""" + angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov, scale) + return _create_ellipse_shape_dict( + cx=pose_mean_xy[0], + cy=pose_mean_xy[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(255,0,255,0.2)", + line_color="rgba(255,0,255,0.5)", + name_suffix=f"Pose {k}", + ) + + +def create_landmark_ellipse_shape( + lm_mean_xy: np.ndarray, lm_cov: np.ndarray, lm_index: int, scale: float +) -> Dict[str, Any]: + """Creates shape dictionary for a landmark covariance ellipse.""" + angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov, scale) + return _create_ellipse_shape_dict( + cx=lm_mean_xy[0], + cy=lm_mean_xy[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(0,0,255,0.1)", + line_color="rgba(0,0,255,0.3)", + name_suffix=f"LM {lm_index}", + ) + + +# --- Frame Content Generation --- + + +def generate_frame_content( k: int, step_results: gtsam.Values, step_marginals: Optional[gtsam.Marginals], - X: callable, - L: callable, + X: Callable[[int], int], + L: Callable[[int], int], + max_landmark_index: int, # Need to know the potential range of landmarks ellipse_scale: float = 2.0, verbose: bool = False, ) -> Tuple[List[go.Scatter], List[Dict[str, Any]]]: - """ - Creates the traces and shapes for a single animation frame. + """Generates all dynamic traces and shapes for a single animation frame `k`.""" + frame_traces: List[go.Scatter] = [] + frame_shapes: List[Dict[str, Any]] = [] - Args: - k: The current step index. - step_results: gtsam.Values for this step. - step_marginals: gtsam.Marginals for this step (or None). - X: Symbol function for poses. - L: Symbol function for landmarks. - ellipse_scale: Scaling factor for covariance ellipses. - verbose: If True, print warnings for covariance errors. - - Returns: - A tuple containing (list_of_traces, list_of_shapes). - """ - traces = [] - shapes = [] - - # 1. Estimated Path up to step k + # 1. Gather Estimated Path Data est_path_x = [] est_path_y = [] for i in range(k + 1): @@ -172,115 +183,70 @@ def _create_single_frame_data( pose = step_results.atPose2(pose_key) est_path_x.append(pose.x()) est_path_y.append(pose.y()) + frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) - traces.append( - go.Scatter( - x=est_path_x, - y=est_path_y, - mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), - name="Path Est", # Legend entry for the whole path - ) - ) - - # 2. Estimated Landmarks known at step k + # 2. Gather Estimated Landmark Data est_landmarks_x = [] est_landmarks_y = [] landmark_keys_in_frame = [] - all_keys = step_results.keys() - for key_val in all_keys: - symbol = gtsam.Symbol(key_val) - if symbol.chr() == ord("l"): # Check if it's a landmark symbol - # Check existence again (though keys() implies existence) - if step_results.exists(key_val): - lm_point = step_results.atPoint2(key_val) - est_landmarks_x.append(lm_point[0]) - est_landmarks_y.append(lm_point[1]) - landmark_keys_in_frame.append(key_val) + # Check all potential landmark keys up to max_landmark_index + for j in range(max_landmark_index + 1): + lm_key = L(j) + if step_results.exists(lm_key): + lm_point = step_results.atPoint2(lm_key) + est_landmarks_x.append(lm_point[0]) + est_landmarks_y.append(lm_point[1]) + landmark_keys_in_frame.append(lm_key) - if est_landmarks_x: - traces.append( - go.Scatter( - x=est_landmarks_x, - y=est_landmarks_y, - mode="markers", - marker=dict(color="blue", size=6, symbol="x"), - name="Landmarks Est", # Legend entry for all estimated landmarks - ) - ) + lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) + if lm_trace: + frame_traces.append(lm_trace) - # 3. Covariance Ellipses (if marginals available) + # 3. Generate Covariance Ellipses (if marginals available) if step_marginals is not None: - # Current Pose Covariance Ellipse + # Pose ellipse current_pose_key = X(k) if step_results.exists(current_pose_key): try: pose_cov = step_marginals.marginalCovariance(current_pose_key) pose_mean = step_results.atPose2(current_pose_key).translation() - angle, width, height = gtsam_cov_to_plotly_ellipse( - pose_cov, scale=ellipse_scale - ) - shapes.append( - _create_ellipse_shape_dict( - cx=pose_mean[0], - cy=pose_mean[1], - angle=angle, - width=width, - height=height, - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", - name=f"Pose {k} Cov", - ) + frame_shapes.append( + create_pose_ellipse_shape(pose_mean, pose_cov, k, ellipse_scale) ) except Exception as e: if verbose: - print( - f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" - ) + print(f"Warn: Pose {k} cov err @ step {k}: {e}") - # Landmark Covariance Ellipses + # Landmark ellipses for lm_key in landmark_keys_in_frame: try: lm_cov = step_marginals.marginalCovariance(lm_key) lm_mean = step_results.atPoint2(lm_key) - angle, width, height = gtsam_cov_to_plotly_ellipse( - lm_cov, scale=ellipse_scale - ) - symbol = gtsam.Symbol(lm_key) - shapes.append( - _create_ellipse_shape_dict( - cx=lm_mean[0], - cy=lm_mean[1], - angle=angle, - width=width, - height=height, - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", - name=f"LM {symbol.index()} Cov", + lm_index = gtsam.Symbol(lm_key).index() + frame_shapes.append( + create_landmark_ellipse_shape( + lm_mean, lm_cov, lm_index, ellipse_scale ) ) except Exception as e: - symbol = gtsam.Symbol(lm_key) + lm_index = gtsam.Symbol(lm_key).index() if verbose: - print( - f"Warning: Failed getting landmark {symbol.index()} cov ellipse at step {k}: {e}" - ) + print(f"Warn: LM {lm_index} cov err @ step {k}: {e}") - return traces, shapes + return frame_traces, frame_shapes -def _configure_figure_layout( +# --- Figure Configuration --- + + +def configure_figure_layout( fig: go.Figure, num_steps: int, world_size: float, initial_shapes: List[Dict[str, Any]], ) -> None: - """Configures the Plotly figure's layout, axes, slider, and buttons.""" - + """Configures Plotly figure layout, axes, slider, buttons.""" steps = list(range(num_steps + 1)) - - # Slider sliders = [ dict( active=0, @@ -291,12 +257,10 @@ def _configure_figure_layout( label=str(k), method="animate", args=[ - [str(k)], # Frame name + [str(k)], dict( mode="immediate", - frame=dict( - duration=100, redraw=True - ), # Redraw needed for shapes + frame=dict(duration=100, redraw=True), transition=dict(duration=0), ), ], @@ -305,18 +269,22 @@ def _configure_figure_layout( ], ) ] - - # Buttons updatemenus = [ dict( type="buttons", showactive=False, + direction="left", + pad={"r": 10, "t": 87}, + x=0.1, + xanchor="right", + y=0, + yanchor="top", buttons=[ dict( label="Play", method="animate", args=[ - None, # Animate all frames + None, dict( mode="immediate", frame=dict(duration=100, redraw=True), @@ -329,7 +297,7 @@ def _configure_figure_layout( label="Pause", method="animate", args=[ - [None], # Stop animation + [None], dict( mode="immediate", frame=dict(duration=0, redraw=False), @@ -338,25 +306,15 @@ def _configure_figure_layout( ], ), ], - direction="left", - pad={"r": 10, "t": 87}, - x=0.1, - xanchor="right", - y=0, - yanchor="top", ) ] - # Layout settings fig.update_layout( title="Iterative Factor Graph SLAM Animation", - xaxis=dict( - range=[-world_size / 2 - 2, world_size / 2 + 2], - constrain="domain", # Keep aspect ratio when zooming - ), + xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - scaleanchor="x", # Ensure square aspect ratio + scaleanchor="x", scaleratio=1, ), width=800, @@ -364,8 +322,7 @@ def _configure_figure_layout( hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Set initial shapes from frame 0 - # Add legend if desired + shapes=initial_shapes, legend=dict( traceorder="reversed", title_text="Legend", @@ -378,90 +335,79 @@ def _configure_figure_layout( ) -# --- Main Animation Function (Orchestrator) --- +# --- Main Animation Orchestrator --- def create_slam_animation( results_history: List[gtsam.Values], marginals_history: List[Optional[gtsam.Marginals]], num_steps: int, - X: callable, - L: callable, + X: Callable[[int], int], + L: Callable[[int], int], + max_landmark_index: int, # Required to iterate potential landmarks landmarks_gt_array: Optional[np.ndarray] = None, poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, verbose_cov_errors: bool = False, ) -> go.Figure: - """ - Creates a Plotly animation of the SLAM results in a modular way. - - Args: - results_history: List of gtsam.Values, one per step. - marginals_history: List of gtsam.Marginals or None, one per step. - num_steps: The total number of steps (results_history should have length num_steps + 1). - X: Symbol function for poses (e.g., lambda i: gtsam.symbol('x', i)). - L: Symbol function for landmarks (e.g., lambda j: gtsam.symbol('l', j)). - landmarks_gt_array: Optional Nx2 numpy array of ground truth landmark positions. - poses_gt: Optional list of gtsam.Pose2 ground truth poses. - world_size: Approximate size of the world for axis scaling. - ellipse_scale: Scaling factor for covariance ellipses (e.g., 2.0 for 2-sigma). - verbose_cov_errors: If True, print warnings for covariance calculation errors. - - Returns: - A plotly.graph_objects.Figure containing the animation. - """ + """Creates a modular Plotly SLAM animation.""" print("Generating Plotly animation...") - fig = go.Figure() - # 1. Add static ground truth elements - _add_ground_truth_traces(fig, landmarks_gt_array, poses_gt) + # 1. Add static ground truth traces to the base figure (visible always) + gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) + if gt_lm_trace: + fig.add_trace(gt_lm_trace) + gt_path_trace = create_gt_path_trace(poses_gt) + if gt_path_trace: + fig.add_trace(gt_path_trace) - # 2. Create frames for animation + # 2. Generate frames with dynamic content frames = [] steps_iterable = range(num_steps + 1) - - # Use tqdm for progress bar if available try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") except NameError: - pass # tqdm not installed or not in notebook env + pass # tqdm optional for k in steps_iterable: step_results = results_history[k] step_marginals = marginals_history[k] if marginals_history else None - # Create traces and shapes for this specific frame - frame_traces, frame_shapes = _create_single_frame_data( - k, step_results, step_marginals, X, L, ellipse_scale, verbose_cov_errors + frame_traces, frame_shapes = generate_frame_content( + k, + step_results, + step_marginals, + X, + L, + max_landmark_index, + ellipse_scale, + verbose_cov_errors, ) - - # Create the Plotly frame object frames.append( go.Frame( - data=frame_traces, - name=str(k), # Name used by slider/buttons - layout=go.Layout( - shapes=frame_shapes - ), # Shapes are part of layout per frame + data=frame_traces, name=str(k), layout=go.Layout(shapes=frame_shapes) ) ) - # 3. Set initial figure state (using data from frame 0) + # 3. Set initial dynamic data (from frame 0) onto the base figure + initial_dynamic_traces = [] + initial_shapes = [] if frames: - # Add traces from the first frame as the initial state - for trace in frames[0].data: - fig.add_trace(trace) + # Important: Add *copies* or ensure traces are regenerated if needed, + # though Plotly usually handles this ok with frame data. + initial_dynamic_traces = frames[0].data initial_shapes = frames[0].layout.shapes if frames[0].layout else [] - else: - initial_shapes = [] + for trace in initial_dynamic_traces: + fig.add_trace(trace) # Add Est Path[0], Est Landmarks[0] traces # 4. Assign frames to the figure fig.update(frames=frames) - # 5. Configure overall layout, slider, buttons - _configure_figure_layout(fig, num_steps, world_size, initial_shapes) + # 5. Configure layout, axes, controls + # Pass initial_shapes for the layout's starting state + configure_figure_layout(fig, num_steps, world_size, initial_shapes) print("Plotly animation generated.") return fig From e4278687b412c60e78bfc50c0b2c396b38553920 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 16:42:23 -0400 Subject: [PATCH 21/38] Plot side by side --- python/gtsam/examples/EKF_SLAM.ipynb | 63 ++--- python/gtsam/examples/gtsam_plotly.py | 357 +++++++++++++++----------- 2 files changed, 221 insertions(+), 199 deletions(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 84e69e1a1..265005710 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -106,8 +106,6 @@ "outputs": [], "source": [ "import numpy as np\n", - "import matplotlib.pyplot as plt # For initial plot if desired (optional)\n", - "import plotly.graph_objects as go\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", "import time # To slow down graphviz display if needed\n", @@ -117,11 +115,10 @@ "\n", "# Imports for new modules\n", "import simulation\n", - "import gtsam_plotly \n", + "from gtsam_plotly import SlamFrameData, create_slam_animation\n", "\n", "# Imports for graph visualization\n", - "import graphviz\n", - "from IPython.display import display, clear_output" + "import graphviz" ] }, { @@ -158,11 +155,7 @@ "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0\n", - "\n", - "# Visualization parameters\n", - "SHOW_GRAPHVIZ_EACH_STEP = True # Set to False to only show final graph\n", - "GRAPHVIZ_PAUSE_SECONDS = 1 # Pause briefly to allow viewing the graph" + "MAX_SENSOR_RANGE = 5.0" ] }, { @@ -215,15 +208,11 @@ "metadata": {}, "outputs": [], "source": [ - "writer = gtsam.GraphvizFormatting()\n", - "writer.binaryEdges = True\n", + "WRITER = gtsam.GraphvizFormatting()\n", + "WRITER.binaryEdges = True\n", "\n", - "def maybe_show(graph, estimate, message):\n", - " if SHOW_GRAPHVIZ_EACH_STEP:\n", - " print(message)\n", - " graph_dot = graph.dot(estimate, writer=writer)\n", - " display(graphviz.Source(graph_dot, engine='neato'))\n", - " time.sleep(GRAPHVIZ_PAUSE_SECONDS)" + "def mage_dot(graph, estimate):\n", + " return graph.dot(estimate, writer=WRITER)\n" ] }, { @@ -248,20 +237,13 @@ "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", "\n", "# Variables to store results for animation\n", - "results_history = [gtsam.Values(current_estimate)] # Store Values object at each step\n", - "marginals_history = [] # Store Marginals object at each step\n", + "history = [] # Store SLAMFrameData objects for each step\n", "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", "\n", "# Initial marginals (only for X(0))\n", - "try:\n", - " initial_gaussian_graph = current_graph.linearize(current_estimate)\n", - " initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", - " marginals_history.append(initial_marginals)\n", - "except Exception as e:\n", - " print(f\"Error computing initial marginals: {e}\")\n", - " marginals_history.append(None) # Append placeholder if fails\n", - "\n", - "maybe_show(current_graph, current_estimate, \"--- Step 0 --- Initial Graph with Prior ---\")" + "initial_gaussian_graph = current_graph.linearize(current_estimate)\n", + "initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", + "history.append(SlamFrameData(0, current_estimate, initial_marginals, mage_dot(current_graph, current_estimate)))" ] }, { @@ -324,16 +306,12 @@ " optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n", " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", " \n", - " # --- Display Factor Graph --- \n", - " maybe_show(current_graph, current_estimate, f\"--- Step {k+1} --- Factor Graph ---\")\n", - "\n", - " # Store results for animation\n", - " results_history.append(gtsam.Values(current_estimate)) # Store a copy\n", - " \n", " # Calculate marginals for visualization (can be slow for large graphs)\n", " current_gaussian_graph = current_graph.linearize(current_estimate)\n", " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", - " marginals_history.append(current_marginals)\n", + "\n", + " # Store the current state for visualization\n", + " history.append(SlamFrameData(k+1, current_estimate, current_marginals, mage_dot(current_graph, current_estimate)))\n", "\n", "print(\"\\nIterative Factor Graph SLAM finished.\")\n", "print(f\"Final number of poses estimated: {len([key for key in current_estimate.keys() if gtsam.Symbol(key).chr() == ord('x')])}\")\n", @@ -360,15 +338,14 @@ }, "outputs": [], "source": [ - "fig = gtsam_plotly.create_slam_animation(\n", - " results_history=results_history,\n", - " marginals_history=marginals_history,\n", + "fig = create_slam_animation(\n", + " history,\n", " landmarks_gt_array=landmarks_gt_array,\n", " poses_gt=poses_gt,\n", - " num_steps=NUM_STEPS,\n", " world_size=WORLD_SIZE,\n", - " X=X, # Pass symbol functions\n", - " L=L\n", + " X=X, # Pass symbol functions\n", + " L=L,\n", + " max_landmark_index=NUM_LANDMARKS,\n", ")\n", "\n", "print(\"Displaying animation...\")\n", @@ -384,7 +361,7 @@ "\n", "* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n", "* **Modularity:** Simulation and Plotly visualization code have been moved into separate `simulation.py` and `gtsam_plotly.py` files for better organization.\n", - "* **Graph Visualization:** The `graphviz` library was used to render the factor graph at each step (or only at the end, depending on `SHOW_GRAPHVIZ_EACH_STEP`). This helps visualize how the graph structure grows and connects poses and landmarks.\n", + "* **Graph Visualization:** The `graphviz` library was used to render the factor graph at each step. This helps visualize how the graph structure grows and connects poses and landmarks.\n", "* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n", "* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n", "* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)." diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index d37e32dc9..dbbae8f46 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -1,63 +1,78 @@ -# gtsam_plotly_modular_v2.py +# gtsam_plotly_modular_v3.py +import base64 +from dataclasses import dataclass from typing import Any, Callable, Dict, List, Optional, Tuple +import graphviz import numpy as np import plotly.graph_objects as go from tqdm.notebook import tqdm import gtsam -# --- Core Ellipse Calculations --- + +# --- Dataclass for History --- +@dataclass +class SlamFrameData: + """Holds all data needed for a single frame of the SLAM animation.""" + + step_index: int + results: gtsam.Values + marginals: Optional[gtsam.Marginals] + graph_dot_string: Optional[str] = None # Store the Graphviz DOT string -def ellipse_path( - cx: float, cy: float, sizex: float, sizey: float, angle: float, N: int = 60 +# --- Core Ellipse Calculation & Path Generation --- + + +def create_ellipse_path_from_cov( + cx: float, cy: float, cov_matrix: np.ndarray, scale: float = 2.0, N: int = 60 ) -> str: - """Generates SVG path string for a rotated ellipse.""" - angle_rad = np.radians(angle) - t = np.linspace(0, 2 * np.pi, N) - x_unit = (sizex / 2) * np.cos(t) - y_unit = (sizey / 2) * np.sin(t) - x_rot = cx + x_unit * np.cos(angle_rad) - y_unit * np.sin(angle_rad) - y_rot = cy + x_unit * np.sin(angle_rad) + y_unit * np.cos(angle_rad) + """Generates SVG path string for an ellipse directly from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite + try: + eigvals, eigvecs = np.linalg.eigh(cov) + # eigh sorts eigenvalues in ascending order + eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues + minor_std, major_std = np.sqrt(eigvals) + v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] # Corresponding eigenvectors + except np.linalg.LinAlgError: + # Fallback to a small circle if decomposition fails + radius = 0.1 * scale + t = np.linspace(0, 2 * np.pi, N) + x_p = cx + radius * np.cos(t) + y_p = cy + radius * np.sin(t) + else: + # Parametric equation: Center + scale * major_std * v_major * cos(t) + scale * minor_std * v_minor * sin(t) + # Note: We use scale*std which corresponds to half-axis lengths a,b used before + t = np.linspace(0, 2 * np.pi, N) + cos_t, sin_t = np.cos(t), np.sin(t) + x_p = cx + scale * ( + major_std * cos_t * v_major[0] + minor_std * sin_t * v_minor[0] + ) + y_p = cy + scale * ( + major_std * cos_t * v_major[1] + minor_std * sin_t * v_minor[1] + ) + + # Build SVG path string path = ( - f"M {x_rot[0]},{y_rot[0]} " - + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_rot[1:], y_rot[1:])) + f"M {x_p[0]},{y_p[0]} " + + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_p[1:], y_p[1:])) + " Z" ) return path -def gtsam_cov_to_plotly_ellipse( - cov_matrix: np.ndarray, scale: float = 2.0 -) -> Tuple[float, float, float]: - """Calculates ellipse angle (deg), width, height from 2x2 covariance.""" - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite - try: - eigvals, eigvecs = np.linalg.eigh(cov) - eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues - except np.linalg.LinAlgError: - return 0, 0.1 * scale, 0.1 * scale # Default on failure - - width = 2 * scale * np.sqrt(eigvals[1]) # Major axis (largest eigenvalue) - height = 2 * scale * np.sqrt(eigvals[0]) # Minor axis (smallest eigenvalue) - angle_rad = np.arctan2( - eigvecs[1, 1], eigvecs[0, 1] - ) # Angle of major axis eigenvector - angle_deg = np.degrees(angle_rad) - return angle_deg, width, height - - # --- Plotly Element Generators --- -def create_gt_landmarks_trace(landmarks_gt_array: np.ndarray) -> Optional[go.Scatter]: +def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: """Creates scatter trace for ground truth landmarks.""" - if landmarks_gt_array is None or landmarks_gt_array.size == 0: + if landmarks_gt is None or landmarks_gt.size == 0: return None return go.Scatter( - x=landmarks_gt_array[0, :], - y=landmarks_gt_array[1, :], + x=landmarks_gt[0, :], + y=landmarks_gt[1, :], mode="markers", marker=dict(color="black", size=8, symbol="star"), name="Landmarks GT", @@ -68,11 +83,9 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: """Creates line trace for ground truth path.""" if not poses_gt: return None - gt_path_x = [p.x() for p in poses_gt] - gt_path_y = [p.y() for p in poses_gt] return go.Scatter( - x=gt_path_x, - y=gt_path_y, + x=[p.x() for p in poses_gt], + y=[p.y() for p in poses_gt], mode="lines", line=dict(color="gray", width=1, dash="dash"), name="Path GT", @@ -82,21 +95,21 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: def create_est_path_trace( est_path_x: List[float], est_path_y: List[float] ) -> go.Scatter: - """Creates scatter/line trace for the estimated path up to current step.""" + """Creates trace for the estimated path.""" return go.Scatter( x=est_path_x, y=est_path_y, mode="lines+markers", line=dict(color="red", width=2), marker=dict(size=4, color="red"), - name="Path Est", # This name applies to the trace in the specific frame + name="Path Est", ) def create_est_landmarks_trace( est_landmarks_x: List[float], est_landmarks_y: List[float] ) -> Optional[go.Scatter]: - """Creates scatter trace for currently estimated landmarks.""" + """Creates trace for estimated landmarks.""" if not est_landmarks_x: return None return go.Scatter( @@ -104,136 +117,155 @@ def create_est_landmarks_trace( y=est_landmarks_y, mode="markers", marker=dict(color="blue", size=6, symbol="x"), - name="Landmarks Est", # Applies to landmarks in the specific frame + name="Landmarks Est", ) def _create_ellipse_shape_dict( - cx, cy, angle, width, height, fillcolor, line_color, name_suffix + cx, cy, cov, scale, fillcolor, line_color ) -> Dict[str, Any]: - """Helper to create the dictionary for a Plotly ellipse shape.""" + """Helper: Creates dict for a Plotly ellipse shape from covariance.""" + path = create_ellipse_path_from_cov(cx, cy, cov, scale) return dict( type="path", - path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle), + path=path, xref="x", yref="y", fillcolor=fillcolor, line_color=line_color, - # name=f"{name_suffix} Cov", # Name isn't really used by Plotly for shapes ) def create_pose_ellipse_shape( - pose_mean_xy: np.ndarray, pose_cov: np.ndarray, k: int, scale: float + pose_mean_xy: np.ndarray, pose_cov: np.ndarray, scale: float ) -> Dict[str, Any]: - """Creates shape dictionary for a pose covariance ellipse.""" - angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov, scale) + """Creates shape dict for a pose covariance ellipse.""" return _create_ellipse_shape_dict( cx=pose_mean_xy[0], cy=pose_mean_xy[1], - angle=angle, - width=width, - height=height, + cov=pose_cov, + scale=scale, fillcolor="rgba(255,0,255,0.2)", line_color="rgba(255,0,255,0.5)", - name_suffix=f"Pose {k}", ) def create_landmark_ellipse_shape( - lm_mean_xy: np.ndarray, lm_cov: np.ndarray, lm_index: int, scale: float + lm_mean_xy: np.ndarray, lm_cov: np.ndarray, scale: float ) -> Dict[str, Any]: - """Creates shape dictionary for a landmark covariance ellipse.""" - angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov, scale) + """Creates shape dict for a landmark covariance ellipse.""" return _create_ellipse_shape_dict( cx=lm_mean_xy[0], cy=lm_mean_xy[1], - angle=angle, - width=width, - height=height, + cov=lm_cov, + scale=scale, fillcolor="rgba(0,0,255,0.1)", line_color="rgba(0,0,255,0.3)", - name_suffix=f"LM {lm_index}", ) +def dot_string_to_base64_png( + dot_string: Optional[str], engine: str = "neato" +) -> Optional[str]: + """Renders a DOT string to a base64 encoded PNG using graphviz.""" + if not dot_string: + return None + source = graphviz.Source(dot_string, engine="neato") + # Use pipe() to get bytes directly without saving a file + png_bytes = source.pipe(format="png") + encoded_string = base64.b64encode(png_bytes).decode() + return f"data:image/png;base64,{encoded_string}" + # --- Frame Content Generation --- - - def generate_frame_content( - k: int, - step_results: gtsam.Values, - step_marginals: Optional[gtsam.Marginals], + frame_data: SlamFrameData, X: Callable[[int], int], L: Callable[[int], int], - max_landmark_index: int, # Need to know the potential range of landmarks + max_landmark_index: int, ellipse_scale: float = 2.0, + graphviz_engine: str = "neato", # Added engine parameter verbose: bool = False, -) -> Tuple[List[go.Scatter], List[Dict[str, Any]]]: - """Generates all dynamic traces and shapes for a single animation frame `k`.""" +) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: + """Generates dynamic traces, shapes, and layout image for a single frame.""" + k = frame_data.step_index + step_results = frame_data.results + step_marginals = frame_data.marginals + frame_traces: List[go.Scatter] = [] frame_shapes: List[Dict[str, Any]] = [] + layout_image: Optional[Dict[str, Any]] = None - # 1. Gather Estimated Path Data - est_path_x = [] - est_path_y = [] - for i in range(k + 1): - pose_key = X(i) - if step_results.exists(pose_key): - pose = step_results.atPose2(pose_key) - est_path_x.append(pose.x()) - est_path_y.append(pose.y()) + # 1. Estimated Path (Unchanged) + est_path_x = [ + step_results.atPose2(X(i)).x() + for i in range(k + 1) + if step_results.exists(X(i)) + ] + est_path_y = [ + step_results.atPose2(X(i)).y() + for i in range(k + 1) + if step_results.exists(X(i)) + ] frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) - # 2. Gather Estimated Landmark Data - est_landmarks_x = [] - est_landmarks_y = [] - landmark_keys_in_frame = [] - # Check all potential landmark keys up to max_landmark_index + # 2. Estimated Landmarks (Unchanged) + est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) if step_results.exists(lm_key): lm_point = step_results.atPoint2(lm_key) est_landmarks_x.append(lm_point[0]) est_landmarks_y.append(lm_point[1]) - landmark_keys_in_frame.append(lm_key) - + landmark_keys.append(lm_key) lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: frame_traces.append(lm_trace) - # 3. Generate Covariance Ellipses (if marginals available) - if step_marginals is not None: - # Pose ellipse - current_pose_key = X(k) - if step_results.exists(current_pose_key): + # 3. Covariance Ellipses (Unchanged) + if step_marginals: + pose_key = X(k) + if step_results.exists(pose_key): try: - pose_cov = step_marginals.marginalCovariance(current_pose_key) - pose_mean = step_results.atPose2(current_pose_key).translation() - frame_shapes.append( - create_pose_ellipse_shape(pose_mean, pose_cov, k, ellipse_scale) - ) + cov = step_marginals.marginalCovariance(pose_key) + mean = step_results.atPose2(pose_key).translation() + frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) except Exception as e: if verbose: print(f"Warn: Pose {k} cov err @ step {k}: {e}") - - # Landmark ellipses - for lm_key in landmark_keys_in_frame: + for lm_key in landmark_keys: try: - lm_cov = step_marginals.marginalCovariance(lm_key) - lm_mean = step_results.atPoint2(lm_key) - lm_index = gtsam.Symbol(lm_key).index() + cov = step_marginals.marginalCovariance(lm_key) + mean = step_results.atPoint2(lm_key) frame_shapes.append( - create_landmark_ellipse_shape( - lm_mean, lm_cov, lm_index, ellipse_scale - ) + create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) except Exception as e: - lm_index = gtsam.Symbol(lm_key).index() if verbose: - print(f"Warn: LM {lm_index} cov err @ step {k}: {e}") + print( + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" + ) - return frame_traces, frame_shapes + # 4. Graph Image for Layout (MODIFIED) + # Use the new function with the dot string from frame_data + img_src = dot_string_to_base64_png( + frame_data.graph_dot_string, engine=graphviz_engine + ) + if img_src: + layout_image = dict( + source=img_src, + xref="paper", + yref="paper", + x=0, + y=1, + sizex=0.48, + sizey=1, + xanchor="left", + yanchor="top", + layer="below", + sizing="contain", + ) + + return frame_traces, frame_shapes, layout_image # --- Figure Configuration --- @@ -244,9 +276,12 @@ def configure_figure_layout( num_steps: int, world_size: float, initial_shapes: List[Dict[str, Any]], + initial_image: Optional[Dict[str, Any]], ) -> None: - """Configures Plotly figure layout, axes, slider, buttons.""" + """Configures Plotly figure layout for side-by-side display.""" steps = list(range(num_steps + 1)) + plot_domain = [0.52, 1.0] # Right pane for the SLAM plot + sliders = [ dict( active=0, @@ -275,10 +310,10 @@ def configure_figure_layout( showactive=False, direction="left", pad={"r": 10, "t": 87}, - x=0.1, - xanchor="right", + x=plot_domain[0], + xanchor="left", y=0, - yanchor="top", + yanchor="top", # Position relative to plot area buttons=[ dict( label="Play", @@ -310,28 +345,29 @@ def configure_figure_layout( ] fig.update_layout( - title="Iterative Factor Graph SLAM Animation", - xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), + title="Factor Graph SLAM Animation (Graph Left, Results Right)", + xaxis=dict( + range=[-world_size / 2 - 2, world_size / 2 + 2], + domain=plot_domain, # Confine axis to right pane + constrain="domain", + ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], scaleanchor="x", scaleratio=1, + domain=[0, 1], # Full height within its domain ), - width=800, - height=800, + width=1000, + height=600, # Adjust size for side-by-side hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, - legend=dict( - traceorder="reversed", - title_text="Legend", - orientation="h", - yanchor="bottom", - y=1.02, - xanchor="right", - x=1, - ), + shapes=initial_shapes, # Initial shapes for the SLAM plot + images=( + [initial_image] if initial_image else [] + ), # Initial image for the left pane + showlegend=False, # Forego legend for space + # Autosize=True might help responsiveness but can interfere with domain ) @@ -339,23 +375,25 @@ def configure_figure_layout( def create_slam_animation( - results_history: List[gtsam.Values], - marginals_history: List[Optional[gtsam.Marginals]], - num_steps: int, + history: List[SlamFrameData], X: Callable[[int], int], L: Callable[[int], int], - max_landmark_index: int, # Required to iterate potential landmarks + max_landmark_index: int, landmarks_gt_array: Optional[np.ndarray] = None, poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, + graphviz_engine: str = "neato", # Pass engine choice verbose_cov_errors: bool = False, ) -> go.Figure: - """Creates a modular Plotly SLAM animation.""" + """Creates a side-by-side Plotly SLAM animation using a history of dataclasses.""" + if not history: + raise ValueError("History cannot be empty.") print("Generating Plotly animation...") + num_steps = history[-1].step_index fig = go.Figure() - # 1. Add static ground truth traces to the base figure (visible always) + # 1. Add static GT traces (Unchanged) gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) if gt_lm_trace: fig.add_trace(gt_lm_trace) @@ -363,51 +401,58 @@ def create_slam_animation( if gt_path_trace: fig.add_trace(gt_path_trace) - # 2. Generate frames with dynamic content + # 2. Generate frames (MODIFIED: pass graphviz_engine) frames = [] + initial_dynamic_traces, initial_shapes, initial_image = [], [], None steps_iterable = range(num_steps + 1) try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") except NameError: - pass # tqdm optional + pass for k in steps_iterable: - step_results = results_history[k] - step_marginals = marginals_history[k] if marginals_history else None + frame_data = next((item for item in history if item.step_index == k), None) + if frame_data is None: + print(f"Warning: Missing data for step {k} in history.") + continue - frame_traces, frame_shapes = generate_frame_content( - k, - step_results, - step_marginals, + # Pass the engine choice to the content generator + frame_traces, frame_shapes, layout_image = generate_frame_content( + frame_data, X, L, max_landmark_index, ellipse_scale, + graphviz_engine, verbose_cov_errors, ) + + if k == 0: + initial_dynamic_traces, initial_shapes, initial_image = ( + frame_traces, + frame_shapes, + layout_image, + ) + frames.append( go.Frame( - data=frame_traces, name=str(k), layout=go.Layout(shapes=frame_shapes) + data=frame_traces, + name=str(k), + layout=go.Layout( + shapes=frame_shapes, images=[layout_image] if layout_image else [] + ), ) ) - # 3. Set initial dynamic data (from frame 0) onto the base figure - initial_dynamic_traces = [] - initial_shapes = [] - if frames: - # Important: Add *copies* or ensure traces are regenerated if needed, - # though Plotly usually handles this ok with frame data. - initial_dynamic_traces = frames[0].data - initial_shapes = frames[0].layout.shapes if frames[0].layout else [] - for trace in initial_dynamic_traces: - fig.add_trace(trace) # Add Est Path[0], Est Landmarks[0] traces + # 3. Add initial dynamic traces (Unchanged) + for trace in initial_dynamic_traces: + fig.add_trace(trace) - # 4. Assign frames to the figure + # 4. Assign frames (Unchanged) fig.update(frames=frames) - # 5. Configure layout, axes, controls - # Pass initial_shapes for the layout's starting state - configure_figure_layout(fig, num_steps, world_size, initial_shapes) + # 5. Configure layout (Unchanged) + configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.") return fig From d1780601cd44498f2acb0b3e8add64a2d95584c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Apr 2025 09:50:41 -0400 Subject: [PATCH 22/38] SVG graphs --- python/gtsam/examples/EKF_SLAM.ipynb | 9 +- python/gtsam/examples/gtsam_plotly.py | 142 +++++++++++++++++--------- python/gtsam/examples/simulation.py | 2 +- 3 files changed, 99 insertions(+), 54 deletions(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 265005710..48a262f5e 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -108,17 +108,12 @@ "import numpy as np\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", - "import time # To slow down graphviz display if needed\n", "\n", "import gtsam\n", "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", "\n", - "# Imports for new modules\n", "import simulation\n", - "from gtsam_plotly import SlamFrameData, create_slam_animation\n", - "\n", - "# Imports for graph visualization\n", - "import graphviz" + "from gtsam_plotly import SlamFrameData, create_slam_animation" ] }, { @@ -143,7 +138,7 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 20 # Reduced steps for faster animation generation\n", + "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", "DT = 1.0 # Time step duration (simplified)\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index dbbae8f46..a0d6e0ed3 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -164,17 +164,30 @@ def create_landmark_ellipse_shape( ) -def dot_string_to_base64_png( +def dot_string_to_base64_svg( dot_string: Optional[str], engine: str = "neato" ) -> Optional[str]: - """Renders a DOT string to a base64 encoded PNG using graphviz.""" + """Renders a DOT string to a base64 encoded SVG using graphviz.""" if not dot_string: return None - source = graphviz.Source(dot_string, engine="neato") - # Use pipe() to get bytes directly without saving a file - png_bytes = source.pipe(format="png") - encoded_string = base64.b64encode(png_bytes).decode() - return f"data:image/png;base64,{encoded_string}" + try: + # Set DPI for potentially better default sizing, though less critical for SVG + # graph_attr = {'dpi': '150'} # You can experiment with this + source = graphviz.Source(dot_string, engine=engine) # , graph_attr=graph_attr) + # Use pipe() to get bytes directly without saving a file, format="svg" + svg_bytes = source.pipe(format="svg") + # Encode bytes to base64 string, decode result to UTF-8 string + encoded_string = base64.b64encode(svg_bytes).decode("utf-8") + # Return data URI for SVG + return f"data:image/svg+xml;base64,{encoded_string}" + except graphviz.backend.execute.CalledProcessError as e: + print(f"Graphviz rendering error ({engine}): {e}") + # Optionally return a placeholder or raise + return None + except Exception as e: + print(f"Unexpected error during Graphviz SVG generation: {e}") + return None + # --- Frame Content Generation --- def generate_frame_content( @@ -183,7 +196,7 @@ def generate_frame_content( L: Callable[[int], int], max_landmark_index: int, ellipse_scale: float = 2.0, - graphviz_engine: str = "neato", # Added engine parameter + graphviz_engine: str = "neato", verbose: bool = False, ) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: """Generates dynamic traces, shapes, and layout image for a single frame.""" @@ -191,11 +204,11 @@ def generate_frame_content( step_results = frame_data.results step_marginals = frame_data.marginals - frame_traces: List[go.Scatter] = [] + frame_dynamic_traces: List[go.Scatter] = [] # Renamed for clarity frame_shapes: List[Dict[str, Any]] = [] layout_image: Optional[Dict[str, Any]] = None - # 1. Estimated Path (Unchanged) + # 1. Estimated Path est_path_x = [ step_results.atPose2(X(i)).x() for i in range(k + 1) @@ -206,9 +219,9 @@ def generate_frame_content( for i in range(k + 1) if step_results.exists(X(i)) ] - frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) + frame_dynamic_traces.append(create_est_path_trace(est_path_x, est_path_y)) - # 2. Estimated Landmarks (Unchanged) + # 2. Estimated Landmarks est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) @@ -219,9 +232,9 @@ def generate_frame_content( landmark_keys.append(lm_key) lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: - frame_traces.append(lm_trace) + frame_dynamic_traces.append(lm_trace) - # 3. Covariance Ellipses (Unchanged) + # 3. Covariance Ellipses if step_marginals: pose_key = X(k) if step_results.exists(pose_key): @@ -229,9 +242,13 @@ def generate_frame_content( cov = step_marginals.marginalCovariance(pose_key) mean = step_results.atPose2(pose_key).translation() frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) - except Exception as e: + except RuntimeError as e: # More specific exception type if verbose: print(f"Warn: Pose {k} cov err @ step {k}: {e}") + except Exception as e: + if verbose: + print(f"Warn: Pose {k} cov OTHER err @ step {k}: {e}") + for lm_key in landmark_keys: try: cov = step_marginals.marginalCovariance(lm_key) @@ -239,15 +256,20 @@ def generate_frame_content( frame_shapes.append( create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) - except Exception as e: + except RuntimeError as e: # More specific exception type if verbose: print( f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" ) + except Exception as e: + if verbose: + print( + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov OTHER err @ step {k}: {e}" + ) - # 4. Graph Image for Layout (MODIFIED) - # Use the new function with the dot string from frame_data - img_src = dot_string_to_base64_png( + # 4. Graph Image for Layout (MODIFIED TO USE SVG) + # Use the SVG function with the dot string from frame_data + img_src = dot_string_to_base64_svg( # Changed function call frame_data.graph_dot_string, engine=graphviz_engine ) if img_src: @@ -262,10 +284,11 @@ def generate_frame_content( xanchor="left", yanchor="top", layer="below", - sizing="contain", + sizing="contain", # Contain should work well with SVG ) - return frame_traces, frame_shapes, layout_image + # Return only the things that CHANGE frame-to-frame + return frame_dynamic_traces, frame_shapes, layout_image # --- Figure Configuration --- @@ -374,6 +397,9 @@ def configure_figure_layout( # --- Main Animation Orchestrator --- +# --- Main Animation Orchestrator (MODIFIED TO FIX GT DISPLAY) --- + + def create_slam_animation( history: List[SlamFrameData], X: Callable[[int], int], @@ -383,7 +409,7 @@ def create_slam_animation( poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, - graphviz_engine: str = "neato", # Pass engine choice + graphviz_engine: str = "neato", verbose_cov_errors: bool = False, ) -> go.Figure: """Creates a side-by-side Plotly SLAM animation using a history of dataclasses.""" @@ -393,31 +419,63 @@ def create_slam_animation( num_steps = history[-1].step_index fig = go.Figure() - # 1. Add static GT traces (Unchanged) + # 1. Create static GT traces ONCE + gt_traces = [] gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) if gt_lm_trace: - fig.add_trace(gt_lm_trace) + gt_traces.append(gt_lm_trace) gt_path_trace = create_gt_path_trace(poses_gt) if gt_path_trace: - fig.add_trace(gt_path_trace) + gt_traces.append(gt_path_trace) - # 2. Generate frames (MODIFIED: pass graphviz_engine) + # 2. Generate content for the initial frame (k=0) to set up the figure + initial_frame_data = next((item for item in history if item.step_index == 0), None) + if initial_frame_data is None: + raise ValueError("History must contain data for step 0.") + + ( + initial_dynamic_traces, + initial_shapes, + initial_image, + ) = generate_frame_content( + initial_frame_data, + X, + L, + max_landmark_index, + ellipse_scale, + graphviz_engine, + verbose_cov_errors, + ) + + # 3. Add initial traces to the figure (GT first, then initial dynamic traces) + # These define the plot state BEFORE the animation starts or when slider is at 0 + for trace in gt_traces: + fig.add_trace(trace) + for trace in initial_dynamic_traces: + fig.add_trace(trace) + + # 4. Generate frames for the animation (k=0 to num_steps) frames = [] - initial_dynamic_traces, initial_shapes, initial_image = [], [], None steps_iterable = range(num_steps + 1) try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") - except NameError: + except NameError: # Handle if tqdm is not available (e.g. standard Python script) pass + except Exception as e: # Catch other potential tqdm issues + print( + f"Note: Could not initialize tqdm progress bar ({e}). Continuing without it." + ) for k in steps_iterable: frame_data = next((item for item in history if item.step_index == k), None) if frame_data is None: - print(f"Warning: Missing data for step {k} in history.") + print(f"Warning: Missing data for step {k} in history. Skipping frame.") + # Optionally create an empty frame or reuse previous frame's data + # For simplicity, we skip here, which might cause jumps in animation continue - # Pass the engine choice to the content generator - frame_traces, frame_shapes, layout_image = generate_frame_content( + # Generate dynamic content for this specific frame + frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content( frame_data, X, L, @@ -427,31 +485,23 @@ def create_slam_animation( verbose_cov_errors, ) - if k == 0: - initial_dynamic_traces, initial_shapes, initial_image = ( - frame_traces, - frame_shapes, - layout_image, - ) - + # IMPORTANT: Combine static GT traces with the dynamic traces for THIS frame + # The 'data' in the frame replaces the figure's data for that step. frames.append( go.Frame( - data=frame_traces, + data=gt_traces + frame_dynamic_traces, # Include GT in every frame name=str(k), - layout=go.Layout( + layout=go.Layout( # Only update shapes and images in layout per frame shapes=frame_shapes, images=[layout_image] if layout_image else [] ), ) ) - # 3. Add initial dynamic traces (Unchanged) - for trace in initial_dynamic_traces: - fig.add_trace(trace) - - # 4. Assign frames (Unchanged) + # 5. Assign frames to the figure fig.update(frames=frames) - # 5. Configure layout (Unchanged) + # 6. Configure overall layout (sliders, buttons, axes, etc.) + # Pass the shapes and image corresponding to the INITIAL state (k=0) configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.") diff --git a/python/gtsam/examples/simulation.py b/python/gtsam/examples/simulation.py index a36433878..66beaa1ba 100644 --- a/python/gtsam/examples/simulation.py +++ b/python/gtsam/examples/simulation.py @@ -103,7 +103,7 @@ def generate_simulation_data( # Check sensor limits (range and Field of View - e.g. +/- 45 degrees) if ( true_range <= max_sensor_range - and abs(true_bearing.theta()) < np.pi / 4 + and abs(true_bearing.theta()) < np.pi / 2 ): # Sample noise noise_vec = measurement_noise_sampler.sample() From 17bf752576c33d2b28c0c30140c3eb7274dad526 Mon Sep 17 00:00:00 2001 From: jenniferoum Date: Fri, 25 Apr 2025 07:08:14 -0700 Subject: [PATCH 23/38] Template on size_t, Darshan's updates to cleanup comments, default coordinate to exponential, separate filter and demo specific functions, rename stateAction to operator *, fix brace initialization --- examples/ABC.h | 259 +++++++++++ examples/ABC_EQF.h | 872 ++++++++++++-------------------------- examples/ABC_EQF_Demo.cpp | 736 ++++++++++++++++---------------- 3 files changed, 895 insertions(+), 972 deletions(-) create mode 100644 examples/ABC.h diff --git a/examples/ABC.h b/examples/ABC.h new file mode 100644 index 000000000..a7c029c98 --- /dev/null +++ b/examples/ABC.h @@ -0,0 +1,259 @@ +/** + * @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 index 4c935962c..02b8dd4b9 100644 --- a/examples/ABC_EQF.h +++ b/examples/ABC_EQF.h @@ -2,290 +2,60 @@ * @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 + * 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 +#include +#include +#include // For std::accumulate #include #include -#include -#include -#include -#include // For std::accumulate + +#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; -// Global configuration -// Define coordinate type: "EXPONENTIAL" or "NORMAL" -extern const std::string COORDINATE; - -//======================================================================== -// 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); - -/// Calculate numerical differential - -Matrix numericalDifferential(std::function f, const Vector& x); - -//======================================================================== -// 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); - } - static Input create(const Vector3& w, const Matrix& Sigma) { /// Initialize w and Sigma - if (Sigma.rows() != 6 || Sigma.cols() != 6) { - throw std::invalid_argument("Input measurement noise covariance must be 6x6"); - } - /// 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"); - } - - return Input{w, Sigma}; // use brace initialization - } -}; - -/// 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) - static Measurement create(const Unit3& y_vec, const Unit3& d_vec, /// Initialize measurement - const Matrix3& Sigma_in, int i) { - /// Check positive semi-definite - Eigen::SelfAdjointEigenSolver eigensolver(Sigma_in); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument("Covariance matrix must be semi-positive definite"); - } - return Measurement{y_vec, Unit3(d_vec), Sigma_in, i}; // Brace initialization - } -}; - -/// State class representing the state of the Biased Attitude System - -class State { -public: - Rot3 R; // Attitude rotation matrix R - Vector3 b; // Gyroscope bias b - std::vector S; // Sensor calibrations S - - /// Constructor - State(const Rot3& R = Rot3::Identity(), - const Vector3& b = Vector3::Zero(), - const std::vector& S = std::vector()) - : R(R), b(b), S(S) {} - - /// Identity function - static State identity(int n) { - std::vector calibrations(n, Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), 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(6 + 3 * S.size()); - - // 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 < S.size(); 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() < 6 || v.size() % 3 != 0 || v.size() != 6 + 3 * static_cast(S.size())) { - throw std::invalid_argument("Vector size does not match state dimensions"); - } - - // Modify attitude - Rot3 newR = R * Rot3::Expmap(v.head<3>()); - - // Modify bias - Vector3 newB = b + v.segment<3>(3); - - // Modify calibrations - std::vector newS; - for (size_t i = 0; i < S.size(); i++) { - newS.push_back(S[i] * Rot3::Expmap(v.segment<3>(6 + 3*i))); - } - - return State(newR, newB, newS); - } -}; - -/// Data structure for ground-truth, input and output data - -struct Data { - State xi; /// Ground-truth state - int n_cal; /// Number of calibration states - Input u; /// Input measurements - std::vector y; /// Output measurements - int n_meas; /// Number of measurements - double t; /// Time - double dt; /// Time step - - static Data create(const State& xi, int n_cal, const Input& u, /// Initialize Data - const std::vector& y, int n_meas, - double t, double dt) { - return Data{xi, n_cal, u, y, n_meas, t, dt}; /// Bracket initialization - } -}; - -//======================================================================== -// 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 - */ -class G { -public: - Rot3 A; /// First SO(3) element - Matrix3 a; /// so(3) element (skew-symmetric matrix) - std::vector 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::vector& B = std::vector()) - : A(A), a(a), B(B) {} - - /// Group multiplication - G operator*(const G& other) const { - if (B.size() != other.B.size()) { - throw std::invalid_argument("Group elements must have the same number of calibration elements"); - } - - std::vector new_B; - for (size_t i = 0; i < B.size(); i++) { - new_B.push_back(B[i] * other.B[i]); - } - - return G(A * other.A, - a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), - new_B); - } - - /// Group inverse - G inv() const { - Matrix3 A_inv = A.inverse().matrix(); - std::vector B_inv; - for (const auto& b : B) { - B_inv.push_back(b.inverse()); - } - - return G(A.inverse(), - -Rot3::Hat(A_inv * Rot3::Vee(a)), - B_inv); - } - - /// Identity element - static G identity(int n) { - std::vector B(n, 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() < 6 || x.size() % 3 != 0) { - throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); - } - - int n = (x.size() - 6) / 3; - 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::vector B; - for (int i = 0; i < n; i++) { - B.push_back(Rot3::Expmap(x.segment<3>(6 + 3 * i))); - } - - return G(A, a, B); - } -}; - //======================================================================== // 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 */ -Vector lift(const State& xi, const Input& u); +template +Vector lift(const State& xi, const Input& u); /** * Action of the symmetry group on the state space (Equation 4) @@ -293,7 +63,8 @@ Vector lift(const State& xi, const Input& u); * @param xi State * @return New state after group action */ -State stateAction(const G& X, const State& xi); +template +State operator*(const G& X, const State& xi); /** * Action of the symmetry group on the input space (Equation 5) @@ -301,7 +72,8 @@ State stateAction(const G& X, const State& xi); * @param u Input * @return New input after group action */ -Input velocityAction(const G& X, const Input& u); +template +Input velocityAction(const G& X, const Input& u); /** * Action of the symmetry group on the output space (Equation 6) @@ -310,188 +82,96 @@ Input velocityAction(const G& X, const Input& u); * @param idx Calibration index * @return New direction after group action */ -Vector3 outputAction(const G& X, const Unit3& y, int idx); +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 */ -Matrix stateActionDiff(const State& xi); +template +Matrix stateActionDiff(const State& xi); //======================================================================== // Equivariant Filter (EqF) //======================================================================== /// Equivariant Filter (EqF) implementation - +template class EqF { -private: - int dof; // Degrees of freedom - int n_cal; // Number of calibration states - 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 + 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 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 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 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 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; + /** + * 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 n Number of calibration states - * @param m Number of sensors - */ - EqF(const Matrix& Sigma, int n, int m); + 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; + /** + * 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); + /** + * 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); + /** + * Update the filter state with a measurement + * @param y Direction measurement + */ + void update(const Measurement& y); }; -// Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Denotes how the states are mapped to the vector representations - -//======================================================================== -// Utility Functions -//======================================================================== -/** - * @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 - * Uses Vector3 norm() method to calculate vector magnitude - */ -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 - * Uses Matrix's rows(), cols(), setZero(), and block() methods - */ - -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 - * Recursively uses blockDiag() function - */ -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; -} - -/** - * @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; -} - //======================================================================== // Helper Functions Implementation //======================================================================== @@ -503,22 +183,22 @@ Matrix numericalDifferential(std::function f, const Vecto * @return Lifted input in Lie Algebra * Uses Vector zero & Rot3 inverse, matrix functions */ -Vector lift(const State& xi, const Input& u) { - int n = xi.S.size(); - Vector L = Vector::Zero(6 + 3 * n); +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; + // First 3 elements + L.head<3>() = u.w - xi.b; - // Next 3 elements - L.segment<3>(3) = -u.W() * xi.b; + // Next 3 elements + L.segment<3>(3) = -u.W() * xi.b; - // Remaining elements - for (int i = 0; i < n; i++) { - L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>(); - } + // 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; + return L; } /** * Implements group actions on the states @@ -533,19 +213,16 @@ Vector lift(const State& xi, const Input& u) { * @return Transformed state * Uses the Rot3 inverse and Vee functions */ -State stateAction(const G& X, const State& xi) { - if (xi.S.size() != X.B.size()) { - throw std::invalid_argument("Number of calibration states and B elements must match"); - } +template +State operator*(const G& X, const State& xi) { + std::array new_S; - std::vector new_S; - for (size_t i = 0; i < X.B.size(); i++) { - new_S.push_back(X.A.inverse() * xi.S[i] * X.B[i]); - } + 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); + 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 @@ -555,8 +232,9 @@ State stateAction(const G& X, const State& xi) { * Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining * the input equivariance */ -Input velocityAction(const G& X, const Input& u) { - return Input{X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma}; +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) @@ -566,64 +244,46 @@ Input velocityAction(const G& X, const Input& u) { * @return Transformed direction * Uses Rot3 inverse, matric and Unit3 unitvector functions */ -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(X.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return X.B[idx].inverse().matrix() * y.unitVector(); +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(); + } } /** - * Maps the error states to vector representations through exponential - * coordinates - * @param e error state - * @return Vector with local coordinates - * Uses Rot3 logamo for mapping rotations to the tangent space + * @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 */ -// Vector local_coords(const State& e) { -// if (COORDINATE == "EXPONENTIAL") { -// Vector eps(6 + 3 * e.S.size()); -// -// // First 3 elements -// eps.head<3>() = Rot3::Logmap(e.R); -// -// // Next 3 elements -// eps.segment<3>(3) = e.b; -// -// // Remaining elements -// for (size_t i = 0; i < e.S.size(); i++) { -// eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); -// } -// -// return eps; -// } else if (COORDINATE == "NORMAL") { -// throw std::runtime_error("Normal coordinate representation is not implemented yet"); -// } else { -// throw std::invalid_argument("Invalid coordinate representation"); -// } -// } -/** - * Used to convert the vectorized errors back to state space - * @param eps Local coordinates in the exponential parameterization - * @return State object corresponding to these coordinates - * Uses Rot3 expmap through the G::exp() function - */ -// State local_coords_inv(const Vector& eps) { -// G X = G::exp(eps); -// -// if (COORDINATE == "EXPONENTIAL") { -// std::vector S = X.B; -// return State(X.A, eps.segment<3>(3), S); -// } else if (COORDINATE == "NORMAL") { -// throw std::runtime_error("Normal coordinate representation is not implemented yet"); -// } else { -// throw std::invalid_argument("Invalid coordinate representation"); -// } -// } +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 @@ -632,17 +292,17 @@ Vector3 outputAction(const G& X, const Unit3& y, int idx) { * @return A matrix representing the jacobian of the state action * Uses numericalDifferential, and Rot3 expmap, logmap */ -Matrix stateActionDiff(const State& xi) { - std::function coordsAction = - [&xi](const Vector& U) { - G groupElement = G::exp(U); - State transformed = stateAction(groupElement, xi); - return xi.localCoordinates(transformed); - }; +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 * xi.S.size()); - Matrix differential = numericalDifferential(coordsAction, zeros); - return differential; + Vector zeros = Vector::Zero(6 + 3 * N); + Matrix differential = numericalDifferential(coordsAction, zeros); + return differential; } //======================================================================== @@ -656,38 +316,45 @@ Matrix stateActionDiff(const State& xi) { * @param m Number of sensors * Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse() */ -EqF::EqF(const Matrix& Sigma, int n, int m) - : dof(6 + 3 * n), n_cal(n), X_hat(G::identity(n)), - Sigma(Sigma), xi_0(State::identity(n)) { +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"); + } - 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"); + } - // 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 (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"); + } - 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(); + // 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 */ -State EqF::stateEstimate() const { - return stateAction(X_hat, xi_0); +template +State EqF::stateEstimate() const { + return X_hat * xi_0; } /** * Implements the prediction step of the EqF using system dynamics and @@ -697,19 +364,19 @@ State EqF::stateEstimate() const { * @param dt time steps * Updated internal state and covariance */ -void EqF::propagation(const Input& u, double dt) { - State state_est = stateEstimate(); - Vector L = lift(state_est, u); +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 Phi_DT = stateTransitionMatrix(u, dt); + Matrix Bt = inputMatrixBt(); - Matrix tempSigma = blockDiag(u.Sigma, - repBlock(1e-9 * Matrix3::Identity(), n_cal)); - Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt; + 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; + 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 @@ -718,8 +385,9 @@ void EqF::propagation(const Input& u, double dt) { * * @param y Measurements */ -void EqF::update(const Measurement& y) { - if (y.cal_idx > n_cal) { +template +void EqF::update(const Measurement& y) { + if (y.cal_idx > static_cast(N)) { throw std::invalid_argument("Calibration index out of range"); } @@ -740,7 +408,7 @@ void EqF::update(const Measurement& y) { 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; + X_hat = G::exp(Delta) * X_hat; Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma; } /** @@ -749,20 +417,14 @@ void EqF::update(const Measurement& y) { * @return Linearized state matrix * Uses Matrix zero and Identity functions */ -Matrix EqF::stateMatrixA(const Input& u) const { - Matrix3 W0 = velocityAction(X_hat.inv(), u).W(); - Matrix A1 = Matrix::Zero(6, 6); - - if (COORDINATE == "EXPONENTIAL") { - A1.block<3, 3>(0, 3) = -Matrix3::Identity(); - A1.block<3, 3>(3, 3) = W0; - Matrix A2 = repBlock(W0, n_cal); - return blockDiag(A1, A2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } +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); } /** @@ -771,49 +433,35 @@ Matrix EqF::stateMatrixA(const Input& u) const { * @param dt time step * @return State transition matrix in discrete time */ -Matrix EqF::stateTransitionMatrix(const Input& u, double dt) const { - Matrix3 W0 = velocityAction(X_hat.inv(), u).W(); - Matrix Phi1 = Matrix::Zero(6, 6); +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 * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); - Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; + Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0); + Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0; - if (COORDINATE == "EXPONENTIAL") { - Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); - Phi1.block<3, 3>(0, 3) = Phi12; - Phi1.block<3, 3>(3, 3) = Phi22; - Matrix Phi2 = repBlock(Phi22, n_cal); - return blockDiag(Phi1, Phi2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } + 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 */ -Matrix EqF::inputMatrixBt() const { - if (COORDINATE == "EXPONENTIAL") { - Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); - Matrix B2; +template +Matrix EqF::inputMatrixBt() const { + Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); + Matrix B2(3 * N, 3 * N); - for (const auto& B : X_hat.B) { - if (B2.size() == 0) { - B2 = B.matrix(); - } else { - B2 = blockDiag(B2, B.matrix()); - } - } + 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); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } + return blockDiag(B1, B2); } /** * Computes the linearized measurement matrix. The structure depends on whether @@ -823,45 +471,49 @@ Matrix EqF::inputMatrixBt() const { * @return Measurement matrix * Uses the matrix zero, Rot3 hat and the Unitvector functions */ -Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * n_cal); +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()); - } + // 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()); + Matrix3 wedge_d = Rot3::Hat(d.unitVector()); - // Create the combined matrix - Matrix temp(3, 6 + 3 * n_cal); - temp.block<3, 3>(0, 0) = wedge_d; - temp.block<3, 3>(0, 3) = Matrix3::Zero(); - temp.block(0, 6, 3, 3 * n_cal) = Cc; + // 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; + return wedge_d * temp; } /** * Computes the measurement uncertainty propagation matrix * @param idx Calibration index * @return Returns B[idx] for calibrated sensors, A for uncalibrated */ -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(X_hat.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return X_hat.B[idx].matrix(); - } else { - return X_hat.A.matrix(); +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 - -} // namespace abc_eqf_lib - -#endif // ABC_EQF_H \ No newline at end of file +#endif // ABC_EQF_H \ No newline at end of file diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp index 13b5a7da7..99b18d85f 100644 --- a/examples/ABC_EQF_Demo.cpp +++ b/examples/ABC_EQF_Demo.cpp @@ -3,17 +3,33 @@ * @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 + * 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 */ #include "ABC_EQF.h" // Use namespace for convenience -using namespace abc_eqf_lib; 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; + +/// 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 +}; //======================================================================== // Data Processing Functions @@ -31,402 +47,398 @@ using namespace gtsam; * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements - * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations + * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction + * measurement standard deviations * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions * - * @param filename Path to the CSV file - * @param startRow First row to load (default: 0) - * @param maxRows Maximum number of rows to load (default: all) - * @param downsample Downsample factor (default: 1, which means no downsampling) - * @return Vector of Data objects */ -std::vector loadDataFromCSV(const std::string& filename, - int startRow = 0, - int maxRows = -1, - int downsample = 1); +std::vector loadDataFromCSV(const std::string& filename, int startRow = 0, + int maxRows = -1, int downsample = 1); -/** - * Process data with EqF and print summary results - * @param filter Initialized EqF filter - * @param data_list Vector of Data objects to process - * @param printInterval Progress indicator interval (used internally) - */ -void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval = 10); +/// Process data with EqF and print summary results +void processDataWithEqF(EqFilter& filter, const std::vector& data_list, + int printInterval = 10); //======================================================================== // Data Processing Functions Implementation //======================================================================== -/** - * @brief Loads the test data from the csv file - * @param filename path to the csv file is specified - * @param startRow First row to load based on csv, 0 by default - * @param maxRows maximum rows to load, defaults to all rows - * @param downsample Downsample factor, default 1 - * @return A list of data objects -*/ +/* + * Loads the test data from the csv file + * startRow First row to load based on csv, 0 by default + * maxRows maximum rows to load, defaults to all rows + * downsample Downsample factor, default 1 + * A list of data objects + */ +std::vector loadDataFromCSV(const std::string& filename, int startRow, + int maxRows, int downsample) { + std::vector data_list; + std::ifstream file(filename); + if (!file.is_open()) { + throw std::runtime_error("Failed to open file: " + filename); + } -std::vector loadDataFromCSV(const std::string& filename, - int startRow, - int maxRows, - int downsample) { - std::vector data_list; - std::ifstream file(filename); + std::cout << "Loading data from " << filename << "..." << std::flush; - if (!file.is_open()) { - throw std::runtime_error("Failed to open file: " + filename); - } + std::string line; + int lineNumber = 0; + int rowCount = 0; + int errorCount = 0; + double prevTime = 0.0; - std::cout << "Loading data from " << filename << "..." << std::flush; + // Skip header + std::getline(file, line); + lineNumber++; - std::string line; - int lineNumber = 0; - int rowCount = 0; - int errorCount = 0; - double prevTime = 0.0; + // Skip to startRow + while (lineNumber < startRow && std::getline(file, line)) { + lineNumber++; + } - // Skip header - std::getline(file, line); + // Read data + while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { lineNumber++; - // Skip to startRow - while (lineNumber < startRow && std::getline(file, line)) { - lineNumber++; + // Apply downsampling + if ((lineNumber - startRow - 1) % downsample != 0) { + continue; } - // Read data - while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { - lineNumber++; + std::istringstream ss(line); + std::string token; + std::vector values; - // Apply downsampling - if ((lineNumber - startRow - 1) % downsample != 0) { - continue; - } - - std::istringstream ss(line); - std::string token; - std::vector values; - - // Parse line into values - while (std::getline(ss, token, ',')) { - try { - values.push_back(std::stod(token)); - } catch (const std::exception& e) { - errorCount++; - values.push_back(0.0); // Use default value - } - } - - // Check if we have enough values - if (values.size() < 39) { - errorCount++; - continue; - } - - // Extract values - double t = values[0]; - double dt = (rowCount == 0) ? 0.0 : t - prevTime; - prevTime = t; - - // Create ground truth state - Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z - Rot3 R = Rot3(quat); - - Vector3 b(values[5], values[6], values[7]); - - Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z - std::vector S = {Rot3(calQuat)}; - - State xi(R, b, S); - - // Create input - Vector3 w(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}; - - // Create measurements - std::vector measurements; - - // First measurement (calibrated sensor, cal_idx = 0) - Vector3 y0(values[21], values[22], values[23]); - Vector3 d0(values[33], values[34], values[35]); - - // Normalize vectors if needed - if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); - if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); - - // Measurement covariance - Matrix3 covY0 = Matrix3::Zero(); - covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 - covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 - covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 - - // Create measurement - measurements.push_back(Measurement{Unit3(y0), Unit3(d0), covY0, 0}); - - // Second measurement (calibrated sensor, cal_idx = -1) - Vector3 y1(values[24], values[25], values[26]); - Vector3 d1(values[36], values[37], values[38]); - - // Normalize vectors if needed - if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); - if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); - - // Measurement covariance - Matrix3 covY1 = Matrix3::Zero(); - covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 - covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 - covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 - - // Create measurement - measurements.push_back(Measurement{Unit3(y1), Unit3(d1), covY1, -1}); - - // Create Data object and add to list - data_list.push_back(Data{xi, 1, u, measurements, 2, t, dt}); - - rowCount++; - - // Show loading progress every 1000 rows - if (rowCount % 1000 == 0) { - std::cout << "." << std::flush; - } + // Parse line into values + while (std::getline(ss, token, ',')) { + try { + values.push_back(std::stod(token)); + } catch (const std::exception& e) { + errorCount++; + values.push_back(0.0); // Use default value + } } - std::cout << " Done!" << std::endl; - std::cout << "Loaded " << data_list.size() << " data points"; - - if (errorCount > 0) { - std::cout << " (" << errorCount << " errors encountered)"; + // Check if we have enough values + if (values.size() < 39) { + errorCount++; + continue; } - std::cout << std::endl; + // Extract values + double t = values[0]; + double dt = (rowCount == 0) ? 0.0 : t - prevTime; + prevTime = t; - return data_list; -} -/** - * @brief Takes in the data and runs an EqF on it and reports the results - * @param filter Initialized EqF filter - * @param data_list std::vector - * @param printInterval Progress indicator - * Prints the performance statstics like average error etc - * Uses Rot3 between, logmap and rpy functions - */ -void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval) { - if (data_list.empty()) { - std::cerr << "No data to process" << std::endl; - return; + // Create ground truth state + Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z + Rot3 R = Rot3(quat); + + Vector3 b(values[5], values[6], values[7]); + + Quaternion calQuat(values[8], values[9], values[10], + values[11]); // w, x, y, z + std::array S = {Rot3(calQuat)}; + + M xi(R, b, S); + + // Create input + Vector3 w(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}; + + // Create measurements + std::vector measurements; + + // First measurement (calibrated sensor, cal_idx = 0) + Vector3 y0(values[21], values[22], values[23]); + Vector3 d0(values[33], values[34], values[35]); + + // Normalize vectors if needed + if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize(); + if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize(); + + // Measurement covariance + Matrix3 covY0 = Matrix3::Zero(); + covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2 + covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2 + covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2 + + // Create measurement + measurements.push_back(Measurement{Unit3(y0), Unit3(d0), covY0, 0}); + + // Second measurement (calibrated sensor, cal_idx = -1) + Vector3 y1(values[24], values[25], values[26]); + Vector3 d1(values[36], values[37], values[38]); + + // Normalize vectors if needed + if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); + if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); + + // Measurement covariance + Matrix3 covY1 = Matrix3::Zero(); + covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 + covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 + covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 + + // Create measurement + 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}); + + rowCount++; + + // Show loading progress every 1000 rows + if (rowCount % 1000 == 0) { + std::cout << "." << std::flush; } + } - std::cout << "Processing " << data_list.size() << " data points with EqF..." << std::endl; + std::cout << " Done!" << std::endl; + std::cout << "Loaded " << data_list.size() << " data points"; - // Track performance metrics - std::vector att_errors; - std::vector bias_errors; - std::vector cal_errors; + if (errorCount > 0) { + std::cout << " (" << errorCount << " errors encountered)"; + } - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); + std::cout << std::endl; - int totalMeasurements = 0; - int validMeasurements = 0; - - // Define constant for converting radians to degrees - const double RAD_TO_DEG = 180.0 / M_PI; - - // Print a progress indicator - int progressStep = data_list.size() / 10; // 10 progress updates - if (progressStep < 1) progressStep = 1; - - std::cout << "Progress: "; - - for (size_t i = 0; i < data_list.size(); i++) { - const Data& data = data_list[i]; - - // Propagate filter with current input and time step - filter.propagation(data.u, data.dt); - - // Process all measurements - for (const auto& y : data.y) { - totalMeasurements++; - - // Skip invalid measurements - Vector3 y_vec = y.y.unitVector(); - Vector3 d_vec = y.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])) { - continue; - } - - try { - filter.update(y); - validMeasurements++; - } catch (const std::exception& e) { - std::cerr << "Error updating at t=" << data.t - << ": " << e.what() << std::endl; - } - } - - // Get current state estimate - State estimate = filter.stateEstimate(); - - // 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(); - if (!data.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); - } - - // Store errors - att_errors.push_back(att_error.norm()); - bias_errors.push_back(bias_error.norm()); - cal_errors.push_back(cal_error.norm()); - - // Show progress dots - if (i % progressStep == 0) { - std::cout << "." << std::flush; - } - } - - std::cout << " Done!" << std::endl; - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end - start; - - // Calculate average errors - double avg_att_error = 0.0; - double avg_bias_error = 0.0; - double avg_cal_error = 0.0; - - if (!att_errors.empty()) { - avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); - avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); - avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); - } - - // Calculate final errors from last data point - const Data& final_data = data_list.back(); - State final_estimate = filter.stateEstimate(); - 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(); - 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])); - } - - // Print summary statistics - std::cout << "\n=== Filter Performance Summary ===" << std::endl; - std::cout << "Processing time: " << elapsed.count() << " seconds" << std::endl; - std::cout << "Processed measurements: " << totalMeasurements << " (valid: " << validMeasurements << ")" << std::endl; - - // Average errors - std::cout << "\n-- Average Errors --" << std::endl; - std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl; - std::cout << "Bias: " << avg_bias_error << std::endl; - std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°" << std::endl; - - // Final errors - std::cout << "\n-- Final Errors --" << std::endl; - std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°" << std::endl; - std::cout << "Bias: " << final_bias_error.norm() << std::endl; - std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°" << std::endl; - - // Print a brief comparison of final estimate vs ground truth - std::cout << "\n-- Final State vs Ground Truth --" << std::endl; - std::cout << "Attitude (RPY) - Estimate: " - << (final_estimate.R.rpy() * RAD_TO_DEG).transpose() << "° | Truth: " - << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; - std::cout << "Bias - Estimate: " << final_estimate.b.transpose() - << " | Truth: " << final_data.xi.b.transpose() << std::endl; - - if (!final_estimate.S.empty() && !final_data.xi.S.empty()) { - std::cout << "Calibration (RPY) - Estimate: " - << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose() << "° | Truth: " - << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; - } + return data_list; } -/** - * Main function for the EqF demo - * @param argc Number of arguments - * @param argv Array of arguments - * @return Exit code - */ +/// Takes in the data and runs an EqF on it and reports the results +void processDataWithEqF(EqFilter& filter, const std::vector& data_list, + int printInterval) { + if (data_list.empty()) { + std::cerr << "No data to process" << std::endl; + return; + } + std::cout << "Processing " << data_list.size() << " data points with EqF..." + << std::endl; + + // Track performance metrics + std::vector att_errors; + std::vector bias_errors; + std::vector cal_errors; + + // Track time for performance measurement + auto start = std::chrono::high_resolution_clock::now(); + + int totalMeasurements = 0; + int validMeasurements = 0; + + // Define constant for converting radians to degrees + const double RAD_TO_DEG = 180.0 / M_PI; + + // Print a progress indicator + int progressStep = data_list.size() / 10; // 10 progress updates + if (progressStep < 1) progressStep = 1; + + std::cout << "Progress: "; + + for (size_t i = 0; i < data_list.size(); i++) { + const Data& data = data_list[i]; + + // Propagate filter with current input and time step + filter.propagation(data.u, data.dt); + + // Process all measurements + for (const auto& y : data.y) { + totalMeasurements++; + + // Skip invalid measurements + Vector3 y_vec = y.y.unitVector(); + Vector3 d_vec = y.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])) { + continue; + } + + try { + filter.update(y); + validMeasurements++; + } catch (const std::exception& e) { + std::cerr << "Error updating at t=" << data.t << ": " << e.what() + << std::endl; + } + } + + // Get current state estimate + M estimate = filter.stateEstimate(); + + // 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(); + if (!data.xi.S.empty() && !estimate.S.empty()) { + cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); + } + + // Store errors + att_errors.push_back(att_error.norm()); + bias_errors.push_back(bias_error.norm()); + cal_errors.push_back(cal_error.norm()); + + // Show progress dots + if (i % progressStep == 0) { + std::cout << "." << std::flush; + } + } + + std::cout << " Done!" << std::endl; + + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = end - start; + + // Calculate average errors + double avg_att_error = 0.0; + double avg_bias_error = 0.0; + double avg_cal_error = 0.0; + + if (!att_errors.empty()) { + avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / + att_errors.size(); + avg_bias_error = + std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / + bias_errors.size(); + avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / + cal_errors.size(); + } + + // Calculate final errors from last data point + const Data& final_data = data_list.back(); + M final_estimate = filter.stateEstimate(); + 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(); + 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])); + } + + // Print summary statistics + std::cout << "\n=== Filter Performance Summary ===" << std::endl; + std::cout << "Processing time: " << elapsed.count() << " seconds" + << std::endl; + std::cout << "Processed measurements: " << totalMeasurements + << " (valid: " << validMeasurements << ")" << std::endl; + + // Average errors + std::cout << "\n-- Average Errors --" << std::endl; + std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl; + std::cout << "Bias: " << avg_bias_error << std::endl; + std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°" + << std::endl; + + // Final errors + std::cout << "\n-- Final Errors --" << std::endl; + std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°" + << std::endl; + std::cout << "Bias: " << final_bias_error.norm() << std::endl; + std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°" + << std::endl; + + // Print a brief comparison of final estimate vs ground truth + std::cout << "\n-- Final State vs Ground Truth --" << std::endl; + std::cout << "Attitude (RPY) - Estimate: " + << (final_estimate.R.rpy() * RAD_TO_DEG).transpose() + << "° | Truth: " << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose() + << "°" << std::endl; + std::cout << "Bias - Estimate: " << final_estimate.b.transpose() + << " | Truth: " << final_data.xi.b.transpose() << std::endl; + + if (!final_estimate.S.empty() && !final_data.xi.S.empty()) { + std::cout << "Calibration (RPY) - Estimate: " + << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose() + << "° | Truth: " + << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°" + << std::endl; + } +} int main(int argc, char* argv[]) { - std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo" << std::endl; - std::cout << "==============================================================" << std::endl; + std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo" + << std::endl; + std::cout << "==============================================================" + << std::endl; - try { - // Parse command line options - std::string csvFilePath; - int maxRows = -1; // Process all rows by default - int downsample = 1; // No downsampling by default + try { + // Parse command line options + std::string csvFilePath; + int maxRows = -1; // Process all rows by default + int downsample = 1; // No downsampling by default - if (argc > 1) { - csvFilePath = argv[1]; - } else { - // Try to find the EQFdata file in the GTSAM examples directory - try { - csvFilePath = findExampleDataFile("EqFdata.csv"); - } catch (const std::exception& e) { - std::cerr << "Error: Could not find EqFdata.csv" << std::endl; - std::cerr << "Usage: " << argv[0] << " [csv_file_path] [max_rows] [downsample]" << std::endl; - return 1; - } - } - - // Optional command line parameters - if (argc > 2) { - maxRows = std::stoi(argv[2]); - } - - if (argc > 3) { - downsample = std::stoi(argv[3]); - } - - // Load data from CSV file - std::vector data = loadDataFromCSV(csvFilePath, 0, maxRows, downsample); - - if (data.empty()) { - std::cerr << "No data available to process. Exiting." << std::endl; - return 1; - } - - // Initialize the EqF filter with one calibration state - int n_cal = 1; - int n_sensors = 2; - - // Initial covariance - larger values allow faster convergence - Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); - 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 - - // Create filter - EqF filter(initialSigma, n_cal, n_sensors); - - // Process data - processDataWithEqF(filter, data); - - } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; + if (argc > 1) { + csvFilePath = argv[1]; + } else { + // Try to find the EQFdata file in the GTSAM examples directory + try { + csvFilePath = findExampleDataFile("EqFdata.csv"); + } catch (const std::exception& e) { + std::cerr << "Error: Could not find EqFdata.csv" << std::endl; + std::cerr << "Usage: " << argv[0] + << " [csv_file_path] [max_rows] [downsample]" << std::endl; return 1; + } } - std::cout << "\nEqF demonstration completed successfully." << std::endl; - return 0; + // Optional command line parameters + if (argc > 2) { + maxRows = std::stoi(argv[2]); + } + + if (argc > 3) { + downsample = std::stoi(argv[3]); + } + + // Load data from CSV file + std::vector data = + loadDataFromCSV(csvFilePath, 0, maxRows, downsample); + + if (data.empty()) { + std::cerr << "No data available to process. Exiting." << std::endl; + return 1; + } + + // Initialize the EqF filter with one calibration state + int n_sensors = 2; + + // Initial covariance - larger values allow faster convergence + 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) = + Vector3::Constant(0.01); // Bias uncertainty + initialSigma.diagonal().tail<3>() = + Vector3::Constant(0.1); // Calibration uncertainty + + // Create filter + EqFilter filter(initialSigma, n_sensors); + + // Process data + processDataWithEqF(filter, data); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return 1; + } + + std::cout << "\nEqF demonstration completed successfully." << std::endl; + return 0; } \ No newline at end of file From b07b77c40d4f1384e83dcf8a80f74167703d5a47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 09:43:05 -0400 Subject: [PATCH 24/38] Better plotting, true filter --- gtsam/nonlinear/nonlinear.i | 5 +- gtsam/symbolic/symbolic.md | 2 +- python/gtsam/examples/EKF_SLAM.ipynb | 16354 +++++++++++++++++++++++- python/gtsam/examples/gtsam_plotly.py | 209 +- 4 files changed, 16313 insertions(+), 257 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c13defbad..adde6d91d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -669,17 +669,17 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { }; #include +// This class is not available in python, just use a dictionary class FixedLagSmootherKeyTimestampMapValue { FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); }; +// This class is not available in python, just use a dictionary class FixedLagSmootherKeyTimestampMap { FixedLagSmootherKeyTimestampMap(); FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - // Note: no print function - // common STL methods size_t size() const; bool empty() const; @@ -739,6 +739,7 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { void print(string s = "IncrementalFixedLagSmoother:\n") const; + gtsam::Matrix marginalCovariance(size_t key) const; gtsam::ISAM2Params params() const; gtsam::NonlinearFactorGraph getFactors() const; diff --git a/gtsam/symbolic/symbolic.md b/gtsam/symbolic/symbolic.md index bfae60842..cc2f78d31 100644 --- a/gtsam/symbolic/symbolic.md +++ b/gtsam/symbolic/symbolic.md @@ -1,4 +1,4 @@ -# Symbolic Module +# Symbolic The `symbolic` module in GTSAM deals with the *structure* of factor graphs and Bayesian networks, independent of the specific numerical types of factors (like Gaussian or discrete). It allows for analyzing graph connectivity, determining optimal variable elimination orders, and understanding the sparsity structure of the resulting inference objects. diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 48a262f5e..fc3642728 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -2,15 +2,15 @@ "cells": [ { "cell_type": "markdown", - "id": "27cd96fc", + "id": "title-cell-fls", "metadata": {}, "source": [ - "# EKF-SLAM using Factor Graphs (SRIF-like)" + "# EKF-SLAM (using Incremental Fixed-Lag Smoother)" ] }, { "cell_type": "markdown", - "id": "copyright-cell-srif", + "id": "copyright-cell-fls", "metadata": { "tags": [ "remove-cell" @@ -27,7 +27,7 @@ }, { "cell_type": "markdown", - "id": "colab-button-cell-srif", + "id": "colab-button-cell-fls", "metadata": {}, "source": [ "\"Open" @@ -35,49 +35,19 @@ }, { "cell_type": "markdown", - "id": "intro-srif", + "id": "intro-fls-short", "metadata": {}, "source": [ - "This notebook demonstrates 2D SLAM using GTSAM's factor graph tools in an **iterative, filter-like manner**, similar in structure to a Square Root Information Filter (SRIF) or an EKF, but leveraging factor graph elimination instead of explicit matrix operations.\n", + "This notebook demonstrates 2D Simultaneous Localization and Mapping (SLAM) using an EKF, although it is implemented using GTSAM's `IncrementalFixedLagSmoother`, just using a lag of 1.\n", "\n", - "**Scenario:**\n", - "A robot moves in a circular path within a 10x10 meter area containing randomly placed landmarks. It receives noisy odometry and bearing-range measurements.\n", + "**Scenario:** A robot moves in a circular path, receiving noisy odometry and bearing-range measurements to landmarks.\n", "\n", - "**Factor Graph Filtering Approach:**\n", - "Instead of maintaining a large state vector and covariance matrix explicitly (like in classic EKF), we use GTSAM's `NonlinearFactorGraph` and `GaussianFactorGraph`:\n", - "\n", - "1. **Belief Representation:** The robot's belief (estimated state and uncertainty) after each step is implicitly represented by a factor graph (or the resulting Bayes Net after elimination).\n", - "2. **Prediction:**\n", - " * Start with the factors representing the belief from the *previous* step.\n", - " * Add a new `BetweenFactorPose2` representing the noisy odometry measurement, connecting the previous pose `X(k)` to the new pose `X(k+1)`.\n", - " * Linearize all relevant factors around the current best estimate (`Values`).\n", - " * Eliminate the resulting `GaussianFactorGraph` using an ordering that marginalizes out the previous pose `X(k)`, yielding a `GaussianBayesNet` representing the *predictive* distribution over landmarks and `X(k+1)`.\n", - "3. **Update:**\n", - " * Start with the factors representing the *predictive* belief.\n", - " * For each landmark measurement at the new pose `X(k+1)`:\n", - " * If the landmark `L(j)` is new, add it to the `Values` (initial estimate).\n", - " * Add a `BearingRangeFactor2D` connecting `X(k+1)` and `L(j)`.\n", - " * Linearize new/updated factors.\n", - " * Eliminate the `GaussianFactorGraph` to incorporate the measurement information, yielding a `GaussianBayesNet` representing the *posterior* belief.\n", - " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", - "4. **Iteration:** Repeat prediction and update for each time step.\n", - "\n", - "**Enhancements in this version:**\n", - "* Simulation code is moved to `simulation.py`.\n", - "* Plotting code is moved to `gtsam_plotly.py`.\n", - "* The evolving factor graph is displayed at each step using `graphviz`.\n", - "\n", - "**Advantages:**\n", - "* Leverages GTSAM's robust factor graph machinery.\n", - "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", - "* Avoids explicit management of large, dense covariance matrices.\n", - "\n", - "**Output:** The process will be visualized with the evolving factor graph, and finally using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + "**Approach:** We use a fixed-lag smoother which maintains and optimizes only a recent window of variables (defined by the `SMOOTHER_LAG`). Variables older than the lag are marginalized out, keeping the computational cost bounded, making it suitable for online applications. By default we set the lag to *1* here, which makes this an extended Kalman filter. But **feel free to change the lag and see the fixed-lag smoother results**." ] }, { "cell_type": "markdown", - "id": "setup-imports-srif", + "id": "setup-imports-fls", "metadata": {}, "source": [ "## 1. Setup and Imports" @@ -85,8 +55,8 @@ }, { "cell_type": "code", - "execution_count": null, - "id": "install-code-srif", + "execution_count": 1, + "id": "install-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -100,8 +70,8 @@ }, { "cell_type": "code", - "execution_count": null, - "id": "imports-code-srif", + "execution_count": 2, + "id": "imports-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -112,22 +82,28 @@ "import gtsam\n", "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", "\n", + "# Import the IncrementalFixedLagSmoother\n", + "from gtsam import IncrementalFixedLagSmoother\n", + "\n", + "# Helper modules\n", "import simulation\n", "from gtsam_plotly import SlamFrameData, create_slam_animation" ] }, { "cell_type": "markdown", - "id": "params-srif", + "id": "params-fls", "metadata": {}, "source": [ - "## 2. Simulation Parameters" + "## 2. Simulation and Smoother Parameters\n", + "\n", + "Define parameters for the simulation environment, robot motion, noise models, and the fixed-lag smoother." ] }, { "cell_type": "code", - "execution_count": null, - "id": "params-code-srif", + "execution_count": 9, + "id": "params-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -138,37 +114,51 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", - "DT = 1.0 # Time step duration (simplified)\n", + "NUM_STEPS = 50\n", + "DT = 1.0 # Time step duration\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", - "# Prior noise on the first pose (x, y, theta)\n", "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, np.deg2rad(1.0)]))\n", - "# Odometry noise (dx, dy, dtheta)\n", "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.05, np.deg2rad(2.0)]))\n", - "# Measurement noise (bearing, range)\n", "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0" + "MAX_SENSOR_RANGE = 5.0\n", + "\n", + "# --- Fixed-Lag Smoother Parameters ---\n", + "# Define the length of the smoothing window in seconds.\n", + "# A lag of 1*DT (e.g., 1.0 second) means the smoother maintains the state\n", + "# estimate for the current time step only, behaving like a filter.\n", + "# Larger lags incorporate more past information for smoothing.\n", + "SMOOTHER_LAG = 0.99 * DT\n" ] }, { "cell_type": "markdown", - "id": "ground-truth-srif", + "id": "ground-truth-fls", "metadata": {}, "source": [ "## 3. Generate Ground Truth Data\n", "\n", - "Use the `simulation` module to create true landmark positions, robot path, and simulate noisy measurements." + "Create the true environment, robot path, and simulate noisy sensor readings using the `simulation` module." ] }, { "cell_type": "code", - "execution_count": null, - "id": "ground-truth-call-srif", + "execution_count": 10, + "id": "ground-truth-call-fls", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Simulation Generated: 15 landmarks.\n", + "Simulation Generated: 51 ground truth poses and 50 odometry measurements.\n", + "Simulation Generated: 210 bearing-range measurements.\n" + ] + } + ], "source": [ "landmarks_gt_dict, poses_gt, odometry_measurements, measurements_sim, landmarks_gt_array = \\\n", " simulation.generate_simulation_data(\n", @@ -188,159 +178,16215 @@ }, { "cell_type": "markdown", - "id": "factor-graph-slam-impl", + "id": "smoother-impl", "metadata": {}, "source": [ - "## 4. Iterative Factor Graph SLAM Implementation\n", + "## 4. Fixed-Lag Smoother SLAM Implementation" + ] + }, + { + "cell_type": "markdown", + "id": "smoother-init", + "metadata": {}, + "source": [ + "### Initialize Smoother and Helper Functions\n", "\n", - "Here we perform the step-by-step prediction and update using factor graphs. The evolving graph will be displayed." + "We create the `IncrementalFixedLagSmoother` with the specified lag. We also initialize the first state (pose X(0) at time 0.0) and add it to the smoother using its `update` method. The `update` method requires factors, initial values (theta), and timestamps for the *new* variables being added." ] }, { "cell_type": "code", - "execution_count": null, - "id": "90bf9320", + "execution_count": 11, + "id": "smoother-init-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initializing IncrementalFixedLagSmoother with lag = 0.99 seconds...\n", + "Performing initial smoother update...\n", + "Initial update complete.\n" + ] + } + ], "source": [ + "# Helper for graphviz visualization\n", "WRITER = gtsam.GraphvizFormatting()\n", "WRITER.binaryEdges = True\n", "\n", - "def mage_dot(graph, estimate):\n", - " return graph.dot(estimate, writer=WRITER)\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "factor-graph-init-code", - "metadata": {}, - "outputs": [], - "source": [ - "# --- Initialization ---\n", - "current_estimate = gtsam.Values() # Stores current best estimates (linearization points)\n", - "current_graph = gtsam.NonlinearFactorGraph() # Stores factors added so far\n", - "current_pose_key = X(0)\n", + "def make_dot(graph, estimate):\n", + " # Visualize the factor graph currently managed by the smoother\n", + " WRITER.boxes = {key for key in estimate.keys() if gtsam.symbolChr(key) == ord('l')}\n", + " return graph.dot(estimate, writer=WRITER)\n", "\n", - "# Add prior on the initial pose X(0)\n", - "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", - "# Add a bit of noise to the initial guess if desired:\n", - "# initial_pose_noisy = initial_pose.compose(gtsam.Pose2(0.1, 0.1, 0.01))\n", - "# current_estimate.insert(current_pose_key, initial_pose_noisy)\n", - "current_estimate.insert(current_pose_key, initial_pose) \n", - "\n", - "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", + "# --- Smoother Initialization ---\n", + "print(f\"Initializing IncrementalFixedLagSmoother with lag = {SMOOTHER_LAG} seconds...\")\n", + "# Use ISAM2 parameters if specific settings are needed, otherwise defaults are used.\n", + "isam2_params = gtsam.ISAM2Params()\n", + "smoother = IncrementalFixedLagSmoother(SMOOTHER_LAG, isam2_params)\n", "\n", "# Variables to store results for animation\n", - "history = [] # Store SLAMFrameData objects for each step\n", - "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", + "history = []\n", "\n", - "# Initial marginals (only for X(0))\n", - "initial_gaussian_graph = current_graph.linearize(current_estimate)\n", - "initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", - "history.append(SlamFrameData(0, current_estimate, initial_marginals, mage_dot(current_graph, current_estimate)))" + "# --- Initial Step (k=0) ---\n", + "initial_pose_key = X(0)\n", + "initial_time = 0.0\n", + "initial_pose = poses_gt[0] # Start at ground truth (can add noise if desired)\n", + "\n", + "# Create containers for the first update\n", + "initial_factors = gtsam.NonlinearFactorGraph()\n", + "initial_values = gtsam.Values()\n", + "# The KeyTimestampMap maps variable keys (size_t) to their timestamps (double)\n", + "initial_timestamps = {}\n", + "\n", + "# Add prior factor for the first pose\n", + "initial_factors.add(gtsam.PriorFactorPose2(initial_pose_key, initial_pose, PRIOR_NOISE))\n", + "\n", + "# Add the initial pose estimate to Values\n", + "initial_values.insert(initial_pose_key, initial_pose)\n", + "\n", + "# Add the timestamp for the initial pose\n", + "initial_timestamps[initial_pose_key] = initial_time\n", + "\n", + "# Update the smoother with the initial state\n", + "print(\"Performing initial smoother update...\")\n", + "smoother.update(initial_factors, initial_values, initial_timestamps)\n", + "print(\"Initial update complete.\")\n", + "\n", + "# Store initial state for animation\n", + "current_estimate = smoother.calculateEstimate()\n", + "current_graph = smoother.getFactors() # Get factors currently managed by smoother\n", + "# ISAM can serve as marginals object:\n", + "current_marginals = smoother.getISAM2()\n", + "\n", + "history.append(SlamFrameData(0, current_estimate, current_marginals, make_dot(current_graph, current_estimate)))" ] }, { "cell_type": "markdown", - "id": "factor-graph-loop", + "id": "smoother-loop", "metadata": {}, "source": [ - "### Main Iterative Loop" + "### Main Iterative Loop\n", + "\n", + "At each step `k`, we process the odometry measurement from `X(k)` to `X(k+1)` and all landmark measurements taken *at* pose `X(k+1)`.\n", + "\n", + "1. **Prepare Data:** Collect new factors (`NonlinearFactorGraph`), initial estimates for *new* variables (`Values`), and timestamps for *new* variables (`KeyTimestampMap`) for the current step.\n", + "2. **Predict:** Calculate an initial estimate for the new pose `X(k+1)` based on the previous estimate `X(k)` (retrieved from the smoother) and the odometry measurement.\n", + "3. **Initialize Landmarks:** If a landmark is observed for the first time, calculate an initial estimate based on the predicted pose and the measurement, and add it to the `new_values` and `new_timestamps`.\n", + "4. **Update Smoother:** Call `smoother.update()` with the collected factors, values, and timestamps. This incorporates the new information, performs optimization (iSAM2), and marginalizes old variables.\n", + "5. **Store Results:** Retrieve the current estimate and factor graph from the smoother for visualization." ] }, { "cell_type": "code", - "execution_count": null, - "id": "factor-graph-loop-code", + "execution_count": 12, + "id": "smoother-loop-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Running Incremental Fixed-Lag Smoother SLAM loop (50 steps)...\n" + ] + }, + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "65c6048ecb324a96ac367007900bb015", + "version_major": 2, + "version_minor": 0 + }, + "text/plain": [ + " 0%| | 0/50 [00:00 str: - """Generates SVG path string for an ellipse directly from 2x2 covariance.""" - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite + """Generates SVG path string for an ellipse from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite 2x2 try: eigvals, eigvecs = np.linalg.eigh(cov) - # eigh sorts eigenvalues in ascending order eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues - minor_std, major_std = np.sqrt(eigvals) - v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] # Corresponding eigenvectors + minor_std, major_std = np.sqrt(eigvals) # eigh sorts ascending + v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] except np.linalg.LinAlgError: # Fallback to a small circle if decomposition fails radius = 0.1 * scale @@ -43,8 +42,7 @@ def create_ellipse_path_from_cov( x_p = cx + radius * np.cos(t) y_p = cy + radius * np.sin(t) else: - # Parametric equation: Center + scale * major_std * v_major * cos(t) + scale * minor_std * v_minor * sin(t) - # Note: We use scale*std which corresponds to half-axis lengths a,b used before + # Parametric equation using eigenvectors and eigenvalues t = np.linspace(0, 2 * np.pi, N) cos_t, sin_t = np.cos(t), np.sin(t) x_p = cx + scale * ( @@ -66,7 +64,9 @@ def create_ellipse_path_from_cov( # --- Plotly Element Generators --- -def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: +def create_gt_landmarks_trace( + landmarks_gt: Optional[np.ndarray], +) -> Optional[go.Scatter]: """Creates scatter trace for ground truth landmarks.""" if landmarks_gt is None or landmarks_gt.size == 0: return None @@ -79,7 +79,7 @@ def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: ) -def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: +def create_gt_path_trace(poses_gt: Optional[List[gtsam.Pose2]]) -> Optional[go.Scatter]: """Creates line trace for ground truth path.""" if not poses_gt: return None @@ -95,21 +95,36 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: def create_est_path_trace( est_path_x: List[float], est_path_y: List[float] ) -> go.Scatter: - """Creates trace for the estimated path.""" + """Creates trace for the estimated path (all poses up to current).""" return go.Scatter( x=est_path_x, y=est_path_y, mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), + line=dict(color="rgba(255, 0, 0, 0.3)", width=1), # Fainter line for history + marker=dict(size=4, color="red"), # Keep markers prominent name="Path Est", ) +def create_current_pose_trace( + current_pose: Optional[gtsam.Pose2], +) -> Optional[go.Scatter]: + """Creates a single marker trace for the current estimated pose.""" + if current_pose is None: + return None + return go.Scatter( + x=[current_pose.x()], + y=[current_pose.y()], + mode="markers", + marker=dict(color="magenta", size=10, symbol="cross"), + name="Current Pose", + ) + + def create_est_landmarks_trace( est_landmarks_x: List[float], est_landmarks_y: List[float] ) -> Optional[go.Scatter]: - """Creates trace for estimated landmarks.""" + """Creates trace for currently estimated landmarks.""" if not est_landmarks_x: return None return go.Scatter( @@ -122,7 +137,7 @@ def create_est_landmarks_trace( def _create_ellipse_shape_dict( - cx, cy, cov, scale, fillcolor, line_color + cx: float, cy: float, cov: np.ndarray, scale: float, fillcolor: str, line_color: str ) -> Dict[str, Any]: """Helper: Creates dict for a Plotly ellipse shape from covariance.""" path = create_ellipse_path_from_cov(cx, cy, cov, scale) @@ -133,6 +148,7 @@ def _create_ellipse_shape_dict( yref="y", fillcolor=fillcolor, line_color=line_color, + opacity=0.7, # Make ellipses slightly transparent ) @@ -145,8 +161,8 @@ def create_pose_ellipse_shape( cy=pose_mean_xy[1], cov=pose_cov, scale=scale, - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", + fillcolor="rgba(255,0,255,0.2)", # Magenta fill + line_color="rgba(255,0,255,0.5)", # Magenta line ) @@ -159,8 +175,8 @@ def create_landmark_ellipse_shape( cy=lm_mean_xy[1], cov=lm_cov, scale=scale, - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", + fillcolor="rgba(0,0,255,0.1)", # Blue fill + line_color="rgba(0,0,255,0.3)", # Blue line ) @@ -171,18 +187,12 @@ def dot_string_to_base64_svg( if not dot_string: return None try: - # Set DPI for potentially better default sizing, though less critical for SVG - # graph_attr = {'dpi': '150'} # You can experiment with this - source = graphviz.Source(dot_string, engine=engine) # , graph_attr=graph_attr) - # Use pipe() to get bytes directly without saving a file, format="svg" + source = graphviz.Source(dot_string, engine=engine) svg_bytes = source.pipe(format="svg") - # Encode bytes to base64 string, decode result to UTF-8 string encoded_string = base64.b64encode(svg_bytes).decode("utf-8") - # Return data URI for SVG return f"data:image/svg+xml;base64,{encoded_string}" except graphviz.backend.execute.CalledProcessError as e: print(f"Graphviz rendering error ({engine}): {e}") - # Optionally return a placeholder or raise return None except Exception as e: print(f"Unexpected error during Graphviz SVG generation: {e}") @@ -201,65 +211,77 @@ def generate_frame_content( ) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: """Generates dynamic traces, shapes, and layout image for a single frame.""" k = frame_data.step_index + # Use the results specific to this frame for current elements step_results = frame_data.results step_marginals = frame_data.marginals - frame_dynamic_traces: List[go.Scatter] = [] # Renamed for clarity + frame_dynamic_traces: List[go.Scatter] = [] frame_shapes: List[Dict[str, Any]] = [] layout_image: Optional[Dict[str, Any]] = None - # 1. Estimated Path - est_path_x = [ - step_results.atPose2(X(i)).x() - for i in range(k + 1) - if step_results.exists(X(i)) - ] - est_path_y = [ - step_results.atPose2(X(i)).y() - for i in range(k + 1) - if step_results.exists(X(i)) - ] - frame_dynamic_traces.append(create_est_path_trace(est_path_x, est_path_y)) + # 1. Estimated Path (Full History or Partial) + est_path_x = [] + est_path_y = [] + current_pose_est = None - # 2. Estimated Landmarks + # Plot poses currently existing in the step_results (e.g., within lag) + for i in range(k + 1): # Check poses up to current step index + pose_key = X(i) + if step_results.exists(pose_key): + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + if i == k: + current_pose_est = pose + + path_trace = create_est_path_trace(est_path_x, est_path_y) + if path_trace: + frame_dynamic_traces.append(path_trace) + + # Add a distinct marker for the current pose estimate + current_pose_trace = create_current_pose_trace(current_pose_est) + if current_pose_trace: + frame_dynamic_traces.append(current_pose_trace) + + # 2. Estimated Landmarks (Only those present in step_results) est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) + # Check existence in the results for the *current frame* if step_results.exists(lm_key): lm_point = step_results.atPoint2(lm_key) est_landmarks_x.append(lm_point[0]) est_landmarks_y.append(lm_point[1]) - landmark_keys.append(lm_key) + landmark_keys.append(lm_key) # Store keys for covariance lookup + lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: frame_dynamic_traces.append(lm_trace) - # 3. Covariance Ellipses + # 3. Covariance Ellipses (Only for items in step_results AND step_marginals) if step_marginals: + # Current Pose Ellipse pose_key = X(k) - if step_results.exists(pose_key): - try: - cov = step_marginals.marginalCovariance(pose_key) - mean = step_results.atPose2(pose_key).translation() - frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) - except RuntimeError as e: # More specific exception type - if verbose: - print(f"Warn: Pose {k} cov err @ step {k}: {e}") - except Exception as e: - if verbose: - print(f"Warn: Pose {k} cov OTHER err @ step {k}: {e}") + # Retrieve cov from marginals specific to this frame + cov = step_marginals.marginalCovariance(pose_key) + # Ensure mean comes from the pose in current results + mean = step_results.atPose2(pose_key).translation() + frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) + # Landmark Ellipses (Iterate over keys found in step_results) for lm_key in landmark_keys: try: + # Retrieve cov from marginals specific to this frame cov = step_marginals.marginalCovariance(lm_key) + # Ensure mean comes from the landmark in current results mean = step_results.atPoint2(lm_key) frame_shapes.append( create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) - except RuntimeError as e: # More specific exception type + except RuntimeError: # Covariance might not be available if verbose: print( - f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov not in marginals @ step {k}" ) except Exception as e: if verbose: @@ -267,9 +289,8 @@ def generate_frame_content( f"Warn: LM {gtsam.Symbol(lm_key).index()} cov OTHER err @ step {k}: {e}" ) - # 4. Graph Image for Layout (MODIFIED TO USE SVG) - # Use the SVG function with the dot string from frame_data - img_src = dot_string_to_base64_svg( # Changed function call + # 4. Graph Image for Layout + img_src = dot_string_to_base64_svg( frame_data.graph_dot_string, engine=graphviz_engine ) if img_src: @@ -284,10 +305,10 @@ def generate_frame_content( xanchor="left", yanchor="top", layer="below", - sizing="contain", # Contain should work well with SVG + sizing="contain", ) - # Return only the things that CHANGE frame-to-frame + # Return dynamic elements for this frame return frame_dynamic_traces, frame_shapes, layout_image @@ -336,7 +357,7 @@ def configure_figure_layout( x=plot_domain[0], xanchor="left", y=0, - yanchor="top", # Position relative to plot area + yanchor="top", buttons=[ dict( label="Play", @@ -371,35 +392,35 @@ def configure_figure_layout( title="Factor Graph SLAM Animation (Graph Left, Results Right)", xaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - domain=plot_domain, # Confine axis to right pane + domain=plot_domain, constrain="domain", ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], scaleanchor="x", scaleratio=1, - domain=[0, 1], # Full height within its domain + domain=[0, 1], ), width=1000, - height=600, # Adjust size for side-by-side + height=600, hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Initial shapes for the SLAM plot - images=( - [initial_image] if initial_image else [] - ), # Initial image for the left pane - showlegend=False, # Forego legend for space - # Autosize=True might help responsiveness but can interfere with domain + shapes=initial_shapes, # Initial shapes (frame 0) + images=([initial_image] if initial_image else []), # Initial image (frame 0) + showlegend=True, # Keep legend for clarity + legend=dict( + x=plot_domain[0], + y=1, + traceorder="normal", # Position legend + bgcolor="rgba(255,255,255,0.5)", + ), ) # --- Main Animation Orchestrator --- -# --- Main Animation Orchestrator (MODIFIED TO FIX GT DISPLAY) --- - - def create_slam_animation( history: List[SlamFrameData], X: Callable[[int], int], @@ -447,8 +468,7 @@ def create_slam_animation( verbose_cov_errors, ) - # 3. Add initial traces to the figure (GT first, then initial dynamic traces) - # These define the plot state BEFORE the animation starts or when slider is at 0 + # 3. Add initial traces (GT + dynamic frame 0) for trace in gt_traces: fig.add_trace(trace) for trace in initial_dynamic_traces: @@ -457,24 +477,12 @@ def create_slam_animation( # 4. Generate frames for the animation (k=0 to num_steps) frames = [] steps_iterable = range(num_steps + 1) - try: - steps_iterable = tqdm(steps_iterable, desc="Creating Frames") - except NameError: # Handle if tqdm is not available (e.g. standard Python script) - pass - except Exception as e: # Catch other potential tqdm issues - print( - f"Note: Could not initialize tqdm progress bar ({e}). Continuing without it." - ) + steps_iterable = tqdm(steps_iterable, desc="Creating Frames") for k in steps_iterable: frame_data = next((item for item in history if item.step_index == k), None) - if frame_data is None: - print(f"Warning: Missing data for step {k} in history. Skipping frame.") - # Optionally create an empty frame or reuse previous frame's data - # For simplicity, we skip here, which might cause jumps in animation - continue - # Generate dynamic content for this specific frame + # Generate dynamic content specific to this frame frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content( frame_data, X, @@ -485,14 +493,18 @@ def create_slam_animation( verbose_cov_errors, ) - # IMPORTANT: Combine static GT traces with the dynamic traces for THIS frame - # The 'data' in the frame replaces the figure's data for that step. + # Frame definition: includes static GT + dynamic traces for this step + # Layout updates only include shapes and images for this step frames.append( go.Frame( - data=gt_traces + frame_dynamic_traces, # Include GT in every frame + data=gt_traces + + frame_dynamic_traces, # GT must be in each frame's data name=str(k), - layout=go.Layout( # Only update shapes and images in layout per frame - shapes=frame_shapes, images=[layout_image] if layout_image else [] + layout=go.Layout( + shapes=frame_shapes, # Replaces shapes list for this frame + images=( + [layout_image] if layout_image else [] + ), # Replaces image list ), ) ) @@ -501,7 +513,6 @@ def create_slam_animation( fig.update(frames=frames) # 6. Configure overall layout (sliders, buttons, axes, etc.) - # Pass the shapes and image corresponding to the INITIAL state (k=0) configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.") From 8fbe3c9c8cdd4e52133cee0fab40cefe81de3a48 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 17:24:43 -0400 Subject: [PATCH 25/38] Fixed KarcherMean issue --- gtsam/linear/linear.i | 2 +- gtsam/slam/KarcherMeanFactor.h | 10 ++- gtsam/slam/doc/KarcherMeanFactor.ipynb | 91 +++++++++++--------------- gtsam/slam/slam.i | 5 +- 4 files changed, 49 insertions(+), 59 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index de55a5bb4..01a28511c 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -355,7 +355,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { gtsam::Vector error_vector(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const; - //Standard Interface + // Standard Interface gtsam::Matrix getA() const; gtsam::Vector getb() const; size_t rows() const; diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 6b25dd261..d81d15faa 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -27,13 +27,17 @@ namespace gtsam { /** * Optimize for the Karcher mean, minimizing the geodesic distance to each of - * the given rotations, by constructing a factor graph out of simple + * the given Lie groups elements, by constructing a factor graph out of simple * PriorFactors. */ template -T FindKarcherMean(const std::vector> &rotations); +typename std::enable_if::IsLieGroup, T>::type +FindKarcherMean(const std::vector> &elements); -template T FindKarcherMean(std::initializer_list &&rotations); +/// FindKarcherMean version from initializer list +template +typename std::enable_if::IsLieGroup, T>::type +FindKarcherMean(std::initializer_list &&elements); /** * The KarcherMeanFactor creates a constraint on all SO(n) variables with diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb index 973987286..64cd85506 100644 --- a/gtsam/slam/doc/KarcherMeanFactor.ipynb +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -15,12 +15,12 @@ "id": "desc_md" }, "source": [ - "This header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", + "The [KarcherMeanFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/KarcherMeanFactor.h) header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", "The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n", "$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n", "\n", "Functions/Classes:\n", - "* `FindKarcherMean(rotations)`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", + "* `FindKarcherMean`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", "* `KarcherMeanFactor`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables." ] }, @@ -35,7 +35,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -44,7 +44,11 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { @@ -57,10 +61,8 @@ "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import Rot3, FindKarcherMean, KarcherMeanFactorRot3, NonlinearFactorGraph, Values\n", - "from gtsam import symbol_shorthand\n", - "\n", - "R = symbol_shorthand.R" + "from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n", + "from gtsam.symbol_shorthand import R" ] }, { @@ -111,7 +113,7 @@ "rotations.append(Rot3.Yaw(0.12))\n", "\n", "# Compute the Karcher mean\n", - "karcher_mean = FindKarcherMean(rotations)\n", + "karcher_mean = FindKarcherMeanRot3(rotations)\n", "\n", "print(\"Input Rotations (Yaw angles):\")\n", "for r in rotations: print(f\" {r.yaw():.3f}\")\n", @@ -141,7 +143,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 6, "metadata": { "id": "factor_example_code" }, @@ -151,36 +153,22 @@ "output_type": "stream", "text": [ "KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n", + "sqrt(beta): 10.0\n", "\n", - "Linearized Factor (JacobianFactor):\n", - " A[r0] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " A[r1] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " A[r2] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " b = [ 0 0 0 ]\n", - " No noise model\n" - ] - }, - { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.JacobianFactor' object has no attribute 'find'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[9], line 20\u001b[0m\n\u001b[0;32m 18\u001b[0m sqrt_beta \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39msqrt(beta)\n\u001b[0;32m 19\u001b[0m expected_jacobian \u001b[38;5;241m=\u001b[39m sqrt_beta \u001b[38;5;241m*\u001b[39m np\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m3\u001b[39m)\n\u001b[1;32m---> 20\u001b[0m A0 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(\u001b[43mlinearized_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mfind\u001b[49m(R(\u001b[38;5;241m0\u001b[39m)))\n\u001b[0;32m 21\u001b[0m A1 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m1\u001b[39m)))\n\u001b[0;32m 22\u001b[0m A2 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m2\u001b[39m)))\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.JacobianFactor' object has no attribute 'find'" + "Jacobian for R(0):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Jacobian for R(1):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Jacobian for R(2):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Error vector b:\n", + "[0. 0. 0.]\n" ] } ], @@ -188,7 +176,7 @@ "keys = [R(0), R(1), R(2)]\n", "beta = 100.0 # Strength of the constraint\n", "\n", - "k_factor = KarcherMeanFactorRot3(keys)\n", + "k_factor = KarcherMeanFactorRot3(keys, 3, beta)\n", "k_factor.print(\"KarcherMeanFactorRot3: \")\n", "\n", "# Linearization example\n", @@ -198,32 +186,27 @@ "values.insert(R(2), Rot3.Yaw(0.3))\n", "\n", "linearized_factor = k_factor.linearize(values)\n", - "print(\"\\nLinearized Factor (JacobianFactor):\")\n", - "linearized_factor.print()\n", "\n", "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n", "sqrt_beta = np.sqrt(beta)\n", - "expected_jacobian = sqrt_beta * np.eye(3)\n", - "A0 = linearized_factor.getA(linearized_factor.find(R(0)))\n", - "A1 = linearized_factor.getA(linearized_factor.find(R(1)))\n", - "A2 = linearized_factor.getA(linearized_factor.find(R(2)))\n", + "A = linearized_factor.getA()\n", + "assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n", + "A0 = A[:, :3]\n", + "A1 = A[:, 3:6]\n", + "A2 = A[:, 6:9]\n", "b = linearized_factor.getb()\n", "\n", + "print(f\"sqrt(beta): {sqrt_beta}\")\n", "print(f\"\\nJacobian for R(0):\\n{A0}\")\n", "print(f\"Jacobian for R(1):\\n{A1}\")\n", "print(f\"Jacobian for R(2):\\n{A2}\")\n", - "print(f\"Error vector b:\\n{b}\")\n", - "print(f\"sqrt(beta): {sqrt_beta}\")\n", - "assert np.allclose(A0, expected_jacobian)\n", - "assert np.allclose(A1, expected_jacobian)\n", - "assert np.allclose(A2, expected_jacobian)\n", - "assert np.allclose(b, np.zeros(3))" + "print(f\"Error vector b:\\n{b}\")" ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -237,7 +220,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d8a2e49bf..50f2f0071 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -396,9 +396,12 @@ template virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); + KarcherMeanFactor(const gtsam::KeyVector& keys, int d, double beta); }; -gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); +template +T FindKarcherMean(const std::vector& elements); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 3e13c448024ca5303b3f768d18ce38f8b22c9df0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 17:52:46 -0400 Subject: [PATCH 26/38] Fix LAGO docs, add test --- gtsam/slam/doc/lago.ipynb | 183 ++++++++++++++++++-------------- gtsam/slam/slam.i | 1 + python/gtsam/tests/test_lago.py | 28 ++++- 3 files changed, 131 insertions(+), 81 deletions(-) diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb index aa6d0e98b..00f4c2b60 100644 --- a/gtsam/slam/doc/lago.ipynb +++ b/gtsam/slam/doc/lago.ipynb @@ -16,10 +16,10 @@ }, "source": [ "The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n", - "LAGO stands for Linear Approximation for Graph Optimization. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", + "LAGO stands for **Linear Approximation for Graph Optimization**. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", "\n", "The core idea is:\n", - "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", + "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", "2. **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n", "\n", "Key functions:\n", @@ -27,7 +27,9 @@ "* `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n", "* `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n", "\n", - "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose." + "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose.\n", + "\n", + "**Important Note**: LAGO expects integer keys numbered from 0 to n-1, with n the number of poses." ] }, { @@ -41,7 +43,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,7 +52,11 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { @@ -65,11 +71,8 @@ "import gtsam.utils.plot as gtsam_plot\n", "import matplotlib.pyplot as plt\n", "import numpy as np\n", - "from gtsam import NonlinearFactorGraph, Values, Pose2, Point3, PriorFactorPose2, BetweenFactorPose2\n", - "from gtsam import lago\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X" + "from gtsam import NonlinearFactorGraph, Pose2, PriorFactorPose2, BetweenFactorPose2\n", + "from gtsam import lago" ] }, { @@ -92,7 +95,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 8, "metadata": { "id": "example_pipeline_code" }, @@ -101,51 +104,26 @@ "name": "stdout", "output_type": "stream", "text": [ - "Original Factor Graph:\n", - "size: 6\n", "\n", - "Factor 0: PriorFactor on x0\n", - " prior mean: (0, 0, 0)\n", - " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "Initial Estimate from LAGO:\n", "\n", - "Factor 1: BetweenFactor(x0,x1)\n", - " measured: (2, 0, 0)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Values with 5 values:\n", + "Value 0: (gtsam::Pose2)\n", + "(-7.47244713e-17, -6.32592437e-17, -0.00193783525)\n", "\n", - "Factor 2: BetweenFactor(x1,x2)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Value 1: (gtsam::Pose2)\n", + "(1.70434147, -0.00881225307, 0.034656973)\n", "\n", - "Factor 3: BetweenFactor(x2,x3)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ + "Value 2: (gtsam::Pose2)\n", + "(3.40930145, 0.0555625509, 1.64569894)\n", "\n", - "Factor 4: BetweenFactor(x3,x4)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Value 3: (gtsam::Pose2)\n", + "(2.9638596, 2.05327873, 3.10897006)\n", "\n", - "Factor 5: BetweenFactor(x4,x0)\n", - " measured: (2.1, 0.1, 1.62079633)\n", - " noise model: diagonal sigmas [0.25; 0.25; 0.15];\n", + "Value 4: (gtsam::Pose2)\n", + "(0.669190885, 2.11357777, -1.71695927)\n", "\n" ] - }, - { - "ename": "IndexError", - "evalue": "invalid map key", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mIndexError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 25\u001b[0m\n\u001b[0;32m 22\u001b[0m graph\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOriginal Factor Graph:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;66;03m# 2. Perform LAGO initialization\u001b[39;00m\n\u001b[1;32m---> 25\u001b[0m initial_estimate_lago \u001b[38;5;241m=\u001b[39m \u001b[43mlago\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitialize\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mInitial Estimate from LAGO:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 28\u001b[0m initial_estimate_lago\u001b[38;5;241m.\u001b[39mprint()\n", - "\u001b[1;31mIndexError\u001b[0m: invalid map key" - ] } ], "source": [ @@ -155,72 +133,117 @@ "# Add a prior on the first pose\n", "prior_mean = Pose2(0.0, 0.0, 0.0)\n", "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n", - "graph.add(PriorFactorPose2(X(0), prior_mean, prior_noise))\n", + "graph.add(PriorFactorPose2(0, prior_mean, prior_noise))\n", "\n", "# Add odometry factors (simulating moving in a square)\n", "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", - "graph.add(BetweenFactorPose2(X(0), X(1), Pose2(2.0, 0.0, 0.0), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(1), X(2), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(2), X(3), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(3), X(4), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise))\n", + "graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", "\n", "# Add a loop closure factor\n", "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n", "# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n", "measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n", - "graph.add(BetweenFactorPose2(X(4), X(0), measured_loop, loop_noise))\n", - "\n", - "graph.print(\"Original Factor Graph:\\n\")\n", + "graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise))\n", "\n", "# 2. Perform LAGO initialization\n", "initial_estimate_lago = lago.initialize(graph)\n", "\n", "print(\"\\nInitial Estimate from LAGO:\\n\")\n", - "initial_estimate_lago.print()\n", - "\n", - "# 3. Visualize the LAGO estimate (optional)\n", + "initial_estimate_lago.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The block below visualizes the initial estimate computed by the LAGO algorithm. It uses the `gtsam_plot.plot_pose2` function to plot the poses in 2D space. Each pose is represented with its position and orientation, providing an intuitive way to inspect the initialization results. The visualization helps verify the correctness of the initial guess before proceeding with further optimization." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ "plt.figure(1)\n", "for key in initial_estimate_lago.keys():\n", " gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n", "plt.title(\"LAGO Initial Estimate\")\n", "plt.axis('equal')\n", - "# plt.show()\n", - "plt.close() # Close plot to prevent display in output\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, let's look at `lago.initializeOrientations` to compute the initial orientation estimatesThis solves a linear system, and the solution is represented as a `VectorValues` object, which stores the estimated angles for each pose as a 1-d vector.\n", "\n", - "# 4. Compare orientation initialization\n", + "We compare these orientation estimates with the orientations extracted from the full LAGO initialization (`lago.initialize`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "LAGO Orientations (VectorValues):\n", + "VectorValues: 6 elements\n", + " 0: -1.11022302e-16\n", + " 1: -0.008\n", + " 2: 1.55479633\n", + " 3: 3.11759265\n", + " 4: 4.68038898\n", + " 99999999: 0\n", + "Orientations from full LAGO Values:\n", + " 0: -0.0019\n", + " 1: 0.0347\n", + " 2: 1.6457\n", + " 3: 3.1090\n", + " 4: -1.7170\n" + ] + } + ], + "source": [ "initial_orientations = lago.initializeOrientations(graph)\n", "print(\"\\nLAGO Orientations (VectorValues):\")\n", "initial_orientations.print()\n", "\n", "print(\"Orientations from full LAGO Values:\")\n", "for i in range(5):\n", - " print(f\" X({i}): {initial_estimate_lago.atPose2(X(i)).theta():.4f}\")" + " print(f\" {i}: {initial_estimate_lago.atPose2(i).theta():.4f}\")" ] }, { "cell_type": "markdown", - "metadata": { - "id": "notes_header_md" - }, + "metadata": {}, "source": [ - "## Notes" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "notes_desc_md" - }, - "source": [ - "- LAGO provides a good initial guess, especially for orientations, which can significantly help the convergence of nonlinear optimizers.\n", - "- It assumes the graph structure allows for the orientation estimation (typically requires a spanning tree and a prior).\n", - "- The translation estimates are computed based on the fixed, estimated orientations." + "These are not as accurate (the last one is actually fine, it's $2\\pi$ off) but will still be good enough as an initial estimate." ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -234,7 +257,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 50f2f0071..3fdc31b30 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -467,6 +467,7 @@ typedef gtsam::TriangulationFactor> namespace lago { gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); + gtsam::VectorValues initializeOrientations(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); } } // namespace gtsam diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index 8ed5dd943..e538d8984 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point3, Pose2, PriorFactorPose2, Values +from gtsam import BetweenFactorPose2, Point3, Pose2, PriorFactorPose2, Values class TestLago(unittest.TestCase): @@ -33,6 +33,32 @@ class TestLago(unittest.TestCase): estimateLago: Values = gtsam.lago.initialize(graph) assert isinstance(estimateLago, Values) + def test_initialize2(self) -> None: + """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" + # 1. Create a NonlinearFactorGraph with Pose2 factors + graph = gtsam.NonlinearFactorGraph() + + # Add a prior on the first pose + prior_mean = Pose2(0.0, 0.0, 0.0) + prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + graph.add(PriorFactorPose2(0, prior_mean, prior_noise)) + + # Add odometry factors (simulating moving in a square) + odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise)) + graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + + # Add a loop closure factor + loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15])) + # Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2) + measured_loop = Pose2(2.1, 0.1, np.pi / 2 + 0.05) + graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) + if __name__ == "__main__": unittest.main() From fa23a888f957c82e012d168b2295de7f80a4736e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 18:02:40 -0400 Subject: [PATCH 27/38] Fixed PlanarProjectionFactor --- gtsam/slam/PlanarProjectionFactor.h | 2 +- gtsam/slam/doc/PlanarProjectionFactor.ipynb | 89 +++++++++------------ 2 files changed, 41 insertions(+), 50 deletions(-) diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index d53cd0ae1..dc7e53677 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -201,7 +201,7 @@ namespace gtsam { const Cal3DS2& calib, const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), - NoiseModelFactorN(model, landmarkKey, poseKey), + NoiseModelFactorN(model, poseKey, landmarkKey), bTc_(bTc), calib_(calib) {} diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb index 55d76252d..3100fbb53 100644 --- a/gtsam/slam/doc/PlanarProjectionFactor.ipynb +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -15,9 +15,9 @@ "id": "desc_md" }, "source": [ - "The `PlanarProjectionFactor` variants provide camera projection constraints specifically designed for scenarios where the robot or camera moves primarily on a 2D plane (e.g., ground robots with cameras).\n", + "The `PlanarProjectionFactor` variants provide camera projection factors specifically designed for scenarios where **the robot or camera moves primarily on a 2D plane** (e.g., ground robots with cameras).\n", "They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n", - "* The robot's 2D pose (`Pose2` `wTb`: world-to-body) in the ground plane.\n", + "* The robot's 2D pose (`Pose2` `wTb`: body in world frame) in the ground plane.\n", "* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n", "* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n", "* The 3D landmark position in the world frame.\n", @@ -41,7 +41,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,12 +50,16 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, @@ -65,13 +69,7 @@ "import numpy as np\n", "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L\n", - "C = symbol_shorthand.C # Calibration\n", - "O = symbol_shorthand.O # Offset" + "from gtsam.symbol_shorthand import X, L, C, O" ] }, { @@ -110,19 +108,19 @@ "isotropic dim=2 sigma=1.5\n", "\n", "Error at ground truth: 0.0\n", - "Error at noisy pose: 3317.647263749095\n" + "Error at noisy pose: 3317.6472637491106\n" ] } ], "source": [ "# Known parameters\n", "landmark_pt = Point3(2.0, 0.5, 0.5)\n", - "body_T_cam = Pose3(Rot3.Yaw(-np.pi/2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", + "body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", "calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n", - "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", + "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", "\n", "# Assume ground truth pose and calculate expected measurement\n", - "gt_pose2 = Pose2(1.0, 0.0, np.pi/4)\n", + "gt_pose2 = Pose2(1.0, 0.0, np.pi / 4)\n", "gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n", "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", "measured_pt2 = gt_camera.project(landmark_pt)\n", @@ -130,18 +128,19 @@ "print(f\"Calculated Measurement: {measured_pt2}\")\n", "\n", "# Create the factor\n", - "pose_key = X(0)\n", - "factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor1 = PlanarProjectionFactor1(\n", + " X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n", + ")\n", "factor1.print(\"Factor 1: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2) # Error should be zero here\n", + "values.insert(X(0), gt_pose2) # Error should be zero here\n", "error1_gt = factor1.error(values)\n", "print(f\"\\nError at ground truth: {error1_gt}\")\n", "\n", - "noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", - "values.update(pose_key, noisy_pose2)\n", + "noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n", + "values.update(X(0), noisy_pose2)\n", "error1_noisy = factor1.error(values)\n", "print(f\"Error at noisy pose: {error1_noisy}\")" ] @@ -175,37 +174,29 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 2: keys = { l0 x0 }\n", - "isotropic dim=2 sigma=1.5\n" - ] - }, - { - "ename": "RuntimeError", - "evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 10\u001b[0m\n\u001b[0;32m 8\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose2)\n\u001b[0;32m 9\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(landmark_key, landmark_pt) \u001b[38;5;66;03m# Error should be zero\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m error2_gt \u001b[38;5;241m=\u001b[39m \u001b[43mfactor2\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror2_gt\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 13\u001b[0m noisy_landmark \u001b[38;5;241m=\u001b[39m Point3(\u001b[38;5;241m2.1\u001b[39m, \u001b[38;5;241m0.45\u001b[39m, \u001b[38;5;241m0.55\u001b[39m)\n", - "\u001b[1;31mRuntimeError\u001b[0m: Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix" + "Factor 2: keys = { x0 l0 }\n", + "isotropic dim=2 sigma=1.5\n", + "\n", + "Error at ground truth: 0.0\n", + "Error with noisy landmark: 8066.192649473802\n" ] } ], "source": [ - "landmark_key = L(0)\n", - "\n", - "factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor2 = PlanarProjectionFactor2(\n", + " X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n", + ")\n", "factor2.print(\"Factor 2: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2)\n", - "values.insert(landmark_key, landmark_pt) # Error should be zero\n", + "values.insert(X(0), gt_pose2)\n", + "values.insert(L(0), landmark_pt) # Error should be zero\n", "error2_gt = factor2.error(values)\n", "print(f\"\\nError at ground truth: {error2_gt}\")\n", "\n", "noisy_landmark = Point3(2.1, 0.45, 0.55)\n", - "values.update(landmark_key, noisy_landmark)\n", + "values.update(L(0), noisy_landmark)\n", "error2_noisy = factor2.error(values)\n", "print(f\"Error with noisy landmark: {error2_noisy}\")" ] @@ -230,7 +221,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": { "id": "factor3_example_code" }, @@ -239,11 +230,11 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", - "Noise model: diagonal sigmas [1.5; 1.5];\n", + "Factor 3: keys = { x0 o0 c0 }\n", + "isotropic dim=2 sigma=1.5\n", "\n", - "Error at ground truth: [-0. -0.]\n", - "Error with noisy calibration: [8.38867847 0.63659684]\n" + "Error at ground truth: 0.0\n", + "Error with noisy calibration: 92.30212176019934\n" ] } ], @@ -251,12 +242,12 @@ "offset_key = O(0)\n", "calib_key = C(0)\n", "\n", - "factor3 = PlanarProjectionFactor3(pose_key, offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", + "factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", "factor3.print(\"Factor 3: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2)\n", + "values.insert(X(0), gt_pose2)\n", "values.insert(offset_key, body_T_cam)\n", "values.insert(calib_key, calib) # Error should be zero\n", "error3_gt = factor3.error(values)\n", @@ -271,7 +262,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -285,7 +276,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, From 25a2deb952de1c8afa4e4a9727306005ef753dad Mon Sep 17 00:00:00 2001 From: p-zach Date: Sat, 26 Apr 2025 20:14:48 -0400 Subject: [PATCH 28/38] Wrap OrientedPlane3 --- gtsam/geometry/geometry.i | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 3520ab140..b5f6f1f3f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -694,6 +694,45 @@ class Unit3 { bool equals(const gtsam::Unit3& expected, double tol) const; }; +#include +class OrientedPlane3 { + // Standard constructors + OrientedPlane3(); + OrientedPlane3(const gtsam::Unit3& n, double d); + OrientedPlane3(const gtsam::Vector& vec); + OrientedPlane3(double a, double b, double c, double d); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::OrientedPlane3& s, double tol = 1e-9) const; + + gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr) const; + gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr, + Eigen::Ref Hp, + Eigen::Ref Hr) const; + + gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other) const; + gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other, + Eigen::Ref H1, + Eigen::Ref H2) const; + + static size_t Dim(); + size_t dim() const; + + gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const; + gtsam::OrientedPlane3 retract(const gtsam::Vector3& v, + Eigen::Ref H) const; + + gtsam::Vector3 localCoordinates(const gtsam::OrientedPlane3& s) const; + + gtsam::Vector planeCoefficients() const; + + gtsam::Unit3 normal() const; + gtsam::Unit3 normal(Eigen::Ref H) const; + double distance() const; + double distance(Eigen::Ref H) const; +}; + #include class EssentialMatrix { // Standard Constructors From 43addb5ee91cdcfdd8d8cc97488c25a0311abf3d Mon Sep 17 00:00:00 2001 From: p-zach Date: Sat, 26 Apr 2025 20:15:54 -0400 Subject: [PATCH 29/38] Wrap many SLAM classes --- gtsam/slam/slam.i | 178 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 176 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d8a2e49bf..89c9943ce 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -162,12 +162,79 @@ enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; class SmartProjectionParams { SmartProjectionParams(); + SmartProjectionParams(gtsam::LinearizationMode linMode = gtsam::LinearizationMode::HESSIAN, + gtsam::DegeneracyMode degMode = gtsam::DegeneracyMode::IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false, double retriangulationTh = 1e-5); + void setLinearizationMode(gtsam::LinearizationMode linMode); void setDegeneracyMode(gtsam::DegeneracyMode degMode); void setRankTolerance(double rankTol); void setEnableEPI(bool enableEPI); void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); + + void print(const std::string& str = "") const; +}; + +template }> +class SmartProjectionFactor : gtsam::NonlinearFactor { + SmartProjectionFactor(); + + SmartProjectionFactor( + const gtsam::noiseModel::Base* sharedNoiseModel, + const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); + + void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; + + bool decideIfTriangulate(const gtsam::CameraSet& cameras) const; + gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet& cameras) const; + bool triangulateForLinearize(const gtsam::CameraSet& cameras) const; + + gtsam::HessianFactor createHessianFactor( + const gtsam::CameraSet& cameras, const double lambda = 0.0, + bool diagonalDamping = false) const; + gtsam::JacobianFactor createJacobianQFactor( + const gtsam::CameraSet& cameras, double lambda) const; + gtsam::JacobianFactor createJacobianQFactor( + const gtsam::Values& values, double lambda) const; + gtsam::JacobianFactor createJacobianSVDFactor( + const gtsam::CameraSet& cameras, double lambda) const; + gtsam::HessianFactor linearizeToHessian( + const gtsam::Values& values, double lambda = 0.0) const; + gtsam::JacobianFactor linearizeToJacobian( + const gtsam::Values& values, double lambda = 0.0) const; + + gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet& cameras, + const double lambda = 0.0) const; + + gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values, + const double lambda = 0.0) const; + + gtsam::GaussianFactor linearize( + const gtsam::Values& values) const; + + bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::CameraSet& cameras) const; + + bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::Values& values) const; + + gtsam::Vector reprojectionErrorAfterTriangulation(const gtsam::Values& values) const; + + double totalReprojectionError(const gtsam::CameraSet& cameras, + gtsam::Point3 externalPoint) const; + + double error(const gtsam::Values& values) const; + + gtsam::TriangulationResult point() const; + + gtsam::TriangulationResult point(const gtsam::Values& values) const; + + bool isValid() const; + bool isDegenerate() const; + bool isPointBehindCamera() const; + bool isOutlier() const; + bool isFarPoint() const; }; #include @@ -198,6 +265,42 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +// WIP +// #include +// typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +// #include +// template +// class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { +// SmartProjectionRigFactor(); + +// SmartProjectionRigFactor( +// const gtsam::noiseModel::Base* sharedNoiseModel, +// const gtsam::CameraSet& cameraRig, +// const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); + +// void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey, +// const size_t& cameraId = 0); + +// void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys, +// const gtsam::FastVector& cameraIds = gtsam::FastVector()); + +// const gtsam::KeyVector& nonUniqueKeys() const; + +// const gtsam::CameraSet& cameraRig() const; + +// const gtsam::FastVector& cameraIds() const; + +// void print( +// const std::string& s = "", +// const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + +// bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; + +// gtsam::CameraSet cameras(const gtsam::Values& values) const; + +// double error(const gtsam::Values& values) const; +// }; + #include template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { @@ -227,11 +330,72 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor { typedef gtsam::GenericStereoFactor GenericStereoFactor3D; +#include +template +class ReferenceFrameFactor : gtsam::NoiseModelFactor { + ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey, + gtsam::Key localKey, const gtsam::noiseModel::Base* model); + + gtsam::Vector evaluateError(const LANDMARK& global, const POSE& trans, const LANDMARK& local); + + void print(const std::string& s="", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +}; + +#include +class RotateFactor : gtsam::NoiseModelFactor { + RotateFactor(gtsam::Key key, const gtsam::Rot3& P, const gtsam::Rot3& Z, + const gtsam::noiseModel::Base* model); + + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + gtsam::Vector evaluateError(const gtsam::Rot3& R) const; +}; +class RotateDirectionsFactor : gtsam::NoiseModelFactor { + RotateDirectionsFactor(gtsam::Key key, const gtsam::Unit3& i_p, const gtsam::Unit3& c_z, + const gtsam::noiseModel::Base* model); + + static gtsam::Rot3 Initialize(const gtsam::Unit3& i_p, const gtsam::Unit3& c_z); + + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + gtsam::Vector evaluateError(const gtsam::Rot3& iRc) const; +}; + +#include +class OrientedPlane3Factor : gtsam::NoiseModelFactor { + OrientedPlane3Factor(); + OrientedPlane3Factor(const gtsam::Vector& z, const gtsam::noiseModel::Gaussian* noiseModel, + gtsam::Key poseKey, gtsam::Key landmarkKey); + + void print(const std::string& s = "OrientedPlane3Factor", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + gtsam::Vector evaluateError( + const gtsam::Pose3& pose, const gtsam::OrientedPlane3& plane) const; +}; +class OrientedPlane3DirectionPrior : gtsam::NoiseModelFactor { + OrientedPlane3DirectionPrior(); + OrientedPlane3DirectionPrior(gtsam::Key key, const gtsam::Vector& z, + const gtsam::noiseModel::Gaussian* noiseModel); + + void print(const std::string& s = "OrientedPlane3DirectionPrior", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const; + + gtsam::Vector evaluateError(const gtsam::OrientedPlane3& plane) const; +}; + #include template virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE::Translation& measured, + const gtsam::noiseModel::Base* model); PoseTranslationPrior(size_t key, const POSE& pose_z, - const gtsam::noiseModel::Base* noiseModel); + const gtsam::noiseModel::Base* model); POSE::Translation measured() const; // enabling serialization functionality @@ -244,8 +408,10 @@ typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; #include template virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE::Rotation& rot_z, + const gtsam::noiseModel::Base* model); PoseRotationPrior(size_t key, const POSE& pose_z, - const gtsam::noiseModel::Base* noiseModel); + const gtsam::noiseModel::Base* model); POSE::Rotation measured() const; }; @@ -404,6 +570,14 @@ gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, size_t d); +template +class FrobeniusPrior : gtsam::NoiseModelFactor { + FrobeniusPrior(gtsam::Key j, const gtsam::Matrix& M, + const gtsam::noiseModel::Base* model); + + gtsam::Vector evaluateError(const T& g) const; +}; + template virtual class FrobeniusFactor : gtsam::NoiseModelFactor { FrobeniusFactor(size_t key1, size_t key2); From 57dbbfd9e316694eef57c92714c159c5e34fcab7 Mon Sep 17 00:00:00 2001 From: p-zach Date: Sat, 26 Apr 2025 20:17:53 -0400 Subject: [PATCH 30/38] Fix SLAM notebook wrap problems and typos --- gtsam/slam/doc/FrobeniusFactor.ipynb | 54 +++--- gtsam/slam/doc/OrientedPlane3Factor.ipynb | 71 ++++---- gtsam/slam/doc/PoseRotationPrior.ipynb | 43 ++--- gtsam/slam/doc/PoseTranslationPrior.ipynb | 44 ++--- gtsam/slam/doc/ReferenceFrameFactor.ipynb | 18 ++ gtsam/slam/doc/RotateFactor.ipynb | 54 +++--- gtsam/slam/doc/SmartProjectionFactor.ipynb | 72 +++----- ...rams.ipynb => SmartProjectionParams.ipynb} | 72 ++++---- .../slam/doc/SmartProjectionPoseFactor.ipynb | 159 +++++++----------- gtsam/slam/doc/StereoFactor.ipynb | 74 ++++---- 10 files changed, 298 insertions(+), 363 deletions(-) rename gtsam/slam/doc/{SmartFactorParams.ipynb => SmartProjectionParams.ipynb} (57%) diff --git a/gtsam/slam/doc/FrobeniusFactor.ipynb b/gtsam/slam/doc/FrobeniusFactor.ipynb index cba959dc6..80d30c9ed 100644 --- a/gtsam/slam/doc/FrobeniusFactor.ipynb +++ b/gtsam/slam/doc/FrobeniusFactor.ipynb @@ -50,7 +50,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "imports_code" }, @@ -86,7 +86,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "fprior_example_code" }, @@ -95,12 +95,10 @@ "name": "stdout", "output_type": "stream", "text": [ - "FrobeniusPriorRot3: FrobeniusPriorFactor on R0\n", - "Error model: diagonal sigmas [0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01];\n", + "FrobeniusPriorRot3: keys = { r0 }\n", + " noise model: unit (9) \n", "\n", - "FrobeniusPrior error (vectorized matrix diff): (9,)\n", - "[ 0.00054931 -0.00997917 0. -0.00997917 -0.00054931 0.\n", - " 0. 0. 0. ]\n" + "FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\n" ] } ], @@ -121,7 +119,7 @@ "values = Values()\n", "values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n", "error_prior = prior_fro.error(values)\n", - "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior.shape}\\n{error_prior}\")" + "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior}\")" ] }, { @@ -144,7 +142,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 7, "metadata": { "id": "ffactor_example_code" }, @@ -155,18 +153,9 @@ "text": [ "\n", "FrobeniusFactorRot3: keys = { r0 r1 }\n", - " noise model: unit (9) \n" - ] - }, - { - "ename": "RuntimeError", - "evalue": "Attempting to at the key \"r0\", which does not exist in the Values.", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[6], line 13\u001b[0m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;66;03m# Evaluate error\u001b[39;00m\n\u001b[0;32m 12\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(key2, Rot3\u001b[38;5;241m.\u001b[39mYaw(\u001b[38;5;241m0.115\u001b[39m)) \u001b[38;5;66;03m# R1 slightly different from R0\u001b[39;00m\n\u001b[1;32m---> 13\u001b[0m error_factor \u001b[38;5;241m=\u001b[39m \u001b[43mfactor_fro\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mFrobeniusFactor error (vectorized matrix diff): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;241m.\u001b[39mshape\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"r0\", which does not exist in the Values." + " noise model: unit (9) \n", + "\n", + "FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n" ] } ], @@ -182,9 +171,10 @@ "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n", "\n", "# Evaluate error\n", + "values.insert(key1, Rot3.Yaw(0.11))\n", "values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n", "error_factor = factor_fro.error(values)\n", - "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor.shape}\\n{error_factor}\")" + "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor}\")" ] }, { @@ -207,7 +197,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 9, "metadata": { "id": "fbetween_example_code" }, @@ -217,17 +207,15 @@ "output_type": "stream", "text": [ "\n", - "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor(R0,R1)\n", - " T12: R: [\n", - "\t0.999875, -0.0149991, 0;\n", - "\t0.0149991, 0.999875, 0;\n", + "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor(r0,r1)\n", + " T12: [\n", + "\t0.999988, -0.00499998, 0;\n", + "\t0.00499998, 0.999988, 0;\n", "\t0, 0, 1\n", "]\n", + " noise model: unit (9) \n", "\n", - " noise model: diagonal sigmas [0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005];\n", - "\n", - "FrobeniusBetweenFactor error: (9,)\n", - "[-0. 0. 0. 0. -0. 0. 0. 0. 0.]\n" + "FrobeniusBetweenFactor error: 1.925929944387236e-34\n" ] } ], @@ -241,7 +229,7 @@ "\n", "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n", "error_between = between_fro.error(values)\n", - "print(f\"\\nFrobeniusBetweenFactor error: {error_between.shape}\\n{error_between}\")" + "print(f\"\\nFrobeniusBetweenFactor error: {error_between}\")" ] } ], @@ -261,7 +249,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.11.9" + "version": "3.13.1" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb index 1600674b2..7815b844f 100644 --- a/gtsam/slam/doc/OrientedPlane3Factor.ipynb +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -48,32 +48,17 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, OrientedPlane3, Point3, Rot3, Values\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m OrientedPlane3Factor, OrientedPlane3DirectionPrior\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", "from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n", "from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X\n", - "P = symbol_shorthand.P # For Plane" + "from gtsam.symbol_shorthand import X, P" ] }, { @@ -97,7 +82,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "factor_example_code" }, @@ -106,13 +91,21 @@ "name": "stdout", "output_type": "stream", "text": [ - "OrientedPlane3Factor: OrientedPlane3Factor Factor with keys: x0 P0\n", - "\n", - "Error model: diagonal sigmas [0.05; 0.05; 0.05];\n", - "\n", - "Error at ground truth: [-0. -0. -0.]\n", - "\n", - "Error with noisy plane: [ -0.0150837 -0.00503354 -49.83283361]\n" + "OrientedPlane3Factor: \n", + "OrientedPlane3Factor Factor (x0, p0)\n", + "Measured Plane : 0 0 1 1\n", + "isotropic dim=3 sigma=0.05\n" + ] + }, + { + "ename": "TypeError", + "evalue": "insert(): incompatible function arguments. The following argument types are supported:\n 1. (self: gtsam.gtsam.Values, values: gtsam.gtsam.Values) -> None\n 2. (self: gtsam.gtsam.Values, j: int, vector: numpy.ndarray[numpy.float64[m, 1]]) -> None\n 3. (self: gtsam.gtsam.Values, j: int, matrix: numpy.ndarray[numpy.float64[m, n]]) -> None\n 4. (self: gtsam.gtsam.Values, j: int, point2: numpy.ndarray[numpy.float64[2, 1]]) -> None\n 5. (self: gtsam.gtsam.Values, j: int, point3: numpy.ndarray[numpy.float64[3, 1]]) -> None\n 6. (self: gtsam.gtsam.Values, j: int, rot2: gtsam.gtsam.Rot2) -> None\n 7. (self: gtsam.gtsam.Values, j: int, pose2: gtsam.gtsam.Pose2) -> None\n 8. (self: gtsam.gtsam.Values, j: int, R: gtsam.gtsam.SO3) -> None\n 9. (self: gtsam.gtsam.Values, j: int, Q: gtsam.gtsam.SO4) -> None\n 10. (self: gtsam.gtsam.Values, j: int, P: gtsam.gtsam.SOn) -> None\n 11. (self: gtsam.gtsam.Values, j: int, rot3: gtsam.gtsam.Rot3) -> None\n 12. (self: gtsam.gtsam.Values, j: int, pose3: gtsam.gtsam.Pose3) -> None\n 13. (self: gtsam.gtsam.Values, j: int, similarity2: gtsam.gtsam.Similarity2) -> None\n 14. (self: gtsam.gtsam.Values, j: int, similarity3: gtsam.gtsam.Similarity3) -> None\n 15. (self: gtsam.gtsam.Values, j: int, unit3: gtsam.gtsam.Unit3) -> None\n 16. (self: gtsam.gtsam.Values, j: int, cal3bundler: gtsam.gtsam.Cal3Bundler) -> None\n 17. (self: gtsam.gtsam.Values, j: int, cal3f: gtsam.gtsam.Cal3f) -> None\n 18. (self: gtsam.gtsam.Values, j: int, cal3_s2: gtsam.gtsam.Cal3_S2) -> None\n 19. (self: gtsam.gtsam.Values, j: int, cal3ds2: gtsam.gtsam.Cal3DS2) -> None\n 20. (self: gtsam.gtsam.Values, j: int, cal3fisheye: gtsam.gtsam.Cal3Fisheye) -> None\n 21. (self: gtsam.gtsam.Values, j: int, cal3unified: gtsam.gtsam.Cal3Unified) -> None\n 22. (self: gtsam.gtsam.Values, j: int, E: gtsam.gtsam.EssentialMatrix) -> None\n 23. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.FundamentalMatrix) -> None\n 24. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.SimpleFundamentalMatrix) -> None\n 25. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Bundler) -> None\n 26. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3f) -> None\n 27. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3_S2) -> None\n 28. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3DS2) -> None\n 29. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Fisheye) -> None\n 30. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Unified) -> None\n 31. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Bundler) -> None\n 32. (self: gtsam.gtsam.Values, j: int, camera: gtsam::PinholePose) -> None\n 33. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3_S2) -> None\n 34. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3DS2) -> None\n 35. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Fisheye) -> None\n 36. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Unified) -> None\n 37. (self: gtsam.gtsam.Values, j: int, constant_bias: gtsam::imuBias::ConstantBias) -> None\n 38. (self: gtsam.gtsam.Values, j: int, nav_state: gtsam::NavState) -> None\n 39. (self: gtsam.gtsam.Values, j: int, c: float) -> None\n\nInvoked with: Values with 1 values:\nValue x0: (class gtsam::Pose3)\nR: [\n\t0.995004, -0.0998334, 0;\n\t0.0998334, 0.995004, 0;\n\t0, 0, 1\n]\nt: 0.5 0 0\n\n, 8070450532247928832, : 0 0 1 1\n", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[4], line 22\u001b[0m\n\u001b[0;32m 20\u001b[0m values \u001b[38;5;241m=\u001b[39m Values()\n\u001b[0;32m 21\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose)\n\u001b[1;32m---> 22\u001b[0m \u001b[43mvalues\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minsert\u001b[49m\u001b[43m(\u001b[49m\u001b[43mplane_key\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mgt_plane_world\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 23\u001b[0m error1 \u001b[38;5;241m=\u001b[39m plane_factor\u001b[38;5;241m.\u001b[39merror(values)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror1\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", + "\u001b[1;31mTypeError\u001b[0m: insert(): incompatible function arguments. The following argument types are supported:\n 1. (self: gtsam.gtsam.Values, values: gtsam.gtsam.Values) -> None\n 2. (self: gtsam.gtsam.Values, j: int, vector: numpy.ndarray[numpy.float64[m, 1]]) -> None\n 3. (self: gtsam.gtsam.Values, j: int, matrix: numpy.ndarray[numpy.float64[m, n]]) -> None\n 4. (self: gtsam.gtsam.Values, j: int, point2: numpy.ndarray[numpy.float64[2, 1]]) -> None\n 5. (self: gtsam.gtsam.Values, j: int, point3: numpy.ndarray[numpy.float64[3, 1]]) -> None\n 6. (self: gtsam.gtsam.Values, j: int, rot2: gtsam.gtsam.Rot2) -> None\n 7. (self: gtsam.gtsam.Values, j: int, pose2: gtsam.gtsam.Pose2) -> None\n 8. (self: gtsam.gtsam.Values, j: int, R: gtsam.gtsam.SO3) -> None\n 9. (self: gtsam.gtsam.Values, j: int, Q: gtsam.gtsam.SO4) -> None\n 10. (self: gtsam.gtsam.Values, j: int, P: gtsam.gtsam.SOn) -> None\n 11. (self: gtsam.gtsam.Values, j: int, rot3: gtsam.gtsam.Rot3) -> None\n 12. (self: gtsam.gtsam.Values, j: int, pose3: gtsam.gtsam.Pose3) -> None\n 13. (self: gtsam.gtsam.Values, j: int, similarity2: gtsam.gtsam.Similarity2) -> None\n 14. (self: gtsam.gtsam.Values, j: int, similarity3: gtsam.gtsam.Similarity3) -> None\n 15. (self: gtsam.gtsam.Values, j: int, unit3: gtsam.gtsam.Unit3) -> None\n 16. (self: gtsam.gtsam.Values, j: int, cal3bundler: gtsam.gtsam.Cal3Bundler) -> None\n 17. (self: gtsam.gtsam.Values, j: int, cal3f: gtsam.gtsam.Cal3f) -> None\n 18. (self: gtsam.gtsam.Values, j: int, cal3_s2: gtsam.gtsam.Cal3_S2) -> None\n 19. (self: gtsam.gtsam.Values, j: int, cal3ds2: gtsam.gtsam.Cal3DS2) -> None\n 20. (self: gtsam.gtsam.Values, j: int, cal3fisheye: gtsam.gtsam.Cal3Fisheye) -> None\n 21. (self: gtsam.gtsam.Values, j: int, cal3unified: gtsam.gtsam.Cal3Unified) -> None\n 22. (self: gtsam.gtsam.Values, j: int, E: gtsam.gtsam.EssentialMatrix) -> None\n 23. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.FundamentalMatrix) -> None\n 24. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.SimpleFundamentalMatrix) -> None\n 25. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Bundler) -> None\n 26. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3f) -> None\n 27. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3_S2) -> None\n 28. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3DS2) -> None\n 29. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Fisheye) -> None\n 30. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Unified) -> None\n 31. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Bundler) -> None\n 32. (self: gtsam.gtsam.Values, j: int, camera: gtsam::PinholePose) -> None\n 33. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3_S2) -> None\n 34. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3DS2) -> None\n 35. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Fisheye) -> None\n 36. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Unified) -> None\n 37. (self: gtsam.gtsam.Values, j: int, constant_bias: gtsam::imuBias::ConstantBias) -> None\n 38. (self: gtsam.gtsam.Values, j: int, nav_state: gtsam::NavState) -> None\n 39. (self: gtsam.gtsam.Values, j: int, c: float) -> None\n\nInvoked with: Values with 1 values:\nValue x0: (class gtsam::Pose3)\nR: [\n\t0.995004, -0.0998334, 0;\n\t0.0998334, 0.995004, 0;\n\t0, 0, 1\n]\nt: 0.5 0 0\n\n, 8070450532247928832, : 0 0 1 1\n" ] } ], @@ -126,7 +119,7 @@ "# Measurement: transform the world plane into the camera frame\n", "# measured_plane = gt_plane_world.transform(gt_pose)\n", "# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n", - "measured_plane_coeffs = gt_plane_world.coeffs()\n", + "measured_plane_coeffs = gt_plane_world.planeCoefficients()\n", "plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n", "\n", "pose_key = X(0)\n", @@ -170,7 +163,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 6, "metadata": { "id": "prior_example_code" }, @@ -179,11 +172,21 @@ "name": "stdout", "output_type": "stream", "text": [ - "OrientedPlane3DirectionPrior: Factor OrientedPlane3DirectionPrior on P0\n", - "Noise model: diagonal sigmas [0.02; 0.02; 0.02];\n", - "\n", - "Error for prior on noisy_plane: [-0.5 -0.5 0.5]\n", - "Error for prior on gt_plane_world: [0. 0. 0.]\n" + "OrientedPlane3DirectionPrior: \n", + "OrientedPlane3DirectionPrior: Prior Factor on p0\n", + "Measured Plane : 0 0 1 0\n", + "isotropic dim=3 sigma=0.02\n" + ] + }, + { + "ename": "RuntimeError", + "evalue": "Attempting to at the key \"p0\", which does not exist in the Values.", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[6], line 9\u001b[0m\n\u001b[0;32m 6\u001b[0m prior_factor\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOrientedPlane3DirectionPrior: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 8\u001b[0m \u001b[38;5;66;03m# Evaluate error using the 'noisy_plane' from the previous step\u001b[39;00m\n\u001b[1;32m----> 9\u001b[0m error_prior \u001b[38;5;241m=\u001b[39m \u001b[43mprior_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m \u001b[38;5;66;03m# values still contains plane_key -> noisy_plane\u001b[39;00m\n\u001b[0;32m 10\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError for prior on noisy_plane: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_prior\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 12\u001b[0m \u001b[38;5;66;03m# Evaluate error for ground truth plane\u001b[39;00m\n", + "\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"p0\", which does not exist in the Values." ] } ], @@ -192,7 +195,7 @@ "measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n", "direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\n", "\n", - "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.coeffs(), direction_noise)\n", + "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)\n", "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n", "\n", "# Evaluate error using the 'noisy_plane' from the previous step\n", diff --git a/gtsam/slam/doc/PoseRotationPrior.ipynb b/gtsam/slam/doc/PoseRotationPrior.ipynb index 444658711..097702a8f 100644 --- a/gtsam/slam/doc/PoseRotationPrior.ipynb +++ b/gtsam/slam/doc/PoseRotationPrior.ipynb @@ -49,27 +49,15 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[2], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseRotationPriorPose\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPriorPose3\n", + "from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPrior3D\n", "from gtsam import symbol_shorthand\n", "\n", "X = symbol_shorthand.X" @@ -95,7 +83,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "create_example_code" }, @@ -104,14 +92,13 @@ "name": "stdout", "output_type": "stream", "text": [ - "PoseRotationPrior: PoseRotationPriorFactor on x0\n", - "Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n", - "Measured Rotation R: [\n", + "PoseRotationPrior: PoseRotationPrior keys = { x0 }\n", + "isotropic dim=3 sigma=0.05\n", + "Measured Rotation [\n", "\t0.707107, -0.707107, 0;\n", "\t0.707107, 0.707107, 0;\n", "\t0, 0, 1\n", - "]\n", - "\n" + "]\n" ] } ], @@ -122,13 +109,13 @@ "# Noise model on rotation (3 dimensions for Rot3)\n", "rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n", "\n", - "# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n", - "factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n", + "# Factor type includes the Pose type, e.g. PoseRotationPrior3D\n", + "factor = PoseRotationPrior3D(pose_key, measured_rotation, rotation_noise)\n", "factor.print(\"PoseRotationPrior: \")\n", "\n", "# Alternative constructor: extract rotation from a full Pose3 prior\n", "full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n", - "factor_from_pose = PoseRotationPriorPose3(pose_key, full_pose_prior, rotation_noise)\n", + "factor_from_pose = PoseRotationPrior3D(pose_key, full_pose_prior, rotation_noise)\n", "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" ] }, @@ -152,7 +139,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 3, "metadata": { "id": "eval_example_code" }, @@ -161,9 +148,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "Error with correct rotation: [0. 0. 0.] (Should be near zero)\n", - "Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n", - "Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n" + "Error with correct rotation: 0.0 (Should be near zero)\n", + "Error with incorrect rotation: 1.9999999999999951 (Should be non-zero)\n", + "Error with different translation: 1.9999999999999951 (Should be same as error2)\n" ] } ], diff --git a/gtsam/slam/doc/PoseTranslationPrior.ipynb b/gtsam/slam/doc/PoseTranslationPrior.ipynb index a71c61c02..0c4e3a4cb 100644 --- a/gtsam/slam/doc/PoseTranslationPrior.ipynb +++ b/gtsam/slam/doc/PoseTranslationPrior.ipynb @@ -19,8 +19,6 @@ "It ignores the rotation component of the pose variable during error calculation.\n", "The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for `Point2` or `Point3`).\n", "\n", - "Error: $ \\text{pose.translation}() - \\text{measured} $ (potentially rotated into world frame if translation is in body frame - check `evaluateError` implementation details in C++). *Correction based on C++ code*: The error is `traits::Local(measured_, pose.translation())`, calculated in the world frame, and the Jacobian involves the pose's rotation.\n", - "\n", "This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement)." ] }, @@ -49,27 +47,15 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n", + "from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPrior3D\n", "from gtsam import symbol_shorthand\n", "\n", "X = symbol_shorthand.X" @@ -95,7 +81,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "create_example_code" }, @@ -104,9 +90,13 @@ "name": "stdout", "output_type": "stream", "text": [ - "PoseTranslationPrior: Factor PoseTranslationPrior on x0\n", - "Noise model: diagonal sigmas [0.5; 0.5; 0.5];\n", - "Measured Translation10 20 5\n" + "PoseTranslationPrior: PoseTranslationPrior keys = { x0 }\n", + "isotropic dim=3 sigma=0.5\n", + "Measured Translation[\n", + "\t10;\n", + "\t20;\n", + "\t5\n", + "]\n" ] } ], @@ -118,12 +108,12 @@ "translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n", "\n", "# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n", - "factor = PoseTranslationPriorPose3(pose_key, measured_translation, translation_noise)\n", + "factor = PoseTranslationPrior3D(pose_key, measured_translation, translation_noise)\n", "factor.print(\"PoseTranslationPrior: \")\n", "\n", "# Alternative constructor: extract translation from a full Pose3 prior\n", "full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored\n", - "factor_from_pose = PoseTranslationPriorPose3(pose_key, full_pose_prior, translation_noise)\n", + "factor_from_pose = PoseTranslationPrior3D(pose_key, full_pose_prior, translation_noise)\n", "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical" ] }, @@ -147,7 +137,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 5, "metadata": { "id": "eval_example_code" }, @@ -156,9 +146,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "Error with correct translation: [0. 0. 0.] (Should be near zero)\n", - "Error with incorrect translation: [ 0.39999999 -0.19999999 0.19999999] (Should be non-zero)\n", - "Error with different rotation: [ 0.39999999 -0.19999999 0.19999999] (Should reflect Jacobian change)\n", + "Error with correct translation: 0.0 (Should be near zero)\n", + "Error with incorrect translation: 0.11999999999999986 (Should be non-zero)\n", + "Error with different rotation: 0.11999999999999986 (Should reflect Jacobian change)\n", "Unwhitened error with different rotation: [ 0.2 -0.1 0.1] (Should be [0.2, -0.1, 0.1])\n" ] } diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb index 13ca2adef..b60f9fa4d 100644 --- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -52,6 +52,24 @@ "%pip install --quiet gtsam-develop" ] }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[]\n" + ] + } + ], + "source": [ + "import gtsam\n", + "print([k for k in gtsam.__dict__.keys() if \"ReferenceFrame\" in k])" + ] + }, { "cell_type": "code", "execution_count": 1, diff --git a/gtsam/slam/doc/RotateFactor.ipynb b/gtsam/slam/doc/RotateFactor.ipynb index edb658d05..51d51ef3e 100644 --- a/gtsam/slam/doc/RotateFactor.ipynb +++ b/gtsam/slam/doc/RotateFactor.ipynb @@ -50,19 +50,7 @@ "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 4\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Rot3, Point3, Unit3, Values\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m RotateFactor, RotateDirectionsFactor\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m R \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mR \u001b[38;5;66;03m# For Rotation key\u001b[39;00m\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", @@ -93,7 +81,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "factor1_example_code" }, @@ -102,14 +90,14 @@ "name": "stdout", "output_type": "stream", "text": [ - "RotateFactor: Factor RotateFactor on R0\n", - "Noise model: diagonal sigmas [0.001; 0.001; 0.001];\n", + "RotateFactor: keys = { r0 }\n", + "isotropic dim=3 sigma=0.001\n", "RotateFactor:]\n", - "p: [0.01 0.02 0.03]\n", - "z: [ 0.03 -0.01 0.02]\n", + "p: 0.01 0.02 0.03\n", + "z: 0.03 -0.01 0.02\n", "\n", - "Error at ground truth R: [-0. -0. 0.]\n", - "Error at noisy R: [ 0.039987 0.00999887 -0.00998887]\n" + "Error at ground truth R: 700.0\n", + "Error at noisy R: 699.2869223608442\n" ] } ], @@ -158,7 +146,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 8, "metadata": { "id": "factor2_example_code" }, @@ -167,15 +155,23 @@ "name": "stdout", "output_type": "stream", "text": [ - "RotateDirectionsFactor: Factor RotateDirectionsFactor on R0\n", - "Noise model: diagonal sigmas [0.01; 0.01];\n", + "RotateDirectionsFactor: keys = { r0 }\n", + "isotropic dim=2 sigma=0.01\n", "RotateDirectionsFactor:\n", - "pUnit3: (1, 0, 0)\n", - "zUnit3: (0, 1, 0)\n", + "p:1\n", + "0\n", + "0\n", + "z:0\n", + "1\n", + "0\n", "\n", - "Check: gt_R * z_body = [ 1.0000000e+00 -6.1232340e-17 0.0000000e+00]\n", - "Error at ground truth R: [-0. 0.]\n", - "Error at noisy R: [-0.99995 -0.99995]\n" + "Check: gt_R * z_body = \n", + ": 1\n", + "6.12323e-17\n", + " 0\n", + "\n", + "Error at ground truth R: 1.874699728327322e-29\n", + "Error at noisy R: 0.999933335111092\n" ] } ], @@ -190,7 +186,7 @@ "# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)\n", "# This corresponds to a -90 deg yaw\n", "gt_R_dir = Rot3.Yaw(-np.pi/2)\n", - "print(f\"\\nCheck: gt_R * z_body = {gt_R_dir * z_body.point3()}\")\n", + "print(f\"\\nCheck: gt_R * z_body = \\n{gt_R_dir.rotate(z_body)}\")\n", "\n", "# Evaluate error\n", "values_dir = Values()\n", diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index 8dd7606ff..291cb0a48 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -40,7 +40,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -49,28 +49,20 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", @@ -104,42 +96,20 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": { "id": "create_example_code" }, "outputs": [ { - "name": "stdout", - "output_type": "stream", - "text": [ - "Smart factor involves 3 measurements.\n", - "SmartFactor: SmartProjectionFactor\n", - "linearizationMode: 0\n", - "triangulationParameters:\n", - "rankTolerance = 1e-09\n", - "enableEPI = false\n", - "landmarkDistanceThreshold = -1\n", - "dynamicOutlierRejectionThreshold = -1\n", - "\n", - "\n", - "result:\n", - "Degenerate\n", - "\n", - "SmartFactorBase, z = \n", - "measurement 0, px = \n", - "[150; 505]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 1, px = \n", - "[470; 495]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 2, px = \n", - "[480; 150]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "Factor on C0 C1 C2\n" + "ename": "AttributeError", + "evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[3], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Add measurements and keys\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m \u001b[43msmart_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43madd\u001b[49m(Point2(\u001b[38;5;241m150\u001b[39m, \u001b[38;5;241m505\u001b[39m), C(\u001b[38;5;241m0\u001b[39m))\n\u001b[0;32m 9\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m470\u001b[39m, \u001b[38;5;241m495\u001b[39m), C(\u001b[38;5;241m1\u001b[39m))\n\u001b[0;32m 10\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m480\u001b[39m, \u001b[38;5;241m150\u001b[39m), C(\u001b[38;5;241m2\u001b[39m))\n", + "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'" ] } ], @@ -179,7 +149,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 7, "metadata": { "id": "eval_example_code" }, @@ -189,9 +159,9 @@ "output_type": "stream", "text": [ "Triangulated point result:\n", - "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "Status.DEGENERATE\n", "\n", - "Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" + "Triangulation failed, error calculation depends on degeneracyMode.\n" ] } ], @@ -209,7 +179,7 @@ "\n", "# Triangulate first to see the implicit point\n", "point_result = smart_factor.point(values)\n", - "print(f\"Triangulated point result:\\n{point_result}\")\n", + "print(f\"Triangulated point result:\\n{point_result.status}\")\n", "\n", "if point_result.valid():\n", " # Calculate error\n", diff --git a/gtsam/slam/doc/SmartFactorParams.ipynb b/gtsam/slam/doc/SmartProjectionParams.ipynb similarity index 57% rename from gtsam/slam/doc/SmartFactorParams.ipynb rename to gtsam/slam/doc/SmartProjectionParams.ipynb index 8ff32bdc1..36e840cb8 100644 --- a/gtsam/slam/doc/SmartFactorParams.ipynb +++ b/gtsam/slam/doc/SmartProjectionParams.ipynb @@ -83,7 +83,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 9, "metadata": { "id": "create_example_code" }, @@ -92,18 +92,27 @@ "name": "stdout", "output_type": "stream", "text": [ - "Default Parameters:\n" - ] - }, - { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mDefault Parameters:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# default_params.print() # TODO\u001b[39;00m\n\u001b[0;32m 5\u001b[0m \n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Custom parameters\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m custom_params \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionParams\u001b[49m\u001b[43m(\u001b[49m\n\u001b[0;32m 8\u001b[0m \u001b[43m \u001b[49m\u001b[43mlinearizationMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mLinearizationMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mJACOBIAN_Q\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 9\u001b[0m \u001b[43m \u001b[49m\u001b[43mdegeneracyMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mDegeneracyMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mZERO_ON_DEGENERACY\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 10\u001b[0m \u001b[43m \u001b[49m\u001b[43mthrowCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[43m \u001b[49m\u001b[43mverboseCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 12\u001b[0m \u001b[43m \u001b[49m\u001b[43mretriangulationTh\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1e-3\u001b[39;49m\n\u001b[0;32m 13\u001b[0m \u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;66;03m# Modify triangulation parameters directly\u001b[39;00m\n\u001b[0;32m 15\u001b[0m custom_params\u001b[38;5;241m.\u001b[39mtriangulation\u001b[38;5;241m.\u001b[39mrankTolerance \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m1e-8\u001b[39m\n", - "\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=, degeneracyMode=, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001" + "Default Parameters:\n", + "linearizationMode: 0\n", + " degeneracyMode: 0\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "useLOST = 0\n", + "noise model\n", + "\n", + "\n", + "Custom Parameters:\n", + "linearizationMode: 2\n", + " degeneracyMode: 1\n", + "rankTolerance = 1e-08\n", + "enableEPI = 0\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = 1\n", + "useLOST = 0\n", + "noise model\n", + "\n" ] } ], @@ -115,25 +124,19 @@ "\n", "# Custom parameters\n", "custom_params = SmartProjectionParams(\n", - " linearizationMode = LinearizationMode.JACOBIAN_Q,\n", - " degeneracyMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", + " linMode = LinearizationMode.JACOBIAN_Q,\n", + " degMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n", " throwCheirality = False,\n", " verboseCheirality = True,\n", " retriangulationTh = 1e-3\n", ")\n", "# Modify triangulation parameters directly\n", - "custom_params.triangulation.rankTolerance = 1e-8\n", - "custom_params.triangulation.enableEPI = False\n", - "custom_params.triangulation.dynamicOutlierRejectionThreshold = 3.0 # Reject points with reproj error > 3*sigma\n", + "custom_params.setRankTolerance(1e-8)\n", + "custom_params.setEnableEPI(False)\n", + "custom_params.setDynamicOutlierRejectionThreshold(3.0) # Reject points with reproj error > 3*sigma\n", "\n", "print(\"\\nCustom Parameters:\")\n", - "custom_params.print()\n", - "\n", - "# Accessing parameters\n", - "print(f\"\\nAccessing Custom Params:\")\n", - "print(f\" Linearization Mode: {custom_params.getLinearizationMode()}\")\n", - "print(f\" Degeneracy Mode: {custom_params.getDegeneracyMode()}\")\n", - "print(f\" Rank Tolerance: {custom_params.getTriangulationParameters().rankTolerance}\")" + "custom_params.print()" ] }, { @@ -156,7 +159,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 10, "metadata": { "id": "usage_example_code" }, @@ -171,27 +174,28 @@ "linearizationMode: 2\n", "triangulationParameters:\n", "rankTolerance = 1e-08\n", - "enableEPI = false\n", + "enableEPI = 0\n", "landmarkDistanceThreshold = -1\n", - "dynamicOutlierRejectionThreshold = 3\n", - "\n", + "dynamicOutlierRejectionThreshold = 1\n", + "useLOST = 0\n", + "noise model\n", "\n", "result:\n", - "Degenerate\n", + "no point, status = 1\n", "\n", "SmartFactorBase, z = \n", - "Factor\n" + " keys = { }\n" ] } ], "source": [ - "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n", + "from gtsam import SmartProjectionPose3Factor, Cal3_S2\n", "\n", "# Example: Using custom params with a smart factor\n", "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", - "K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240)\n", + "K = Cal3_S2(500, 500, 0, 320, 240)\n", "\n", - "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n", + "smart_factor = SmartProjectionPose3Factor(noise, K, custom_params)\n", "print(\"Smart Factor created with custom params:\")\n", "smart_factor.print()" ] diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index be34d6b44..c8d7f3b2e 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -54,28 +54,17 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n\u001b[0;32m 5\u001b[0m Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", + " # SmartProjectionPoseFactor with Cal3_S2 is called SmartProjectionPose3Factor\n", "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n", + " SmartProjectionParams, SmartProjectionPose3Factor,\n", " Cal3_S2)\n", "from gtsam import symbol_shorthand\n", "import graphviz\n", @@ -104,7 +93,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 5, "metadata": { "id": "create_example_code" }, @@ -118,39 +107,45 @@ " SmartProjectionFactor\n", "linearizationMode: 0\n", "triangulationParameters:\n", - "rankTolerance = 1e-09\n", - "enableEPI = false\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", "landmarkDistanceThreshold = -1\n", "dynamicOutlierRejectionThreshold = -1\n", - "\n", + "useLOST = 0\n", + "noise model\n", "\n", "result:\n", - "Degenerate\n", + "no point, status = 1\n", "\n", "SmartFactorBase, z = \n", "measurement 0, px = \n", - "[150; 505]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", + "150\n", + "505\n", + "noise model = unit (2) \n", "measurement 1, px = \n", - "[470; 495]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", + "470\n", + "495\n", + "noise model = unit (2) \n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ "measurement 2, px = \n", - "[480; 150]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "Factor on x0 x1 x2\n" + "480\n", + "150\n", + "noise model = unit (2) \n", + " keys = { x0 x1 x2 }\n" ] } ], "source": [ "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "smart_params = SmartProjectionParams() # Use default params\n", - "K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", + "K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", "\n", - "# Factor type includes the Calibration type\n", - "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n", + "smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\n", "\n", "# Add measurements and keys (Pose keys)\n", "smart_factor.add(Point2(150, 505), X(0))\n", @@ -181,7 +176,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 16, "metadata": { "id": "eval_example_code" }, @@ -190,19 +185,19 @@ "name": "stdout", "output_type": "stream", "text": [ - "Triangulated point result:\n", - "Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n", + "Triangulated point result: Status.VALID\n", + "Triangulated point: [0.28416823 1.95555615 5.67688675]\n", "\n", - "Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n" + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 120243.1626\n" ] } ], "source": [ "# Create Values containing Pose3 objects\n", "values = Values()\n", - "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n", - "pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n", - "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n", + "pose0 = Pose3(Rot3.Ypr(0.1, 0, 0), Point3(-1, 0, 0.5))\n", + "pose1 = Pose3(Rot3.Ypr(0.0, -0.1, 0), Point3(1, 0, 0.5))\n", + "pose2 = Pose3(Rot3.Ypr(0, 0.0, 0.2), Point3(0, 1, 0.5))\n", "\n", "values.insert(X(0), pose0)\n", "values.insert(X(1), pose1)\n", @@ -210,9 +205,10 @@ "\n", "# Triangulate first to see the implicit point\n", "point_result = smart_factor.point(values)\n", - "print(f\"Triangulated point result:\\n{point_result}\")\n", + "print(f\"Triangulated point result: {point_result.status}\")\n", "\n", "if point_result.valid():\n", + " print(f\"Triangulated point: {point_result.get()}\")\n", " # Calculate error\n", " total_error = smart_factor.error(values)\n", " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", @@ -240,7 +236,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": { "id": "linearize_example_code" }, @@ -251,61 +247,29 @@ "text": [ "\n", "Linearized Factor (showing HessianFactor structure):\n", - "RegularHessianFactor(6): density=100% keys={ x0 x1 x2 }\n", - "Augmented information matrix: (dimensions: 6, 6, 6) : \n", - "{\n", - "\t[ 1900, 4864, -698, -5014, -4686, -552; ]\n", - "\t[ 4864, 1.337e+04, 1473, -9.522e+03, -1.623e+04, 447; ]\n", - "\t[ -698, 1473, 4803, 3085, -3660, -1.118e+04; ]\n", - "\t[ -5014, -9.522e+03, 3085, 1318, 2669, -1843; ]\n", - "\t[ -4686, -1.623e+04, -3660, 2669, 4137, -1067; ]\n", - "\t[ -552, 447, -1.118e+04, -1843, -1067, 4035; ]\n", - "\t[ 1318, 2669, -1843, 768, -2084, 849; ]\n", - "\t[ 2669, 4137, -1067, -2084, 6054, -1412; ]\n", - "\t[ -1843, -1067, 4035, 849, -1412, 4392; ]\n", - "\t[ 6273, -1.411e+04, -2161, 11515, 16951, 996; ]\n", - "\t[ -141, -1903, -1.299e+04, 230, -2148, 7017; ]\n", - "\t[ -104, 5192, -1500, -373, -620, -7830; ]\n", - "\t[ -830, 6917, -785, -395, -1398, -708; ]\n", - "\t[ -969, 3198, -1569, 587, 1292, -3268; ]\n", - "\t[ 268, -1.286e+04, 1865, -122, -2205, 2048; ]\n", - "\t[ -708, -1569, -7830, 23563, 2137, 1132; ]\n", - "\t[ -321, 3767, -302, 2137, 15054, -1172; ]\n", - "\t[ -1196, 5036, 1563, 1132, -1172, 8384; ]\n", - "}\n", - "Augmented Diagonal Block [0,0]:\n", - "[ 1900, 4864, -698; ]\n", - "[ 4864, 1.337e+04, 1473; ]\n", - "[ -698, 1473, 4803; ]\n", "\n", - "Augmented Diagonal Block [1,1]:\n", - "[ 768, -2084, 849; ]\n", - "[ -2084, 6054, -1412; ]\n", - "[ 849, -1412, 4392; ]\n", - "\n", - "Augmented Diagonal Block [2,2]:\n", - "[ 23563, 2137, 1132; ]\n", - "[ 2137, 15054, -1172; ]\n", - "[ 1132, -1172, 8384; ]\n", - "\n", - "Off-Diagonal Block [0,1]:\n", - "[ -5014, -4686, -552; ]\n", - "[ -9.522e+03, -1.623e+04, 447; ]\n", - "[ 3085, -3660, -1.118e+04; ]\n", - "\n", - "Off-Diagonal Block [0,2]:\n", - "[ 11515, 16951, 996; ]\n", - "[ 230, -2148, 7017; ]\n", - "[ -373, -620, -7830; ]\n", - "\n", - "Off-Diagonal Block [1,2]:\n", - "[ 6273, -830, -969; ]\n", - "[ -1.411e+04, 6917, 3198; ]\n", - "[ -2161, -785, -1569; ]\n", - "\n", - "Error vector:\n", - "[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n", - "Constant error term: 103876\n" + " keys: x0(6) x1(6) x2(6) \n", + "Augmented information matrix: [\n", + "\t202102, 6373.47, -59741.9, 4386.91, -35145.2, 11091.1, -115337, -44197.1, 12233.6, -8181.39, 19268.4, -7521.18, -81683.2, -724.877, 5044.41, -484.005, 14404.6, -5767.48, 89097.4;\n", + "\t6373.47, 81115.2, -30292.7, 14717.5, -2404.93, -3343.21, 63410.4, 20584.6, -5335.01, 3787.66, -10602.2, 4111.71, -60743.7, -88128.4, 38992.5, -18242.3, 11072.3, -3393.47, 105458;\n", + "\t-59741.9, -30292.7, 27634, -6415.45, 10844.3, -1981.98, 10553.8, 5348.27, -1607.72, 998.024, -1760.06, 696.401, 44568.2, 31147.8, -15125.4, 6542.51, -7986, 2832.46, -62376.9;\n", + "\t4386.91, 14717.5, -6415.45, 2722.09, -996.496, -424.653, 9577.1, 3000.7, -765.229, 551.36, -1601.54, 620.327, -12253.7, -15890.6, 7106.5, -3294.65, 2225.84, -703.855, 20429.2;\n", + "\t-35145.2, -2404.93, 10844.3, -996.496, 6132.48, -1869.56, 18982.5, 7333.62, -2035.73, 1357.9, -3171.11, 1238.23, 15136.7, 1537.88, -1499.49, 376.242, -2675.09, 1054.42, -17138.8;\n", + "\t11091.1, -3343.21, -1981.98, -424.653, -1869.56, 777.22, -9389.67, -3428.6, 932.466, -633.633, 1569.05, -611.241, -1827.86, 3981.42, -1495.55, 805.333, 305.895, -169.934, 204.626;\n", + "\t-115337, 63410.4, 10553.8, 9577.1, 18982.5, -9389.67, 210063, -32012.2, 19847.7, -6615.86, -35357.8, 12992.1, -83063.6, 2877.07, 3675.5, 245.696, 14633.2, -5901.9, 91375.5;\n", + "\t-44197.1, 20584.6, 5348.27, 3000.7, 7333.62, -3428.6, -32012.2, 79820.6, -31086.7, 15340.5, 5564.44, -1509.55, 71381, -87978.3, 31244.3, -17668.1, -12223.3, 5946.04, -40235;\n", + "\t12233.6, -5335.01, -1607.72, -765.229, -2035.73, 932.466, 19847.7, -31086.7, 12383.1, -5991.81, -3406.72, 1051.42, -29836.9, 33051, -11561.9, 6625.01, 5124.61, -2447.34, 18485.3;\n", + "\t-8181.39, 3787.66, 998.024, 551.36, 1357.9, -633.633, -6615.86, 15340.5, -5991.81, 2949.34, 1147.27, -319.227, 13846.5, -16832.1, 5966.68, -3379.52, -2372.04, 1151.02, -7909.48;\n", + "\t19268.4, -10602.2, -1760.06, -1601.54, -3171.11, 1569.05, -35357.8, 5564.44, -3406.72, 1147.27, 5951.86, -2185.74, 14119.3, -690.008, -543.913, -82.7916, -2486.55, 1005.27, -15442.2;\n", + "\t-7521.18, 4111.71, 696.401, 620.327, 1238.23, -611.241, 12992.1, -1509.55, 1051.42, -319.227, -2185.74, 806.502, -4768.82, -371.498, 426.945, -95.4627, 842.323, -333.351, 5486.34;\n", + "\t-81683.2, -60743.7, 44568.2, -12253.7, 15136.7, -1827.86, -83063.6, 71381, -29836.9, 13846.5, 14119.3, -4768.82, 149690, -5708.64, -6412.92, -549.716, -26368.6, 10641.3, -162324;\n", + "\t-724.877, -88128.4, 31147.8, -15890.6, 1537.88, 3981.42, 2877.07, -87978.3, 33051, -16832.1, -690.008, -371.498, -5708.64, 160163, -64108.9, 32675.7, 347.645, -2041.12, -63443.5;\n", + "\t5044.41, 38992.5, -15125.4, 7106.5, -1499.49, -1495.55, 3675.5, 31244.3, -11561.9, 5966.68, -543.913, 426.945, -6412.92, -64108.9, 26167.1, -13114.9, 1394.39, 202.071, 34971.1;\n", + "\t-484.005, -18242.3, 6542.51, -3294.65, 376.242, 805.333, 245.696, -17668.1, 6625.01, -3379.52, -82.7916, -95.4627, -549.716, 32675.7, -13114.9, 6668.86, -37.4944, -372.943, -13620.5;\n", + "\t14404.6, 11072.3, -7986, 2225.84, -2675.09, 305.895, 14633.2, -12223.3, 5124.61, -2372.04, -2486.55, 842.323, -26368.6, 347.645, 1394.39, -37.4944, 4647.64, -1867.77, 28880.4;\n", + "\t-5767.48, -3393.47, 2832.46, -703.855, 1054.42, -169.934, -5901.9, 5946.04, -2447.34, 1151.02, 1005.27, -333.351, 10641.3, -2041.12, 202.071, -372.943, -1867.77, 773.191, -10827.4;\n", + "\t89097.4, 105458, -62376.9, 20429.2, -17138.8, 204.626, 91375.5, -40235, 18485.3, -7909.48, -15442.2, 5486.34, -162324, -63443.5, 34971.1, -13620.5, 28880.4, -10827.4, 240486\n", + "]\n" ] } ], @@ -319,8 +283,7 @@ "if linear_factor:\n", " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", " # Cast to HessianFactor to print its details\n", - " # Note: The factor dimension is Pose3::dimension (6)\n", - " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", + " hessian_factor = gtsam.HessianFactor(linear_factor)\n", " if hessian_factor:\n", " hessian_factor.print()\n", " else:\n", diff --git a/gtsam/slam/doc/StereoFactor.ipynb b/gtsam/slam/doc/StereoFactor.ipynb index 08c1b6ab4..65e3014db 100644 --- a/gtsam/slam/doc/StereoFactor.ipynb +++ b/gtsam/slam/doc/StereoFactor.ipynb @@ -52,7 +52,7 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "%pip install --quiet gtsam" ] }, { @@ -66,6 +66,8 @@ "import gtsam\n", "import numpy as np\n", "from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n", + "# Need StereoCamera for backprojection/triangulation\n", + "from gtsam import StereoCamera \n", "# The Python wrapper often creates specific instantiations\n", "from gtsam import GenericStereoFactor3D\n", "from gtsam import symbol_shorthand\n", @@ -101,7 +103,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": { "id": "create_example_code" }, @@ -170,20 +172,23 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 3, "metadata": { "id": "eval_example_code" }, "outputs": [ { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 4\u001b[0m pose \u001b[38;5;241m=\u001b[39m Pose3(Rot3\u001b[38;5;241m.\u001b[39mRodrigues(\u001b[38;5;241m0.1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m0.2\u001b[39m, \u001b[38;5;241m0.3\u001b[39m), Point3(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m0.5\u001b[39m))\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Triangulate a point that *should* project to measured_stereo\u001b[39;00m\n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m expected_point_camera \u001b[38;5;241m=\u001b[39m \u001b[43mK_stereo\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mbackproject\u001b[49m(measured_stereo)\n\u001b[0;32m 8\u001b[0m landmark \u001b[38;5;241m=\u001b[39m pose\u001b[38;5;241m.\u001b[39mtransformFrom(expected_point_camera)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mExpected landmark point: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlandmark\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'" + "name": "stdout", + "output_type": "stream", + "text": [ + "Expected landmark point (no offset): [ 1.54225239 -2.27112649 2.95849821]\n", + "\n", + "Error (no offset) at expected landmark: 48664.883462255115 (Should be near zero)\n", + "\n", + "Expected landmark point (offset): [ 2.89128008 -3.54882535 1.19789333]\n", + "Error (with offset) at recomputed landmark: 1783675.2295780657 (Should be near zero)\n", + "\n", + "Error (no offset) at noisy landmark: 54320.22670263611\n" ] } ], @@ -192,33 +197,44 @@ "\n", "# Example values\n", "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n", - "# Triangulate a point that *should* project to measured_stereo\n", - "# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n", - "expected_point_camera = K_stereo.backproject(measured_stereo)\n", - "landmark = pose.transformFrom(expected_point_camera)\n", - "print(f\"Expected landmark point: {landmark}\")\n", - "\n", "values.insert(pose_key, pose)\n", + "\n", + "# --- Evaluate factor without offset --- \n", + "# Create a StereoCamera object at the current pose\n", + "camera_no_offset = StereoCamera(pose, K_stereo)\n", + "# Triangulate (backproject) the measurement to get the point in the camera frame\n", + "# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n", + "expected_point_camera = camera_no_offset.backproject(measured_stereo) # Point in camera frame\n", + "# Transform the point from the camera frame to the world frame\n", + "landmark = pose.transformFrom(expected_point_camera) # Point in world frame\n", + "print(f\"Expected landmark point (no offset): {landmark}\")\n", + "\n", "values.insert(landmark_key, landmark)\n", - "\n", - "# Evaluate factor without offset\n", "error_no_offset = factor_no_offset.error(values)\n", - "print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be zero)\")\n", + "print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be near zero)\")\n", "\n", - "# Evaluate factor with offset\n", - "# Need to recompute landmark based on offset pose\n", - "pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n", - "expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n", - "landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n", + "# --- Evaluate factor with offset --- \n", + "# Calculate the actual sensor pose in the world\n", + "pose_with_offset = pose.compose(body_P_sensor) # world_P_sensor = world_P_body * body_P_sensor\n", + "# Create a StereoCamera object at the sensor pose\n", + "camera_with_offset = StereoCamera(pose_with_offset, K_stereo)\n", + "# Triangulate the measurement from the sensor's perspective\n", + "expected_point_offset_cam = camera_with_offset.backproject(measured_stereo) # Point in sensor frame\n", + "# Transform the point from the sensor frame to the world frame\n", + "landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam) # Point in world frame\n", + "print(f\"\\nExpected landmark point (offset): {landmark_offset}\")\n", + "\n", + "# Update the landmark value in Values for the offset factor calculation\n", "values.update(landmark_key, landmark_offset)\n", "error_with_offset = factor_with_offset.error(values)\n", - "print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be zero)\")\n", + "print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be near zero)\")\n", "\n", - "# Evaluate with noisy landmark\n", - "noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n", + "# --- Evaluate with noisy landmark (using the no-offset factor for simplicity) ---\n", + "# Use the original landmark calculated for the no-offset case as the 'ground truth'\n", + "noisy_landmark = landmark + Point3(0.1, -0.05, 0.1) \n", "values.update(landmark_key, noisy_landmark)\n", "error_no_offset_noisy = factor_no_offset.error(values)\n", - "print(f\"Error (no offset) at noisy landmark: {error_no_offset_noisy}\")" + "print(f\"\\nError (no offset) at noisy landmark: {error_no_offset_noisy}\")" ] } ], From 502ff2762b6fd4d0b78601431995d7a4bc2734c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 22:36:53 -0400 Subject: [PATCH 31/38] Add OrientedPlane3 to Values wrapper --- gtsam/nonlinear/values.i | 5 +++ gtsam/slam/doc/OrientedPlane3Factor.ipynb | 53 +++++++++-------------- 2 files changed, 26 insertions(+), 32 deletions(-) diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index bd17958e2..2bce88bdd 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -12,6 +12,7 @@ namespace gtsam { #include #include #include +#include #include #include #include @@ -94,6 +95,7 @@ class Values { void insert(size_t j, const gtsam::EssentialMatrix& E); void insert(size_t j, const gtsam::FundamentalMatrix& F); void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert(size_t j, const gtsam::OrientedPlane3& plane); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); @@ -138,6 +140,7 @@ class Values { void update(size_t j, const gtsam::EssentialMatrix& E); void update(size_t j, const gtsam::FundamentalMatrix& F); void update(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void update(size_t j, const gtsam::OrientedPlane3& plane); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); @@ -179,6 +182,7 @@ class Values { void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E); void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert_or_assign(size_t j, const gtsam::OrientedPlane3& plane); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); @@ -216,6 +220,7 @@ class Values { gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix, + gtsam::OrientedPlane3, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, diff --git a/gtsam/slam/doc/OrientedPlane3Factor.ipynb b/gtsam/slam/doc/OrientedPlane3Factor.ipynb index 7815b844f..e44d3b5a6 100644 --- a/gtsam/slam/doc/OrientedPlane3Factor.ipynb +++ b/gtsam/slam/doc/OrientedPlane3Factor.ipynb @@ -34,7 +34,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -43,7 +43,11 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { @@ -82,7 +86,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 3, "metadata": { "id": "factor_example_code" }, @@ -94,18 +98,11 @@ "OrientedPlane3Factor: \n", "OrientedPlane3Factor Factor (x0, p0)\n", "Measured Plane : 0 0 1 1\n", - "isotropic dim=3 sigma=0.05\n" - ] - }, - { - "ename": "TypeError", - "evalue": "insert(): incompatible function arguments. The following argument types are supported:\n 1. (self: gtsam.gtsam.Values, values: gtsam.gtsam.Values) -> None\n 2. (self: gtsam.gtsam.Values, j: int, vector: numpy.ndarray[numpy.float64[m, 1]]) -> None\n 3. (self: gtsam.gtsam.Values, j: int, matrix: numpy.ndarray[numpy.float64[m, n]]) -> None\n 4. (self: gtsam.gtsam.Values, j: int, point2: numpy.ndarray[numpy.float64[2, 1]]) -> None\n 5. (self: gtsam.gtsam.Values, j: int, point3: numpy.ndarray[numpy.float64[3, 1]]) -> None\n 6. (self: gtsam.gtsam.Values, j: int, rot2: gtsam.gtsam.Rot2) -> None\n 7. (self: gtsam.gtsam.Values, j: int, pose2: gtsam.gtsam.Pose2) -> None\n 8. (self: gtsam.gtsam.Values, j: int, R: gtsam.gtsam.SO3) -> None\n 9. (self: gtsam.gtsam.Values, j: int, Q: gtsam.gtsam.SO4) -> None\n 10. (self: gtsam.gtsam.Values, j: int, P: gtsam.gtsam.SOn) -> None\n 11. (self: gtsam.gtsam.Values, j: int, rot3: gtsam.gtsam.Rot3) -> None\n 12. (self: gtsam.gtsam.Values, j: int, pose3: gtsam.gtsam.Pose3) -> None\n 13. (self: gtsam.gtsam.Values, j: int, similarity2: gtsam.gtsam.Similarity2) -> None\n 14. (self: gtsam.gtsam.Values, j: int, similarity3: gtsam.gtsam.Similarity3) -> None\n 15. (self: gtsam.gtsam.Values, j: int, unit3: gtsam.gtsam.Unit3) -> None\n 16. (self: gtsam.gtsam.Values, j: int, cal3bundler: gtsam.gtsam.Cal3Bundler) -> None\n 17. (self: gtsam.gtsam.Values, j: int, cal3f: gtsam.gtsam.Cal3f) -> None\n 18. (self: gtsam.gtsam.Values, j: int, cal3_s2: gtsam.gtsam.Cal3_S2) -> None\n 19. (self: gtsam.gtsam.Values, j: int, cal3ds2: gtsam.gtsam.Cal3DS2) -> None\n 20. (self: gtsam.gtsam.Values, j: int, cal3fisheye: gtsam.gtsam.Cal3Fisheye) -> None\n 21. (self: gtsam.gtsam.Values, j: int, cal3unified: gtsam.gtsam.Cal3Unified) -> None\n 22. (self: gtsam.gtsam.Values, j: int, E: gtsam.gtsam.EssentialMatrix) -> None\n 23. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.FundamentalMatrix) -> None\n 24. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.SimpleFundamentalMatrix) -> None\n 25. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Bundler) -> None\n 26. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3f) -> None\n 27. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3_S2) -> None\n 28. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3DS2) -> None\n 29. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Fisheye) -> None\n 30. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Unified) -> None\n 31. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Bundler) -> None\n 32. (self: gtsam.gtsam.Values, j: int, camera: gtsam::PinholePose) -> None\n 33. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3_S2) -> None\n 34. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3DS2) -> None\n 35. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Fisheye) -> None\n 36. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Unified) -> None\n 37. (self: gtsam.gtsam.Values, j: int, constant_bias: gtsam::imuBias::ConstantBias) -> None\n 38. (self: gtsam.gtsam.Values, j: int, nav_state: gtsam::NavState) -> None\n 39. (self: gtsam.gtsam.Values, j: int, c: float) -> None\n\nInvoked with: Values with 1 values:\nValue x0: (class gtsam::Pose3)\nR: [\n\t0.995004, -0.0998334, 0;\n\t0.0998334, 0.995004, 0;\n\t0, 0, 1\n]\nt: 0.5 0 0\n\n, 8070450532247928832, : 0 0 1 1\n", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 22\u001b[0m\n\u001b[0;32m 20\u001b[0m values \u001b[38;5;241m=\u001b[39m Values()\n\u001b[0;32m 21\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose)\n\u001b[1;32m---> 22\u001b[0m \u001b[43mvalues\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minsert\u001b[49m\u001b[43m(\u001b[49m\u001b[43mplane_key\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mgt_plane_world\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 23\u001b[0m error1 \u001b[38;5;241m=\u001b[39m plane_factor\u001b[38;5;241m.\u001b[39merror(values)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror1\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[1;31mTypeError\u001b[0m: insert(): incompatible function arguments. The following argument types are supported:\n 1. (self: gtsam.gtsam.Values, values: gtsam.gtsam.Values) -> None\n 2. (self: gtsam.gtsam.Values, j: int, vector: numpy.ndarray[numpy.float64[m, 1]]) -> None\n 3. (self: gtsam.gtsam.Values, j: int, matrix: numpy.ndarray[numpy.float64[m, n]]) -> None\n 4. (self: gtsam.gtsam.Values, j: int, point2: numpy.ndarray[numpy.float64[2, 1]]) -> None\n 5. (self: gtsam.gtsam.Values, j: int, point3: numpy.ndarray[numpy.float64[3, 1]]) -> None\n 6. (self: gtsam.gtsam.Values, j: int, rot2: gtsam.gtsam.Rot2) -> None\n 7. (self: gtsam.gtsam.Values, j: int, pose2: gtsam.gtsam.Pose2) -> None\n 8. (self: gtsam.gtsam.Values, j: int, R: gtsam.gtsam.SO3) -> None\n 9. (self: gtsam.gtsam.Values, j: int, Q: gtsam.gtsam.SO4) -> None\n 10. (self: gtsam.gtsam.Values, j: int, P: gtsam.gtsam.SOn) -> None\n 11. (self: gtsam.gtsam.Values, j: int, rot3: gtsam.gtsam.Rot3) -> None\n 12. (self: gtsam.gtsam.Values, j: int, pose3: gtsam.gtsam.Pose3) -> None\n 13. (self: gtsam.gtsam.Values, j: int, similarity2: gtsam.gtsam.Similarity2) -> None\n 14. (self: gtsam.gtsam.Values, j: int, similarity3: gtsam.gtsam.Similarity3) -> None\n 15. (self: gtsam.gtsam.Values, j: int, unit3: gtsam.gtsam.Unit3) -> None\n 16. (self: gtsam.gtsam.Values, j: int, cal3bundler: gtsam.gtsam.Cal3Bundler) -> None\n 17. (self: gtsam.gtsam.Values, j: int, cal3f: gtsam.gtsam.Cal3f) -> None\n 18. (self: gtsam.gtsam.Values, j: int, cal3_s2: gtsam.gtsam.Cal3_S2) -> None\n 19. (self: gtsam.gtsam.Values, j: int, cal3ds2: gtsam.gtsam.Cal3DS2) -> None\n 20. (self: gtsam.gtsam.Values, j: int, cal3fisheye: gtsam.gtsam.Cal3Fisheye) -> None\n 21. (self: gtsam.gtsam.Values, j: int, cal3unified: gtsam.gtsam.Cal3Unified) -> None\n 22. (self: gtsam.gtsam.Values, j: int, E: gtsam.gtsam.EssentialMatrix) -> None\n 23. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.FundamentalMatrix) -> None\n 24. (self: gtsam.gtsam.Values, j: int, F: gtsam.gtsam.SimpleFundamentalMatrix) -> None\n 25. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Bundler) -> None\n 26. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3f) -> None\n 27. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3_S2) -> None\n 28. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3DS2) -> None\n 29. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Fisheye) -> None\n 30. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholeCameraCal3Unified) -> None\n 31. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Bundler) -> None\n 32. (self: gtsam.gtsam.Values, j: int, camera: gtsam::PinholePose) -> None\n 33. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3_S2) -> None\n 34. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3DS2) -> None\n 35. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Fisheye) -> None\n 36. (self: gtsam.gtsam.Values, j: int, camera: gtsam.gtsam.PinholePoseCal3Unified) -> None\n 37. (self: gtsam.gtsam.Values, j: int, constant_bias: gtsam::imuBias::ConstantBias) -> None\n 38. (self: gtsam.gtsam.Values, j: int, nav_state: gtsam::NavState) -> None\n 39. (self: gtsam.gtsam.Values, j: int, c: float) -> None\n\nInvoked with: Values with 1 values:\nValue x0: (class gtsam::Pose3)\nR: [\n\t0.995004, -0.0998334, 0;\n\t0.0998334, 0.995004, 0;\n\t0, 0, 1\n]\nt: 0.5 0 0\n\n, 8070450532247928832, : 0 0 1 1\n" + "isotropic dim=3 sigma=0.05\n", + "\n", + "Error at ground truth: 0.0\n", + "\n", + "Error with noisy plane: 0.6469041114912286\n" ] } ], @@ -158,12 +155,12 @@ }, "source": [ "A unary factor that puts a prior on the *direction* (normal vector) of an `OrientedPlane3`. The distance component (`d`) of the measured plane is ignored.\n", - "The error is the 3D difference between the estimated plane's normal and the measured plane's normal." + "The error is the difference between the estimated plane's normal and the measured plane's normal, but as directions only have 2 DOF, the noise model also has to have dimension 2." ] }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 4, "metadata": { "id": "prior_example_code" }, @@ -175,25 +172,17 @@ "OrientedPlane3DirectionPrior: \n", "OrientedPlane3DirectionPrior: Prior Factor on p0\n", "Measured Plane : 0 0 1 0\n", - "isotropic dim=3 sigma=0.02\n" - ] - }, - { - "ename": "RuntimeError", - "evalue": "Attempting to at the key \"p0\", which does not exist in the Values.", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[6], line 9\u001b[0m\n\u001b[0;32m 6\u001b[0m prior_factor\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOrientedPlane3DirectionPrior: \u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 8\u001b[0m \u001b[38;5;66;03m# Evaluate error using the 'noisy_plane' from the previous step\u001b[39;00m\n\u001b[1;32m----> 9\u001b[0m error_prior \u001b[38;5;241m=\u001b[39m \u001b[43mprior_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m \u001b[38;5;66;03m# values still contains plane_key -> noisy_plane\u001b[39;00m\n\u001b[0;32m 10\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError for prior on noisy_plane: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_prior\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 12\u001b[0m \u001b[38;5;66;03m# Evaluate error for ground truth plane\u001b[39;00m\n", - "\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"p0\", which does not exist in the Values." + "isotropic dim=2 sigma=0.02\n", + "\n", + "Error for prior on noisy_plane: 0.2550239722533919\n", + "Error for prior on gt_plane_world: 0.0\n" ] } ], "source": [ "# Measured direction prior (e.g., plane normal is close to world Z axis)\n", "measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n", - "direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\n", + "direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.02)\n", "\n", "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)\n", "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n", @@ -211,7 +200,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -225,7 +214,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, From 513bbb43ed05eec5ee8621494f3faa166164a700 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 22:40:03 -0400 Subject: [PATCH 32/38] Fix FindKarcherMean tests --- python/gtsam/tests/test_KarcherMeanFactor.py | 6 +++--- python/gtsam/tests/test_backwards_compatibility.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index 0bc942341..c7154fd18 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -35,7 +35,7 @@ class TestKarcherMean(GtsamTestCase): """ rotations = [R, R.inverse()] expected = Rot3() - actual = gtsam.FindKarcherMean(rotations) + actual = gtsam.FindKarcherMeanRot3(rotations) self.gtsamAssertEquals(expected, actual) def test_find_karcher_mean_identity(self): @@ -47,7 +47,7 @@ class TestKarcherMean(GtsamTestCase): aRb_list = [a1Rb1, a2Rb2, a3Rb3] aRb_expected = Rot3() - aRb = gtsam.FindKarcherMean(aRb_list) + aRb = gtsam.FindKarcherMeanRot3(aRb_list) self.gtsamAssertEquals(aRb, aRb_expected) def test_factor(self): @@ -69,7 +69,7 @@ class TestKarcherMean(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)]) + actual = gtsam.FindKarcherMeanRot3([result.atRot3(1), result.atRot3(2)]) self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2))) diff --git a/python/gtsam/tests/test_backwards_compatibility.py b/python/gtsam/tests/test_backwards_compatibility.py index c64be37a7..c6c7eadca 100644 --- a/python/gtsam/tests/test_backwards_compatibility.py +++ b/python/gtsam/tests/test_backwards_compatibility.py @@ -315,7 +315,7 @@ class TestBackwardsCompatibility(GtsamTestCase): rotations = gtsam.Rot3Vector([R, R.inverse()]) expected = Rot3() - actual = gtsam.FindKarcherMean(rotations) + actual = gtsam.FindKarcherMeanRot3(rotations) self.gtsamAssertEquals(expected, actual) def test_find_karcher_mean_identity(self): @@ -327,7 +327,7 @@ class TestBackwardsCompatibility(GtsamTestCase): aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) aRb_expected = Rot3() - aRb = gtsam.FindKarcherMean(aRb_list) + aRb = gtsam.FindKarcherMeanRot3(aRb_list) self.gtsamAssertEquals(aRb, aRb_expected) def test_factor(self): @@ -354,7 +354,7 @@ class TestBackwardsCompatibility(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean( + actual = gtsam.FindKarcherMeanRot3( gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)])) self.gtsamAssertEquals(expected, actual) From 47812cd89908f343117d531209f5e253899ecaaf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 23:35:03 -0400 Subject: [PATCH 33/38] Expanded SmartFactor wrapping --- gtsam/slam/doc/SmartProjectionFactor.ipynb | 166 +++++------------- gtsam/slam/doc/SmartProjectionParams.ipynb | 22 ++- .../slam/doc/SmartProjectionPoseFactor.ipynb | 51 +++--- gtsam/slam/slam.i | 31 +++- .../covarianceAnalysisCreateFactorGraph.m | 4 +- 5 files changed, 114 insertions(+), 160 deletions(-) diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index 291cb0a48..51746ce34 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -66,13 +66,19 @@ "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n", - " PinholeCameraCal3_S2, Cal3_S2)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "C = symbol_shorthand.C # Key for Camera object" + "from gtsam import (\n", + " Values,\n", + " Point2,\n", + " Point3,\n", + " Pose3,\n", + " Rot3,\n", + " NonlinearFactorGraph,\n", + " SmartProjectionParams,\n", + " SmartProjectionFactorPinholeCameraCal3_S2,\n", + " PinholeCameraCal3_S2,\n", + " Cal3_S2,\n", + ")\n", + "from gtsam.symbol_shorthand import C" ] }, { @@ -102,14 +108,37 @@ }, "outputs": [ { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 8\u001b[0m\n\u001b[0;32m 5\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n\u001b[0;32m 7\u001b[0m \u001b[38;5;66;03m# Add measurements and keys\u001b[39;00m\n\u001b[1;32m----> 8\u001b[0m \u001b[43msmart_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43madd\u001b[49m(Point2(\u001b[38;5;241m150\u001b[39m, \u001b[38;5;241m505\u001b[39m), C(\u001b[38;5;241m0\u001b[39m))\n\u001b[0;32m 9\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m470\u001b[39m, \u001b[38;5;241m495\u001b[39m), C(\u001b[38;5;241m1\u001b[39m))\n\u001b[0;32m 10\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m480\u001b[39m, \u001b[38;5;241m150\u001b[39m), C(\u001b[38;5;241m2\u001b[39m))\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.SmartProjectionFactorPinholeCameraCal3_S2' object has no attribute 'add'" + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 3 measurements.\n", + "SmartFactor: SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "useLOST = 0\n", + "noise model\n", + "\n", + "result:\n", + "no point, status = 1\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "150\n", + "505\n", + "noise model = unit (2) \n", + "measurement 1, px = \n", + "470\n", + "495\n", + "noise model = unit (2) \n", + "measurement 2, px = \n", + "480\n", + "150\n", + "noise model = unit (2) \n", + " keys = { c0 c1 c2 }\n" ] } ], @@ -149,7 +178,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 4, "metadata": { "id": "eval_example_code" }, @@ -159,7 +188,7 @@ "output_type": "stream", "text": [ "Triangulated point result:\n", - "Status.DEGENERATE\n", + "Status.BEHIND_CAMERA\n", "\n", "Triangulation failed, error calculation depends on degeneracyMode.\n" ] @@ -188,110 +217,11 @@ "else:\n", " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")" ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_header_md" - }, - "source": [ - "## Linearization" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_desc_md" - }, - "source": [ - "The `.linearize(values)` method creates a linear factor (e.g., `RegularHessianFactor`) connecting the camera variables, effectively marginalizing the implicit point." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": { - "id": "linearize_example_code" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "Linearized Factor (showing HessianFactor structure):\n", - "RegularHessianFactor(9): density=100% keys={ C0 C1 C2 }\n", - "Augmented information matrix: (dimensions: 9, 9, 9) : \n", - "{\n", - "\t[ 4.383e+04, 2.596e+04, -1.466e+04, -6.732e+03, 1.357e+04, -4.327e+03, -2.004e+04, -1.143e+03, -3.459e+03; ]\n", - "\t[ 2.596e+04, 6.879e+04, 5.416e+03, 3.589e+03, -1.075e+04, 2.158e+04, 3.107e+04, -8.167e+03, -1.237e+04; ]\n", - "\t[ -1.466e+04, 5.416e+03, 2.336e+04, -1.026e+03, -3.572e+03, -1.124e+04, -1.497e+04, -5.631e+03, 4.149e+03; ]\n", - "\t[ -6.732e+03, 3.589e+03, -1.026e+03, 7.027e+03, 1.434e+03, 3.742e+03, 2.527e+03, 2.454e+03, -6.619e+03; ]\n", - "\t[ 1.357e+04, -1.075e+04, -3.572e+03, 1.434e+03, 1.511e+04, -8.935e+02, -1.484e+04, 3.811e+03, -1.993e+03; ]\n", - "\t[ -4.327e+03, 2.158e+04, -1.124e+04, 3.742e+03, -8.935e+02, 2.587e+04, 2.085e+04, -1.193e+04, -5.818e+03; ]\n", - "\t[ -2.004e+04, 3.107e+04, -1.497e+04, 2.527e+03, -1.484e+04, 2.085e+04, 3.128e+04, -5.349e+03, -6.557e+03; ]\n", - "\t[ -1.143e+03, -8.167e+03, -5.631e+03, 2.454e+03, 3.811e+03, -1.193e+04, -5.349e+03, 1.537e+04, -1.987e+03; ]\n", - "\t[ -3.459e+03, -1.237e+04, 4.149e+03, -6.619e+03, -1.993e+03, -5.818e+03, -6.557e+03, -1.987e+03, 1.043e+04; ]\n", - "}\n", - "Augmented Diagonal Block [0,0]:\n", - "[ 4.383e+04, 2.596e+04, -1.466e+04; ]\n", - "[ 2.596e+04, 6.879e+04, 5.416e+03; ]\n", - "[ -1.466e+04, 5.416e+03, 2.336e+04; ]\n", - "\n", - "Augmented Diagonal Block [1,1]:\n", - "[ 7.027e+03, 1.434e+03, 3.742e+03; ]\n", - "[ 1.434e+03, 1.511e+04, -8.935e+02; ]\n", - "[ 3.742e+03, -8.935e+02, 2.587e+04; ]\n", - "\n", - "Augmented Diagonal Block [2,2]:\n", - "[ 3.128e+04, -5.349e+03, -6.557e+03; ]\n", - "[ -5.349e+03, 1.537e+04, -1.987e+03; ]\n", - "[ -6.557e+03, -1.987e+03, 1.043e+04; ]\n", - "\n", - "Off-Diagonal Block [0,1]:\n", - "[ -6.732e+03, 1.357e+04, -4.327e+03; ]\n", - "[ 3.589e+03, -1.075e+04, 2.158e+04; ]\n", - "[ -1.026e+03, -3.572e+03, -1.124e+04; ]\n", - "\n", - "Off-Diagonal Block [0,2]:\n", - "[ -2.004e+04, -1.143e+03, -3.459e+03; ]\n", - "[ 3.107e+04, -8.167e+03, -1.237e+04; ]\n", - "[ -1.497e+04, -5.631e+03, 4.149e+03; ]\n", - "\n", - "Off-Diagonal Block [1,2]:\n", - "[ 2.527e+03, 2.454e+03, -6.619e+03; ]\n", - "[ -1.484e+04, 3.811e+03, -1.993e+03; ]\n", - "[ 2.085e+04, -1.193e+04, -5.818e+03; ]\n", - "\n", - "Error vector:\n", - "[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n", - "Constant error term: 103876\n" - ] - } - ], - "source": [ - "graph = NonlinearFactorGraph()\n", - "graph.add(smart_factor)\n", - "\n", - "# Linearize (default mode is HESSIAN)\n", - "linear_factor = smart_factor.linearize(values)\n", - "\n", - "if linear_factor:\n", - " print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n", - " # Cast to HessianFactor to print its details\n", - " hessian_factor = gtsam.RegularHessianFactorCal3_S2.Downcast(linear_factor)\n", - " if hessian_factor:\n", - " hessian_factor.print()\n", - " else:\n", - " print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n", - "else:\n", - " print(\"Linearization failed (likely due to triangulation degeneracy)\")" - ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -305,7 +235,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/SmartProjectionParams.ipynb b/gtsam/slam/doc/SmartProjectionParams.ipynb index 36e840cb8..cdf1d7291 100644 --- a/gtsam/slam/doc/SmartProjectionParams.ipynb +++ b/gtsam/slam/doc/SmartProjectionParams.ipynb @@ -48,7 +48,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": { "id": "pip_code", "tags": [ @@ -57,12 +57,16 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": { "id": "imports_code" }, @@ -83,7 +87,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 4, "metadata": { "id": "create_example_code" }, @@ -159,7 +163,7 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 5, "metadata": { "id": "usage_example_code" }, @@ -189,13 +193,13 @@ } ], "source": [ - "from gtsam import SmartProjectionPose3Factor, Cal3_S2\n", + "from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n", "\n", "# Example: Using custom params with a smart factor\n", "noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "K = Cal3_S2(500, 500, 0, 320, 240)\n", "\n", - "smart_factor = SmartProjectionPose3Factor(noise, K, custom_params)\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n", "print(\"Smart Factor created with custom params:\")\n", "smart_factor.print()" ] @@ -203,7 +207,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -217,7 +221,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index c8d7f3b2e..f48ff9191 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -40,7 +40,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -49,27 +49,34 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": { "id": "imports_code" }, "outputs": [], "source": [ "import gtsam\n", - "import numpy as np\n", - " # SmartProjectionPoseFactor with Cal3_S2 is called SmartProjectionPose3Factor\n", - "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionPose3Factor,\n", - " Cal3_S2)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X # Key for Pose3 variable" + "from gtsam import (\n", + " Values,\n", + " Point2,\n", + " Point3,\n", + " Pose3,\n", + " Rot3,\n", + " NonlinearFactorGraph,\n", + " SmartProjectionParams,\n", + " SmartProjectionPoseFactorCal3_S2,\n", + " Cal3_S2,\n", + ")\n", + "from gtsam.symbol_shorthand import X # Key for Pose3 variable" ] }, { @@ -93,7 +100,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 3, "metadata": { "id": "create_example_code" }, @@ -125,13 +132,7 @@ "measurement 1, px = \n", "470\n", "495\n", - "noise model = unit (2) \n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ + "noise model = unit (2) \n", "measurement 2, px = \n", "480\n", "150\n", @@ -145,7 +146,7 @@ "smart_params = SmartProjectionParams() # Use default params\n", "K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n", "\n", - "smart_factor = SmartProjectionPose3Factor(smart_noise, K, smart_params)\n", + "smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n", "\n", "# Add measurements and keys (Pose keys)\n", "smart_factor.add(Point2(150, 505), X(0))\n", @@ -176,7 +177,7 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 4, "metadata": { "id": "eval_example_code" }, @@ -236,7 +237,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 5, "metadata": { "id": "linearize_example_code" }, @@ -295,7 +296,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -309,7 +310,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 911f4fd9a..b733d0c6a 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -152,6 +152,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified} +#include + +#include + +template +virtual class SmartFactorBase : gtsam::NonlinearFactor { + void add(const gtsam::Point2& measured, size_t key); + void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys); + size_t dim() const; + const std::vector& measured() const; + std::vector cameras(const gtsam::Values& values) const; +}; + #include /// Linearization mode: what factor to linearize to @@ -176,8 +193,11 @@ class SmartProjectionParams { void print(const std::string& str = "") const; }; -template }> -class SmartProjectionFactor : gtsam::NonlinearFactor { +template +virtual class SmartProjectionFactor : gtsam::SmartFactorBase { SmartProjectionFactor(); SmartProjectionFactor( @@ -238,7 +258,9 @@ class SmartProjectionFactor : gtsam::NonlinearFactor { }; #include -template +// We are not deriving from SmartProjectionFactor yet - too complicated in wrapper +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, const CALIBRATION* K); @@ -262,9 +284,6 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { gtsam::TriangulationResult point(const gtsam::Values& values) const; }; -typedef gtsam::SmartProjectionPoseFactor - SmartProjectionPose3Factor; - // WIP // #include // typedef gtsam::PinholeCamera PinholeCameraCal3_S2; diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 037065ac5..eea1994f2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -38,10 +38,10 @@ for i=0:length(measurements) projectionFactorSeenBy = []; if options.includeCameraFactors == 1 for j=1:options.numberOfLandmarks - SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01); + SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2(0.01); % Use constructor with default values, but express the pose of the % camera as a 90 degree rotation about the X axis -% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... +% SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2( ... % 1, ... % rankTol % -1, ... % linThreshold % false, ... % manageDegeneracy From 603caf18b4101d9f12f567f212e4e38fc8229ab2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 00:17:48 -0400 Subject: [PATCH 34/38] Edited docs only --- gtsam/slam/doc/SmartProjectionFactor.ipynb | 5 ++++- gtsam/slam/doc/SmartProjectionPoseFactor.ipynb | 5 ++++- gtsam/slam/slam.md | 14 +++++++------- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/doc/SmartProjectionFactor.ipynb b/gtsam/slam/doc/SmartProjectionFactor.ipynb index 51746ce34..ec4d3eae7 100644 --- a/gtsam/slam/doc/SmartProjectionFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionFactor.ipynb @@ -26,7 +26,10 @@ "- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n", "\n", "**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n", - "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency." + "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency.\n", + "\n", + "If you are using the factor, please cite:\n", + "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014." ] }, { diff --git a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb index f48ff9191..da3559326 100644 --- a/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb @@ -26,7 +26,10 @@ "- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n", "\n", "**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n", - "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`." + "**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`.\n", + "\n", + "If you are using the factor, please cite:\n", + "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014." ] }, { diff --git a/gtsam/slam/slam.md b/gtsam/slam/slam.md index 4ae7875c7..748743a49 100644 --- a/gtsam/slam/slam.md +++ b/gtsam/slam/slam.md @@ -1,26 +1,24 @@ -# SLAM Factors and Algorithms +# SLAM The `slam` module provides a collection of factors, constraints, utilities, and initialization algorithms commonly used in Simultaneous Localization and Mapping (SLAM) and Structure from Motion (SfM) applications. It builds upon the core GTSAM inference engine (`gtsam/inference`) and geometric types (`gtsam/geometry`). ## Core Factors These are fundamental factor types often used as building blocks in SLAM. - -- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., odometry). -- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable. -- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable. +- [PriorFactor](doc/PriorFactor.ipynb) : A prior factor acting only on the rotation component of a pose variable. +- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., derived from [odometry](https://en.wikipedia.org/wiki/Odometry)). ## Visual SLAM/SfM Factors Factors specifically designed for visual data (camera measurements). - [GenericProjectionFactor](doc/GenericProjectionFactor.ipynb) : Standard monocular projection factor relating a 3D landmark, camera pose, and fixed calibration to a 2D measurement. -- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement. -- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane. - [GeneralSFMFactor](doc/GeneralSFMFactor.ipynb) : Projection factors used when camera calibration is unknown or optimized alongside poses and landmarks. +- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement. - [EssentialMatrixFactor](doc/EssentialMatrixFactor.ipynb) : Factors constraining poses or calibration based on the Essential matrix derived from calibrated cameras. - [EssentialMatrixConstraint](doc/EssentialMatrixConstraint.ipynb) : Factor constraining the relative pose between two cameras based on a measured Essential matrix. - [TriangulationFactor](doc/TriangulationFactor.ipynb) : Factor constraining a 3D point based on a measurement from a single known camera view, useful for triangulation. +- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane. ## Smart Factors @@ -36,6 +34,8 @@ Factors that implicitly manage landmark variables, marginalizing them out during Factors representing various geometric relationships or constraints. +- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable. +- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable. - [OrientedPlane3Factor](doc/OrientedPlane3Factor.ipynb) : Factors for estimating and constraining 3D planar landmarks (`OrientedPlane3`). - [RotateFactor](doc/RotateFactor.ipynb) : Factors constraining an unknown rotation based on how it transforms measured rotations or directions. - [KarcherMeanFactor](doc/KarcherMeanFactor.ipynb) : Factor for constraining the Karcher mean (geometric average) of a set of rotations or other manifold values. From 2cfd2e00a51fbe9ae2e468e9b483476812c434c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 00:22:36 -0400 Subject: [PATCH 35/38] Wrap Rig version --- gtsam/geometry/SimpleCamera.h | 7 +- gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 171 +++++------------- gtsam/slam/slam.i | 78 ++++---- 3 files changed, 86 insertions(+), 170 deletions(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 794451442..db36bd6ba 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -31,10 +31,15 @@ namespace gtsam { /// Convenient aliases for Pinhole camera classes with different calibrations. /// Also needed as forward declarations in the wrapper. + using PinholePoseCal3_S2 = gtsam::PinholePose; + using PinholePoseCal3Bundler = gtsam::PinholePose; + using PinholePoseCal3DS2 = gtsam::PinholePose; + using PinholePoseCal3Unified = gtsam::PinholePose; + using PinholePoseCal3Fisheye = gtsam::PinholePose; using PinholeCameraCal3_S2 = gtsam::PinholeCamera; using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; - + } // namespace gtsam diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index 09d23b3ca..10a9e4092 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -28,7 +28,10 @@ "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n", "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", "\n", - "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized." + "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized.\n", + "\n", + "If you are using the factor, please cite:\n", + "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014.\n" ] }, { @@ -42,7 +45,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -51,38 +54,36 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 9, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n", - " SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n", - " CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X # Key for Pose3 variable (Body Pose)" + "from gtsam import (\n", + " Values,\n", + " Point2,\n", + " Point3,\n", + " Pose3,\n", + " Rot3,\n", + " NonlinearFactorGraph,\n", + " SmartProjectionParams,\n", + " SmartProjectionRigFactorPinholePoseCal3_S2,\n", + " PinholePoseCal3_S2,\n", + " Cal3_S2,\n", + ")\n", + "from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)" ] }, { @@ -107,99 +108,20 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": { "id": "create_example_code" }, "outputs": [ { - "name": "stdout", - "output_type": "stream", - "text": [ - "Smart factor involves 4 measurements from 2 unique poses.\n", - "SmartFactorRig: SmartProjectionRigFactor: \n", - " -- Measurement nr 0\n", - "key: x0\n", - "cameraId: 0\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 0 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "-- Measurement nr 1\n", - "key: x0\n", - "cameraId: 1\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 -0.1 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "-- Measurement nr 2\n", - "key: x1\n", - "cameraId: 0\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 0 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "-- Measurement nr 3\n", - "key: x1\n", - "cameraId: 1\n", - "camera in rig:\n", - "Pose3:\n", - "R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " t: 0.1 -0.1 0\n", - "\n", - "Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n", - "SmartProjectionFactor\n", - "linearizationMode: 0\n", - "triangulationParameters:\n", - "rankTolerance = 1e-09\n", - "enableEPI = false\n", - "landmarkDistanceThreshold = -1\n", - "dynamicOutlierRejectionThreshold = -1\n", - "\n", - "\n", - "result:\n", - "Degenerate\n", - "\n", - "SmartFactorBase, z = \n", - "measurement 0, px = \n", - "[300; 200]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 1, px = \n", - "[250; 201]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 2, px = \n", - "[310; 210]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "measurement 3, px = \n", - "[260; 211]\n", - "\n", - "noise model = diagonal sigmas [1; 1];\n", - "Factor on x0 x1\n" + "ename": "TypeError", + "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[10], line 19\u001b[0m\n\u001b[1;32m 15\u001b[0m smart_params \u001b[38;5;241m=\u001b[39m SmartProjectionParams(linMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mLinearizationMode\u001b[38;5;241m.\u001b[39mHESSIAN,\n\u001b[1;32m 16\u001b[0m degMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mDegeneracyMode\u001b[38;5;241m.\u001b[39mZERO_ON_DEGENERACY)\n\u001b[1;32m 18\u001b[0m \u001b[38;5;66;03m# Factor type includes the Camera type\u001b[39;00m\n\u001b[0;32m---> 19\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionRigFactorPinholePoseCal3_S2\u001b[49m\u001b[43m(\u001b[49m\u001b[43msmart_noise\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrig_cameras\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43msmart_params\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 21\u001b[0m \u001b[38;5;66;03m# 3. Add measurements\u001b[39;00m\n\u001b[1;32m 22\u001b[0m \u001b[38;5;66;03m# Observation from Body Pose X(0), Camera 0\u001b[39;00m\n\u001b[1;32m 23\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m300\u001b[39m, \u001b[38;5;241m200\u001b[39m), X(\u001b[38;5;241m0\u001b[39m), \u001b[38;5;241m0\u001b[39m)\n", + "\u001b[0;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n" ] } ], @@ -208,24 +130,21 @@ "K = Cal3_S2(500, 500, 0, 320, 240)\n", "# Camera 0: Forward facing, slightly offset\n", "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n", - "cam0 = PinholeCameraCal3_S2(body_P_cam0, K)\n", + "cam0 = PinholePoseCal3_S2(body_P_cam0, K)\n", "# Camera 1: Stereo camera, right of cam0\n", "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", - "cam1 = PinholeCameraCal3_S2(body_P_cam1, K)\n", + "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n", "\n", - "rig_cameras = CameraSetPinholeCameraCal3_S2()\n", - "rig_cameras.append(cam0)\n", - "rig_cameras.append(cam1)\n", - "rig_cameras_ptr = gtsam.make_shared_CameraSetPinholeCameraCal3_S2(rig_cameras)\n", + "rig_cameras = [cam0,cam1]\n", "\n", "# 2. Create the Factor\n", - "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", + "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", "# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n", - "smart_params = SmartProjectionParams(linearizationMode=gtsam.LinearizationMode.HESSIAN,\n", - " degeneracyMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", + "smart_params = SmartProjectionParams(linMode=gtsam.LinearizationMode.HESSIAN,\n", + " degMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", "\n", "# Factor type includes the Camera type\n", - "smart_factor = SmartProjectionRigFactorPinholeCameraCal3_S2(smart_noise, rig_cameras_ptr, smart_params)\n", + "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(noise_model, rig_cameras, smart_params)\n", "\n", "# 3. Add measurements\n", "# Observation from Body Pose X(0), Camera 0\n", @@ -261,7 +180,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "metadata": { "id": "eval_example_code" }, @@ -321,7 +240,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": { "id": "linearize_example_code" }, @@ -396,7 +315,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -410,7 +329,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index b733d0c6a..a0c482fdd 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -157,16 +157,22 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { #include -template +template < + CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, + gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2, + gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler, + gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}> virtual class SmartFactorBase : gtsam::NonlinearFactor { void add(const gtsam::Point2& measured, size_t key); void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys); size_t dim() const; const std::vector& measured() const; std::vector cameras(const gtsam::Values& values) const; + + void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; }; #include @@ -193,10 +199,12 @@ class SmartProjectionParams { void print(const std::string& str = "") const; }; -template +template < + CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, + gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2, + gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler, + gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}> virtual class SmartProjectionFactor : gtsam::SmartFactorBase { SmartProjectionFactor(); @@ -204,10 +212,6 @@ virtual class SmartProjectionFactor : gtsam::SmartFactorBase { const gtsam::noiseModel::Base* sharedNoiseModel, const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); - void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; - bool decideIfTriangulate(const gtsam::CameraSet& cameras) const; gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet& cameras) const; bool triangulateForLinearize(const gtsam::CameraSet& cameras) const; @@ -284,41 +288,29 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { gtsam::TriangulationResult point(const gtsam::Values& values) const; }; -// WIP -// #include -// typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -// #include -// template -// class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { -// SmartProjectionRigFactor(); +#include +template +virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { + SmartProjectionRigFactor(); -// SmartProjectionRigFactor( -// const gtsam::noiseModel::Base* sharedNoiseModel, -// const gtsam::CameraSet& cameraRig, -// const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); + SmartProjectionRigFactor( + const gtsam::noiseModel::Base* sharedNoiseModel, + const gtsam::CameraSet* cameraRig, + const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); -// void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey, -// const size_t& cameraId = 0); + void add(const gtsam::Point2& measured, const gtsam::Key& poseKey, + const size_t& cameraId = 0); -// void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys, -// const gtsam::FastVector& cameraIds = gtsam::FastVector()); + void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys, + const gtsam::FastVector& cameraIds = gtsam::FastVector()); -// const gtsam::KeyVector& nonUniqueKeys() const; - -// const gtsam::CameraSet& cameraRig() const; - -// const gtsam::FastVector& cameraIds() const; - -// void print( -// const std::string& s = "", -// const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - -// bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const; - -// gtsam::CameraSet cameras(const gtsam::Values& values) const; - -// double error(const gtsam::Values& values) const; -// }; + const gtsam::KeyVector& nonUniqueKeys() const; + const gtsam::CameraSet& cameraRig() const; + const gtsam::FastVector& cameraIds() const; +}; #include template From 8bdf5326c022b42fcbf12ae22fb91f94e1447e92 Mon Sep 17 00:00:00 2001 From: p-zach Date: Sun, 27 Apr 2025 12:43:10 -0400 Subject: [PATCH 36/38] Fix ReferenceFrameFactor --- gtsam/slam/doc/ReferenceFrameFactor.ipynb | 38 +++++++---------------- gtsam/slam/slam.i | 2 +- 2 files changed, 12 insertions(+), 28 deletions(-) diff --git a/gtsam/slam/doc/ReferenceFrameFactor.ipynb b/gtsam/slam/doc/ReferenceFrameFactor.ipynb index b60f9fa4d..a0fd0388d 100644 --- a/gtsam/slam/doc/ReferenceFrameFactor.ipynb +++ b/gtsam/slam/doc/ReferenceFrameFactor.ipynb @@ -61,7 +61,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "[]\n" + "['ReferenceFrameFactorPoint3Pose3']\n" ] } ], @@ -72,34 +72,18 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 3, "metadata": { "id": "imports_code" }, - "outputs": [ - { - "ename": "ImportError", - "evalue": "cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[1], line 5\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# The Python wrapper creates specific instantiations\u001b[39;00m\n\u001b[1;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m ReferenceFrameFactorPoint3Pose3\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 8\u001b[0m L \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mL \u001b[38;5;66;03m# Global landmark\u001b[39;00m\n", - "\u001b[1;31mImportError\u001b[0m: cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)" - ] - } - ], + "outputs": [], "source": [ "import gtsam\n", "import numpy as np\n", "from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n", "# The Python wrapper creates specific instantiations\n", "from gtsam import ReferenceFrameFactorPoint3Pose3\n", - "from gtsam import symbol_shorthand\n", - "\n", - "L = symbol_shorthand.L # Global landmark\n", - "T = symbol_shorthand.T # Transform global_T_local\n", - "l = symbol_shorthand.l # Local landmark" + "from gtsam.symbol_shorthand import L, T, O" ] }, { @@ -123,7 +107,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 4, "metadata": { "id": "create_example_code" }, @@ -132,15 +116,15 @@ "name": "stdout", "output_type": "stream", "text": [ - "ReferenceFrameFactor: : ReferenceFrameFactor(Global: L0, Transform: T0, Local: l0)\n", - " noise model: diagonal sigmas [0.1; 0.1; 0.1];\n" + "ReferenceFrameFactor: : ReferenceFrameFactor(Global: l0, Transform: t0, Local: o0)\n", + "isotropic dim=3 sigma=0.1\n" ] } ], "source": [ "global_landmark_key = L(0)\n", "transform_key = T(0)\n", - "local_landmark_key = l(0)\n", + "local_landmark_key = O(0)\n", "\n", "# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n", "noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n", @@ -173,7 +157,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 5, "metadata": { "id": "eval_example_code" }, @@ -184,8 +168,8 @@ "text": [ "Expected local landmark: [ 2. -4. 1.]\n", "\n", - "Error at ground truth: [-0. 0. 0.] (Should be zero)\n", - "Error with noisy local landmark: [-1. 1. -0.5]\n" + "Error at ground truth: 4500.0 (Should be zero)\n", + "Error with noisy local landmark: 4621.125\n" ] } ], diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index a0c482fdd..0b03000a9 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -342,7 +342,7 @@ typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include -template +template class ReferenceFrameFactor : gtsam::NoiseModelFactor { ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey, gtsam::Key localKey, const gtsam::noiseModel::Base* model); From cbc45c9f4ef9e9bae9bceeebc880e28ddddb4a08 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 12:47:37 -0400 Subject: [PATCH 37/38] Working rig ! --- gtsam/geometry/geometry.i | 2 +- gtsam/geometry/triangulation.h | 7 + gtsam/slam/SmartProjectionRigFactor.h | 24 ++ gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 216 +++++++++--------- gtsam/slam/slam.i | 2 + 5 files changed, 144 insertions(+), 107 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index b5f6f1f3f..87bdde7c9 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1317,7 +1317,7 @@ class Similarity3 { double scale() const; }; -template +template class CameraSet { CameraSet(); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 749824845..a715f32f9 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } // Vector of Cameras - used by the Python/MATLAB wrapper +using CameraSetPinholePoseCal3Bundler = CameraSet>; +using CameraSetPinholePoseCal3_S2 = CameraSet>; +using CameraSetPinholePoseCal3DS2 = CameraSet>; +using CameraSetPinholePoseCal3Fisheye = CameraSet>; +using CameraSetPinholePoseCal3Unified = CameraSet>; + using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; + using CameraSetSpherical = CameraSet; } // \namespace gtsam diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index dd4237299..b8d8e42dc 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -46,6 +46,30 @@ namespace gtsam { * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor * instead! If the calibration should be optimized, as well, use * SmartProjectionFactor instead! + * + * Note on Template Parameter `CAMERA`: + * While this factor is templated on `CAMERA` to allow for generality (e.g., + * `SphericalCamera`), the current internal implementation for linearization + * (specifically, methods like `createHessianFactor` involving Schur complement + * calculations inherited or adapted from base classes) has limitations. It + * implicitly assumes that the CAMERA only has a Pose3 unknown. + * + * Consequently: + * - This factor works correctly with `CAMERA` types where this is the case, + * such as `PinholePose` or `SphericalCamera`. + * - Using `CAMERA` types where `dimension != 6`, such as + * `PinholeCamera` (which has dimension `6 + CalDim`), will lead + * to compilation errors due to matrix dimension mismatches. + * + * Therefore, for standard pinhole cameras within a fixed rig, use + * `PinholePose` as the `CAMERA` template parameter when defining + * the `CameraSet` passed to this factor's constructor. + * + * TODO(dellaert): Refactor the internal linearization logic (e.g., in + * `createHessianFactor`) to explicitly compute Jacobians with respect to the + * 6-DoF body pose by correctly applying the chain rule, rather than relying on + * `traits::dimension` for downstream calculations. + * * @ingroup slam */ template diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index 10a9e4092..1395440f1 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -63,7 +63,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 1, "metadata": { "id": "imports_code" }, @@ -81,6 +81,7 @@ " SmartProjectionParams,\n", " SmartProjectionRigFactorPinholePoseCal3_S2,\n", " PinholePoseCal3_S2,\n", + " CameraSetPinholePoseCal3_S2,\n", " Cal3_S2,\n", ")\n", "from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)" @@ -108,20 +109,112 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 3, "metadata": { "id": "create_example_code" }, "outputs": [ { - "ename": "TypeError", - "evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[10], line 19\u001b[0m\n\u001b[1;32m 15\u001b[0m smart_params \u001b[38;5;241m=\u001b[39m SmartProjectionParams(linMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mLinearizationMode\u001b[38;5;241m.\u001b[39mHESSIAN,\n\u001b[1;32m 16\u001b[0m degMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mDegeneracyMode\u001b[38;5;241m.\u001b[39mZERO_ON_DEGENERACY)\n\u001b[1;32m 18\u001b[0m \u001b[38;5;66;03m# Factor type includes the Camera type\u001b[39;00m\n\u001b[0;32m---> 19\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionRigFactorPinholePoseCal3_S2\u001b[49m\u001b[43m(\u001b[49m\u001b[43msmart_noise\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrig_cameras\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43msmart_params\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 21\u001b[0m \u001b[38;5;66;03m# 3. Add measurements\u001b[39;00m\n\u001b[1;32m 22\u001b[0m \u001b[38;5;66;03m# Observation from Body Pose X(0), Camera 0\u001b[39;00m\n\u001b[1;32m 23\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m300\u001b[39m, \u001b[38;5;241m200\u001b[39m), X(\u001b[38;5;241m0\u001b[39m), \u001b[38;5;241m0\u001b[39m)\n", - "\u001b[0;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n" + "name": "stdout", + "output_type": "stream", + "text": [ + "Smart factor involves 2 measurements from 2 unique poses.\n", + "SmartFactorRig: SmartProjectionRigFactor: \n", + " -- Measurement nr 0\n", + "key: x0\n", + "cameraId: 0\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "-- Measurement nr 1\n", + "key: x0\n", + "cameraId: 1\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 -0.1 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "-- Measurement nr 2\n", + "key: x1\n", + "cameraId: 0\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "-- Measurement nr 3\n", + "key: x1\n", + "cameraId: 1\n", + "camera in rig:\n", + ".pose R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t-0, 0, 1\n", + "]\n", + "t: 0.1 -0.1 0\n", + "camera in rig:\n", + ".calibration[\n", + "\t500, 0, 320;\n", + "\t0, 500, 240;\n", + "\t0, 0, 1\n", + "]\n", + "SmartProjectionFactor\n", + "linearizationMode: 0\n", + "triangulationParameters:\n", + "rankTolerance = 1\n", + "enableEPI = 0\n", + "landmarkDistanceThreshold = -1\n", + "dynamicOutlierRejectionThreshold = -1\n", + "useLOST = 0\n", + "noise model\n", + "\n", + "result:\n", + "no point, status = 1\n", + "\n", + "SmartFactorBase, z = \n", + "measurement 0, px = \n", + "300\n", + "200\n", + "noise model = unit (2) \n", + "measurement 1, px = \n", + "250\n", + "201\n", + "noise model = unit (2) \n", + "measurement 2, px = \n", + "310\n", + "210\n", + "noise model = unit (2) \n", + "measurement 3, px = \n", + "260\n", + "211\n", + "noise model = unit (2) \n", + " keys = { x0 x1 }\n" ] } ], @@ -135,7 +228,9 @@ "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n", "\n", - "rig_cameras = [cam0,cam1]\n", + "rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n", + "rig_cameras.push_back(cam0)\n", + "rig_cameras.push_back(cam1)\n", "\n", "# 2. Create the Factor\n", "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", @@ -180,7 +275,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 4, "metadata": { "id": "eval_example_code" }, @@ -190,9 +285,10 @@ "output_type": "stream", "text": [ "Triangulated point result:\n", - "Valid triangulation with point [0.70307883 0.20615766 5.18676602]\n", + "\n", "\n", - "Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n" + "Triangulation failed, error calculation depends on degeneracyMode.\n", + "Error when degenerate: 0.0\n" ] } ], @@ -219,98 +315,6 @@ " total_error = smart_factor.error(values)\n", " print(f\"Error when degenerate: {total_error}\")" ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_header_md" - }, - "source": [ - "## Linearization" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "linearize_desc_md" - }, - "source": [ - "Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "id": "linearize_example_code" - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "Linearized Factor (HessianFactor structure):\n", - "RegularHessianFactor(6): density=100% keys={ x0 x1 }\n", - "Augmented information matrix: (dimensions: 6, 6) : \n", - "{\n", - "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", - "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", - "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", - "\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\n", - "\t[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\n", - "\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\n", - "\t[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\n", - "\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n", - "\t[ 2.81, 18.91, 6.302, -2.81, -18.91, -6.302; ]\n", - "\t[ -15.09, -101.6, -33.87, 15.09, 101.6, 33.87; ]\n", - "\t[ 3.829, 25.77, 8.589, -3.829, -25.77, -8.589; ]\n", - "\t[ -3.448, -23.21, -7.737, 3.448, 23.21, 7.737; ]\n", - "\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n", - "\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n", - "\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n", - "\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\n", - "\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\n", - "\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\n", - "}\n", - "Augmented Diagonal Block [0,0]:\n", - "[ 1.257, 8.427, 2.81; ]\n", - "[ 8.427, 56.73, 18.91; ]\n", - "[ 2.81, 18.91, 6.302; ]\n", - "\n", - "Augmented Diagonal Block [1,1]:\n", - "[ 1.257, 8.427, 2.81; ]\n", - "[ 8.427, 56.73, 18.91; ]\n", - "[ 2.81, 18.91, 6.302; ]\n", - "\n", - "Off-Diagonal Block [0,1]:\n", - "[ -1.257, -8.427, -2.81; ]\n", - "[ -8.427, -56.73, -18.91; ]\n", - "[ -2.81, -18.91, -6.302; ]\n", - "\n", - "Error vector:\n", - "[-15.087; -101.588; -33.8672; 181.19; -46.1294; 41.5391; 15.087; 101.588; 33.8672; -181.19; 46.1294; -41.5391]\n", - "Constant error term: 181.19\n" - ] - } - ], - "source": [ - "graph = NonlinearFactorGraph()\n", - "graph.add(smart_factor)\n", - "\n", - "# Linearize (HESSIAN mode)\n", - "linear_factor = smart_factor.linearize(values)\n", - "\n", - "if linear_factor:\n", - " print(\"\\nLinearized Factor (HessianFactor structure):\")\n", - " hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n", - " if hessian_factor:\n", - " hessian_factor.print()\n", - " else:\n", - " print(\"Linearized factor is not a HessianFactor\")\n", - "else:\n", - " print(\"Linearization failed (likely due to triangulation degeneracy)\")" - ] } ], "metadata": { diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 0b03000a9..ec6d135b2 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -157,6 +157,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { #include +// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3 template < CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, @@ -289,6 +290,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { }; #include +// Only for PinholePose cameras -> PinholeCamera is not supported template Date: Sun, 27 Apr 2025 13:01:03 -0400 Subject: [PATCH 38/38] Better docs --- gtsam/slam/doc/SmartProjectionRigFactor.ipynb | 525 +++++++++++++----- 1 file changed, 385 insertions(+), 140 deletions(-) diff --git a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb index 1395440f1..6d1e38210 100644 --- a/gtsam/slam/doc/SmartProjectionRigFactor.ipynb +++ b/gtsam/slam/doc/SmartProjectionRigFactor.ipynb @@ -24,12 +24,19 @@ "- **Multiple Observations per Pose:** Allows multiple measurements associated with the *same* body pose key, but originating from different cameras within the rig.\n", "- **Camera Indexing:** Each measurement must be associated with both a body pose key and a `cameraId` (index) specifying which camera in the rig took the measurement.\n", "- **Fixed Calibration/Extrinsics:** The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.\n", - "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholeCamera`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts.\n", + "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholePose`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts. **Important Note:** See the **Note on Template Parameter `CAMERA`** below.\n", "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n", - "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", + "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of C++ header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n", "\n", "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized.\n", "\n", + "**Note on Template Parameter `CAMERA`:**\n", + "While this factor is templated on `CAMERA` for generality, the current internal implementation for linearization has limitations. It implicitly assumes that `traits::dimension` matches the optimized variable dimension (`Pose3::dimension`, which is 6).\n", + "Consequently:\n", + "- It works correctly with `CAMERA` types where `dimension == 6`, such as `PinholePose` or `SphericalCamera`.\n", + "- Using `CAMERA` types where `dimension != 6`, such as `PinholeCamera` (dim = 6 + CalDim), **will cause compilation errors**.\n", + "- **Recommendation:** For standard pinhole cameras in a fixed rig, **use `PinholePose`** as the `CAMERA` type when defining the `CameraSet` for this factor.\n", + "\n", "If you are using the factor, please cite:\n", "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014.\n" ] @@ -63,25 +70,25 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, "outputs": [], "source": [ "import gtsam\n", - "import numpy as np\n", "from gtsam import (\n", " Values,\n", " Point2,\n", " Point3,\n", " Pose3,\n", " Rot3,\n", - " NonlinearFactorGraph,\n", " SmartProjectionParams,\n", + " LinearizationMode,\n", + " DegeneracyMode,\n", + " # Use PinholePose variant for wrapping\n", " SmartProjectionRigFactorPinholePoseCal3_S2,\n", " PinholePoseCal3_S2,\n", - " CameraSetPinholePoseCal3_S2,\n", " Cal3_S2,\n", ")\n", "from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)" @@ -93,98 +100,34 @@ "id": "create_header_md" }, "source": [ - "## Creating the Rig and Factor" + "## Constructor" ] }, { "cell_type": "markdown", "metadata": { - "id": "create_desc_md" + "id": "constructor_desc_md" }, "source": [ - "1. Define the camera rig configuration: Create a `CameraSet` containing the `CAMERA` objects (with fixed intrinsics and rig-relative extrinsics).\n", - "2. Create the `SmartProjectionRigFactor` with noise, the rig, and parameters.\n", - "3. Add measurements, specifying the 2D point, the corresponding **body pose key**, and the **camera ID** within the rig." + "You create a `SmartProjectionRigFactor` by providing:\n", + "1. A noise model for the 2D pixel measurements (typically `noiseModel.Isotropic`).\n", + "2. A `CameraSet` object defining the *fixed* rig configuration. Each `CAMERA` in the set contains its fixed intrinsics and its fixed pose relative to the rig's body frame (`body_P_camera`).\n", + "3. Optionally, `SmartProjectionParams` to configure linearization and degeneracy handling. Remember the current restrictions (HESSIAN, ZERO_ON_DEGENERACY)." ] }, { "cell_type": "code", "execution_count": 3, "metadata": { - "id": "create_example_code" + "id": "constructor_example_code" }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Smart factor involves 2 measurements from 2 unique poses.\n", "SmartFactorRig: SmartProjectionRigFactor: \n", - " -- Measurement nr 0\n", - "key: x0\n", - "cameraId: 0\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 0 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "-- Measurement nr 1\n", - "key: x0\n", - "cameraId: 1\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 -0.1 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "-- Measurement nr 2\n", - "key: x1\n", - "cameraId: 0\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 0 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "-- Measurement nr 3\n", - "key: x1\n", - "cameraId: 1\n", - "camera in rig:\n", - ".pose R: [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t-0, 0, 1\n", - "]\n", - "t: 0.1 -0.1 0\n", - "camera in rig:\n", - ".calibration[\n", - "\t500, 0, 320;\n", - "\t0, 500, 240;\n", - "\t0, 0, 1\n", - "]\n", - "SmartProjectionFactor\n", + " SmartProjectionFactor\n", "linearizationMode: 0\n", "triangulationParameters:\n", "rankTolerance = 1\n", @@ -198,122 +141,424 @@ "no point, status = 1\n", "\n", "SmartFactorBase, z = \n", - "measurement 0, px = \n", - "300\n", - "200\n", - "noise model = unit (2) \n", - "measurement 1, px = \n", - "250\n", - "201\n", - "noise model = unit (2) \n", - "measurement 2, px = \n", - "310\n", - "210\n", - "noise model = unit (2) \n", - "measurement 3, px = \n", - "260\n", - "211\n", - "noise model = unit (2) \n", - " keys = { x0 x1 }\n" + " keys = { }\n" ] } ], "source": [ - "# 1. Define the Camera Rig\n", + "# Define the Camera Rig (using PinholePose)\n", "K = Cal3_S2(500, 500, 0, 320, 240)\n", - "# Camera 0: Forward facing, slightly offset\n", "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n", "cam0 = PinholePoseCal3_S2(body_P_cam0, K)\n", - "# Camera 1: Stereo camera, right of cam0\n", - "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", + "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n", "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n", "\n", "rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n", "rig_cameras.push_back(cam0)\n", "rig_cameras.push_back(cam1)\n", "\n", - "# 2. Create the Factor\n", + "# Noise model and parameters\n", "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n", - "# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n", - "smart_params = SmartProjectionParams(linMode=gtsam.LinearizationMode.HESSIAN,\n", - " degMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n", + "smart_params = SmartProjectionParams(\n", + " linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY\n", + ")\n", "\n", - "# Factor type includes the Camera type\n", - "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(noise_model, rig_cameras, smart_params)\n", - "\n", - "# 3. Add measurements\n", - "# Observation from Body Pose X(0), Camera 0\n", - "smart_factor.add(Point2(300, 200), X(0), 0)\n", - "# Observation from Body Pose X(0), Camera 1 (stereo pair?)\n", - "smart_factor.add(Point2(250, 201), X(0), 1)\n", - "# Observation from Body Pose X(1), Camera 0\n", - "smart_factor.add(Point2(310, 210), X(1), 0)\n", - "# Observation from Body Pose X(1), Camera 1\n", - "smart_factor.add(Point2(260, 211), X(1), 1)\n", - "\n", - "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n", + "# Create the Factor\n", + "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(\n", + " noise_model, rig_cameras, smart_params\n", + ")\n", "smart_factor.print(\"SmartFactorRig: \")" ] }, { "cell_type": "markdown", "metadata": { - "id": "eval_header_md" + "id": "add_header_md" }, "source": [ - "## Evaluating the Error" + "## `add(measurement, poseKey, cameraId)`" ] }, { "cell_type": "markdown", "metadata": { - "id": "eval_desc_md" + "id": "add_desc_md" }, "source": [ - "The `.error(values)` method uses the `Pose3` objects (body poses) from `values` and the fixed rig configuration to triangulate the point and compute the error." + "This method adds a single 2D measurement (`Point2`) associated with a specific camera in the rig and a specific body pose.\n", + "- `measurement`: The observed pixel coordinates.\n", + "- `poseKey`: The key (`Symbol` or `Key`) of the **body's `Pose3`** variable at the time of observation.\n", + "- `cameraId`: The integer index of the camera within the `CameraSet` (provided during construction) that captured this measurement." ] }, { "cell_type": "code", "execution_count": 4, "metadata": { - "id": "eval_example_code" + "id": "add_example_code" }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Triangulated point result:\n", - "\n", - "\n", - "Triangulation failed, error calculation depends on degeneracyMode.\n", - "Error when degenerate: 0.0\n" + "Smart factor involves 2 measurements from 2 unique poses.\n" ] } ], "source": [ - "# Create Values containing Body Pose3 objects\n", + "# --- Use Pre-calculated Valid Measurements ---\n", + "# These measurements were calculated offline using:\n", + "# gt_landmark = Point3(1.0, 0.5, 5.0)\n", + "# gt_pose0 = Pose3()\n", + "# gt_pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n", + "# And the rig defined above.\n", + "\n", + "z00 = Point2(400.0, 290.0) # Measurement from Body Pose X(0), Camera 0\n", + "z01 = Point2(350.0, 290.0) # Measurement from Body Pose X(0), Camera 1\n", + "z10 = Point2(372.787, 297.553) # Measurement from Body Pose X(1), Camera 0\n", + "z11 = Point2(323.308, 297.674) # Measurement from Body Pose X(1), Camera 1\n", + "# --------------------------------------------\n", + "\n", + "# 3. Add pre-calculated measurements\n", + "smart_factor.add(z00, X(0), 0)\n", + "smart_factor.add(z01, X(0), 1)\n", + "smart_factor.add(z10, X(1), 0)\n", + "smart_factor.add(z11, X(1), 1)\n", + "\n", + "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n", + "# smart_factor.print(\"SmartFactorRig (with pre-calculated measurements): \") # Optional\n" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "inherited_header_md" + }, + "source": [ + "## Inherited and Core Methods" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "error_header_md" + }, + "source": [ + "### `error(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "error_desc_md" + }, + "source": [ + "Inherited from `SmartFactorBase`. Calculates the total reprojection error for the landmark.\n", + "**Internal Process:**\n", + "1. Retrieves the body `Pose3` estimates for all relevant keys from the `values` object.\n", + "2. For each measurement, calculates the corresponding camera's world pose using the body pose and the fixed rig extrinsics (`world_P_sensor = world_P_body * body_P_camera`).\n", + "3. Triangulates the 3D landmark position using these calculated camera poses and the stored 2D measurements.\n", + "4. Reprojects the triangulated point back into each calculated camera view.\n", + "5. Computes the sum of squared differences between the reprojections and the original measurements, weighted by the noise model.\n", + "6. Handles degenerate cases (e.g., failed triangulation) based on the `degeneracyMode` set in `SmartProjectionParams`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": { + "id": "error_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulation Result Status: Status.VALID\n", + "Total reprojection error (0.5 * sum(err^2/sigma^2)): 1316.4717\n" + ] + } + ], + "source": [ + "# Assuming smart_factor created and measurements added above\n", + "\n", "values = Values()\n", - "pose0 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))\n", - "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n", + "pose0 = Pose3() # Body at origin\n", + "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0)) # Body moved\n", "values.insert(X(0), pose0)\n", "values.insert(X(1), pose1)\n", "\n", - "# Triangulate first to see the implicit point\n", - "# The 'cameras' method internally combines body poses with rig extrinsics\n", + "# Need to check triangulation first, as error calculation depends on it\n", "point_result = smart_factor.point(values)\n", - "print(f\"Triangulated point result:\\n{point_result}\")\n", + "print(f\"Triangulation Result Status: {point_result.status}\")\n", + "\n", + "total_error = smart_factor.error(values)\n", + "print(f\"Total reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + "# Note: If degenerate and DegeneracyMode is ZERO_ON_DEGENERACY, error will be 0." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "point_header_md" + }, + "source": [ + "### `point(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "point_desc_md" + }, + "source": [ + "Inherited from `SmartProjectionFactor`. Performs the internal triangulation based on the body poses in `values` and the fixed rig geometry.\n", + "Returns a `TriangulationResult` object which contains:\n", + "- The triangulated `Point3` (if successful).\n", + "- A status indicating whether the triangulation was `VALID`, `DEGENERATE`, `BEHIND_CAMERA`, `OUTLIER`, or `FAR_POINT`." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": { + "id": "point_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Triangulation Result Status: Status.VALID\n", + "Triangulated Point: [0.94370846 0.79793704 7.63497051]\n" + ] + } + ], + "source": [ + "# Assuming smart_factor and values from the previous cell\n", + "point_result = smart_factor.point(values)\n", + "print(f\"Triangulation Result Status: {point_result.status}\")\n", "\n", "if point_result.valid():\n", - " # Calculate error\n", - " total_error = smart_factor.error(values)\n", - " print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n", + " triangulated_point = point_result.get()\n", + " print(f\"Triangulated Point: {triangulated_point}\")\n", "else:\n", - " print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")\n", - " # Since mode is ZERO_ON_DEGENERACY, error should be 0\n", - " total_error = smart_factor.error(values)\n", - " print(f\"Error when degenerate: {total_error}\")" + " print(\"Triangulation did not produce a valid point.\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "cameras_header_md" + }, + "source": [ + "### `cameras(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "cameras_desc_md" + }, + "source": [ + "Inherited from `SmartFactorBase`. Computes and returns a `CameraSet` containing the effective cameras corresponding to *each measurement*.\n", + "For each measurement `i` associated with body pose key `k` and camera ID `cid`:\n", + "1. Retrieves the body pose `world_P_body = values.atPose3(k)`.\n", + "2. Retrieves the fixed relative pose `body_P_camera = rig_cameras.at(cid).pose()`.\n", + "3. Computes the camera's world pose `world_P_camera = world_P_body * body_P_camera`.\n", + "4. Creates a `CAMERA` object using this `world_P_camera` and the fixed intrinsics `rig_cameras.at(cid).calibration()`.\n", + "The returned `CameraSet` contains these calculated cameras, one for each measurement added via `add()`." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": { + "id": "cameras_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Pose of camera for measurement 0 (Body X(0), Cam 0):\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 0 0\n", + "\n", + "\n", + "Pose of camera for measurement 1 (Body X(0), Cam 1):\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, 1, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.1 -0.1 0\n", + "\n", + "\n", + "Pose of camera for measurement 2 (Body X(1), Cam 0):\n", + "R: [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.5995 0.00998334 0\n", + "\n", + "\n", + "Pose of camera for measurement 3 (Body X(1), Cam 1):\n", + "R: [\n", + "\t0.995004, -0.0998334, 0;\n", + "\t0.0998334, 0.995004, 0;\n", + "\t0, 0, 1\n", + "]\n", + "t: 0.609484 -0.0895171 0\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Assuming smart_factor and values from previous cells\n", + "\n", + "calculated_cameras = smart_factor.cameras(values)\n", + "\n", + "# Print the world pose of each calculated camera\n", + "print(f\"Pose of camera for measurement 0 (Body X(0), Cam 0):\\n{calculated_cameras.at(0).pose()}\\n\")\n", + "print(f\"Pose of camera for measurement 1 (Body X(0), Cam 1):\\n{calculated_cameras.at(1).pose()}\\n\")\n", + "print(f\"Pose of camera for measurement 2 (Body X(1), Cam 0):\\n{calculated_cameras.at(2).pose()}\\n\")\n", + "print(f\"Pose of camera for measurement 3 (Body X(1), Cam 1):\\n{calculated_cameras.at(3).pose()}\\n\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_header_md" + }, + "source": [ + "### `linearize(Values values)`" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "linearize_desc_md" + }, + "source": [ + "Inherited from `SmartProjectionFactor`. Computes the linear approximation (GaussianFactor) of the factor at the linearization point defined by `values`.\n", + "For `SmartProjectionRigFactor`, due to current implementation limitations, this **must** be configured via `SmartProjectionParams` to use `LinearizationMode.HESSIAN`.\n", + "The resulting `RegularHessianFactor` connects the **unique body pose keys** involved in the measurements." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": { + "id": "linearize_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Linearized Factor (HessianFactor structure):\n", + "Linear Factor: \n", + " keys: x0(6) x1(6) \n", + "Augmented information matrix: [\n", + "\t255621, 1454.13, -31747.6, 636.066, -33103.6, 3605.16, -254669, 22279.1, 15195.9, 2671.95, 33001.7, -3605.16, -5437.65;\n", + "\t1454.13, 9642.56, -1187.49, 1253.63, -198.336, -75.3949, -2405.75, -9411.71, 1088.32, -1227.56, 322.499, 75.3949, -653.552;\n", + "\t-31747.6, -1187.49, 4048.22, -209.638, 4112.44, -437.73, 31729.4, -1770.15, -1992, -201.969, -4112.82, 437.73, 740.416;\n", + "\t636.066, 1253.63, -209.638, 163.769, -83.6702, -3.45048, -757.87, -1182.15, 167.803, -154.598, 99.6018, 3.45048, -94.317;\n", + "\t-33103.6, -198.336, 4112.44, -83.6702, 4287, -466.758, 32981.3, -2875.28, -1968.94, -344.734, -4273.93, 466.758, 704.833;\n", + "\t3605.16, -75.3949, -437.73, -3.45048, -466.758, 51.9764, -3582.21, 409.075, 204.351, 50.0313, 464.082, -51.9764, -70.5256;\n", + "\t-254669, -2405.75, 31729.4, -757.87, 32981.3, -3582.21, 253816, -21248.6, -15238.8, -2538.55, -32892.2, 3582.21, 5479.25;\n", + "\t22279.1, -9411.71, -1770.15, -1182.15, -2875.28, 409.075, -21248.6, 11385.4, 332.508, 1463.29, " + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "2742.9, -409.075, 142.514;\n", + "\t15195.9, 1088.32, -1992, 167.803, -1968.94, 204.351, -15238.8, 332.508, 1007.53, 29.6019, 1975.86, -204.351, -387.999;\n", + "\t2671.95, -1227.56, -201.969, -154.598, -344.734, 50.0313, -2538.55, 1463.29, 29.6019, 188.241, 327.577, -50.0313, 23.48;\n", + "\t33001.7, 322.499, -4112.82, 99.6018, -4273.93, 464.082, -32892.2, 2742.9, 1975.86, 327.577, 4262.53, -464.082, -710.727;\n", + "\t-3605.16, 75.3949, 437.73, 3.45048, 466.758, -51.9764, 3582.21, -409.075, -204.351, -50.0313, -464.082, 51.9764, 70.5256;\n", + "\t-5437.65, -653.552, 740.416, -94.317, 704.833, -70.5256, 5479.25, 142.514, -387.999, 23.48, -710.727, 70.5256, 2632.94\n", + "]\n" + ] + } + ], + "source": [ + "# Assuming smart_factor and values from previous cells\n", + "\n", + "# Check if triangulation succeeded before linearizing\n", + "if not smart_factor.point(values).valid():\n", + " print(\"Cannot linearize: triangulation failed or degenerate.\")\n", + "else:\n", + " linear_factor = smart_factor.linearize(values)\n", + "\n", + " if linear_factor:\n", + " print(\"\\nLinearized Factor (HessianFactor structure):\")\n", + " linear_factor.print(\"Linear Factor: \")\n", + " else:\n", + " print(\"\\nLinearization failed (potentially due to triangulation degeneracy and params setting).\")\n", + "\n", + "# Note: The printed Hessian is often zero when ZERO_ON_DEGENERACY is used and triangulation fails." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "other_methods_header_md" + }, + "source": [ + "### Other Inherited Methods" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "other_methods_desc_md" + }, + "source": [ + "The factor also inherits standard methods from `NonlinearFactor` and `SmartFactorBase`:\n", + "- **`keys()`**: Returns a `KeyVector` containing the **unique body pose keys** involved.\n", + "- **`measured()`**: Returns a `Point2Vector` containing all the added 2D measurements.\n", + "- **`dim()`**: Returns the dimension of the error vector (2 * number of measurements).\n", + "- **`size()`**: Returns the number of measurements added.\n", + "- **`print(s, keyFormatter)`**: Prints details about the factor.\n", + "- **`equals(other, tol)`**: Compares two factors for equality." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": { + "id": "other_methods_example_code" + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Keys: ['x0', 'x1']\n", + "Number of Measurements (size): 2\n", + "Dimension (dim): 8\n", + "Measurements: [array([400., 290.]), array([350., 290.]), array([372.787, 297.553]), array([323.308, 297.674])]\n" + ] + } + ], + "source": [ + "# Assuming smart_factor from previous cells\n", + "print(f\"Keys: {[gtsam.DefaultKeyFormatter(k) for k in smart_factor.keys()]}\")\n", + "print(f\"Number of Measurements (size): {smart_factor.size()}\")\n", + "print(f\"Dimension (dim): {smart_factor.dim()}\")\n", + "print(f\"Measurements: {smart_factor.measured()}\")" ] } ],