12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'borglab:develop' into cibw-prod

release/4.3a0
Yashas Ambati 2025-05-01 02:13:38 -04:00 committed by GitHub
parent bdd0d3e759 42cc0ac922
commit a9a79397a9
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
51 changed files with 26541 additions and 226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

259
examples/ABC.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
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 <size_t N>
class State {
public:
Rot3 R; // Attitude rotation matrix R
Vector3 b; // Gyroscope bias b
std::array<Rot3, N> S; // Sensor calibrations S
/// Constructor
State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(),
const std::array<Rot3, N>& S = std::array<Rot3, N>{})
: R(R), b(b), S(S) {}
/// Identity function
static State identity() {
std::array<Rot3, N> 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<N>& 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<Eigen::Index>(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<Rot3, N> 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 <size_t N>
struct G {
Rot3 A; /// First SO(3) element
Matrix3 a; /// so(3) element (skew-symmetric matrix)
std::array<Rot3, N> 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<Rot3, N>& B = std::array<Rot3, N>{})
: A(A), a(a), B(B) {}
/// Group multiplication
G operator*(const G<N>& other) const {
std::array<Rot3, N> 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<Rot3, N> 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<Rot3, N> 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<Eigen::Index>(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<Rot3, N> 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 <size_t N>
struct traits<abc_eqf_lib::State<N>>
: internal::LieGroupTraits<abc_eqf_lib::State<N>> {};
template <size_t N>
struct traits<abc_eqf_lib::G<N>> : internal::LieGroupTraits<abc_eqf_lib::G<N>> {
};
} // namespace gtsam
#endif // ABC_H

519
examples/ABC_EQF.h Normal file
View File

@ -0,0 +1,519 @@
/**
* @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 <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/dataset.h>
#include <chrono>
#include <cmath>
#include <fstream>
#include <functional>
#include <iostream>
#include <numeric> // For std::accumulate
#include <string>
#include <vector>
#include "ABC.h"
// All implementations are wrapped in this namespace to avoid conflicts
namespace gtsam {
namespace abc_eqf_lib {
using namespace std;
using namespace gtsam;
//========================================================================
// Helper Functions for EqF
//========================================================================
/// Calculate numerical differential
Matrix numericalDifferential(std::function<Vector(const Vector&)> f,
const Vector& x);
/**
* Compute the lift of the system (Theorem 3.8, Equation 7)
* @param xi State
* @param u Input
* @return Lift vector
*/
template <size_t N>
Vector lift(const State<N>& xi, const Input& u);
/**
* Action of the symmetry group on the state space (Equation 4)
* @param X Group element
* @param xi State
* @return New state after group action
*/
template <size_t N>
State<N> operator*(const G<N>& X, const State<N>& xi);
/**
* Action of the symmetry group on the input space (Equation 5)
* @param X Group element
* @param u Input
* @return New input after group action
*/
template <size_t N>
Input velocityAction(const G<N>& X, const Input& u);
/**
* Action of the symmetry group on the output space (Equation 6)
* @param X Group element
* @param y Direction measurement
* @param idx Calibration index
* @return New direction after group action
*/
template <size_t N>
Vector3 outputAction(const G<N>& X, const Unit3& y, int idx);
/**
* Differential of the phi action at E = Id in local coordinates
* @param xi State
* @return Differential matrix
*/
template <size_t N>
Matrix stateActionDiff(const State<N>& xi);
//========================================================================
// Equivariant Filter (EqF)
//========================================================================
/// Equivariant Filter (EqF) implementation
template <size_t N>
class EqF {
private:
int dof; // Degrees of freedom
G<N> X_hat; // Filter state
Matrix Sigma; // Error covariance
State<N> xi_0; // Origin state
Matrix Dphi0; // Differential of phi at origin
Matrix InnovationLift; // Innovation lift matrix
/**
* Return the state matrix A0t (Equation 14a)
* @param u Input
* @return State matrix A0t
*/
Matrix stateMatrixA(const Input& u) const;
/**
* Return the state transition matrix Phi (Equation 17)
* @param u Input
* @param dt Time step
* @return State transition matrix Phi
*/
Matrix stateTransitionMatrix(const Input& u, double dt) const;
/**
* Return the Input matrix Bt
* @return Input matrix Bt
*/
Matrix inputMatrixBt() const;
/**
* Return the measurement matrix C0 (Equation 14b)
* @param d Known direction
* @param idx Calibration index
* @return Measurement matrix C0
*/
Matrix measurementMatrixC(const Unit3& d, int idx) const;
/**
* Return the measurement output matrix Dt
* @param idx Calibration index
* @return Measurement output matrix Dt
*/
Matrix outputMatrixDt(int idx) const;
public:
/**
* Initialize EqF
* @param Sigma Initial covariance
* @param m Number of sensors
*/
EqF(const Matrix& Sigma, int m);
/**
* Return estimated state
* @return Current state estimate
*/
State<N> stateEstimate() const;
/**
* Propagate the filter state
* @param u Angular velocity measurement
* @param dt Time step
*/
void propagation(const Input& u, double dt);
/**
* Update the filter state with a measurement
* @param y Direction measurement
*/
void update(const Measurement& y);
};
//========================================================================
// Helper Functions Implementation
//========================================================================
/**
* Maps system dynamics to the symmetry group
* @param xi State
* @param u Input
* @return Lifted input in Lie Algebra
* Uses Vector zero & Rot3 inverse, matrix functions
*/
template <size_t N>
Vector lift(const State<N>& xi, const Input& u) {
Vector L = Vector::Zero(6 + 3 * N);
// First 3 elements
L.head<3>() = u.w - xi.b;
// Next 3 elements
L.segment<3>(3) = -u.W() * xi.b;
// Remaining elements
for (size_t i = 0; i < N; i++) {
L.segment<3>(6 + 3 * i) = xi.S[i].inverse().matrix() * L.head<3>();
}
return L;
}
/**
* Implements group actions on the states
* @param X A symmetry group element G consisting of the attitude, bias and the
* calibration components X.a -> Rotation matrix containing the attitude X.b ->
* A skew-symmetric matrix representing bias X.B -> A vector of Rotation
* matrices for the calibration components
* @param xi State object
* xi.R -> Attitude (Rot3)
* xi.b -> Gyroscope Bias(Vector 3)
* xi.S -> Vector of calibration matrices(Rot3)
* @return Transformed state
* Uses the Rot3 inverse and Vee functions
*/
template <size_t N>
State<N> operator*(const G<N>& X, const State<N>& xi) {
std::array<Rot3, N> new_S;
for (size_t i = 0; i < N; i++) {
new_S[i] = X.A.inverse() * xi.S[i] * X.B[i];
}
return State<N>(xi.R * X.A, X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)),
new_S);
}
/**
* Transforms the angular velocity measurements b/w frames
* @param X A symmetry group element X with the components
* @param u Inputs
* @return Transformed inputs
* Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining
* the input equivariance
*/
template <size_t N>
Input velocityAction(const G<N>& X, const Input& u) {
return Input{X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma};
}
/**
* Transforms the Direction measurements based on the calibration type ( Eqn 6)
* @param X Group element X
* @param y Direction measurement y
* @param idx Calibration index
* @return Transformed direction
* Uses Rot3 inverse, matric and Unit3 unitvector functions
*/
template <size_t N>
Vector3 outputAction(const G<N>& X, const Unit3& y, int idx) {
if (idx == -1) {
return X.A.inverse().matrix() * y.unitVector();
} else {
if (idx >= static_cast<int>(N)) {
throw std::out_of_range("Calibration index out of range");
}
return X.B[idx].inverse().matrix() * y.unitVector();
}
}
/**
* @brief Calculates the Jacobian matrix using central difference approximation
* @param f Vector function f
* @param x The point at which Jacobian is evaluated
* @return Matrix containing numerical partial derivatives of f at x
* Uses Vector's size() and Zero(), Matrix's Zero() and col() methods
*/
Matrix numericalDifferential(std::function<Vector(const Vector&)> f,
const Vector& x) {
double h = 1e-6;
Vector fx = f(x);
int n = fx.size();
int m = x.size();
Matrix Df = Matrix::Zero(n, m);
for (int j = 0; j < m; j++) {
Vector ej = Vector::Zero(m);
ej(j) = 1.0;
Vector fplus = f(x + h * ej);
Vector fminus = f(x - h * ej);
Df.col(j) = (fplus - fminus) / (2 * h);
}
return Df;
}
/**
* Computes the differential of a state action at the identity of the symmetry
* group
* @param xi State object Xi representing the point at which to evaluate the
* differential
* @return A matrix representing the jacobian of the state action
* Uses numericalDifferential, and Rot3 expmap, logmap
*/
template <size_t N>
Matrix stateActionDiff(const State<N>& xi) {
std::function<Vector(const Vector&)> coordsAction = [&xi](const Vector& U) {
G<N> groupElement = G<N>::exp(U);
State<N> transformed = groupElement * xi;
return xi.localCoordinates(transformed);
};
Vector zeros = Vector::Zero(6 + 3 * N);
Matrix differential = numericalDifferential(coordsAction, zeros);
return differential;
}
//========================================================================
// Equivariant Filter (EqF) Implementation
//========================================================================
/**
* Initializes the EqF with state dimension validation and computes lifted
* innovation mapping
* @param Sigma Initial covariance
* @param n Number of calibration states
* @param m Number of sensors
* Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse()
*/
template <size_t N>
EqF<N>::EqF(const Matrix& Sigma, int m)
: dof(6 + 3 * N),
X_hat(G<N>::identity(N)),
Sigma(Sigma),
xi_0(State<N>::identity()) {
if (Sigma.rows() != dof || Sigma.cols() != dof) {
throw std::invalid_argument(
"Initial covariance dimensions must match the degrees of freedom");
}
// Check positive semi-definite
Eigen::SelfAdjointEigenSolver<Matrix> eigensolver(Sigma);
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
throw std::invalid_argument(
"Covariance matrix must be semi-positive definite");
}
if (N < 0) {
throw std::invalid_argument(
"Number of calibration states must be non-negative");
}
if (m <= 1) {
throw std::invalid_argument(
"Number of direction sensors must be at least 2");
}
// Compute differential of phi
Dphi0 = stateActionDiff(xi_0);
InnovationLift = Dphi0.completeOrthogonalDecomposition().pseudoInverse();
}
/**
* Computes the internal group state to a physical state estimate
* @return Current state estimate
*/
template <size_t N>
State<N> EqF<N>::stateEstimate() const {
return X_hat * xi_0;
}
/**
* Implements the prediction step of the EqF using system dynamics and
* covariance propagation and advances the filter state by symmtery-preserving
* dynamics.Uses a Lie group integrator scheme for discrete time propagation
* @param u Angular velocity measurements
* @param dt time steps
* Updated internal state and covariance
*/
template <size_t N>
void EqF<N>::propagation(const Input& u, double dt) {
State<N> state_est = stateEstimate();
Vector L = lift(state_est, u);
Matrix Phi_DT = stateTransitionMatrix(u, dt);
Matrix Bt = inputMatrixBt();
Matrix tempSigma = blockDiag(u.Sigma, repBlock(1e-9 * I_3x3, N));
Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt;
X_hat = X_hat * G<N>::exp(L * dt);
Sigma = Phi_DT * Sigma * Phi_DT.transpose() + M_DT;
}
/**
* Implements the correction step of the filter using discrete measurements
* Computes the measurement residual, Kalman gain and the updates both the state
* and covariance
*
* @param y Measurements
*/
template <size_t N>
void EqF<N>::update(const Measurement& y) {
if (y.cal_idx > static_cast<int>(N)) {
throw std::invalid_argument("Calibration index out of range");
}
// Get vector representations for checking
Vector3 y_vec = y.y.unitVector();
Vector3 d_vec = y.d.unitVector();
// Skip update if any NaN values are present
if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) ||
std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) {
return; // Skip this measurement
}
Matrix Ct = measurementMatrixC(y.d, y.cal_idx);
Vector3 action_result = outputAction(X_hat.inv(), y.y, y.cal_idx);
Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result;
Matrix Dt = outputMatrixDt(y.cal_idx);
Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();
Matrix K = Sigma * Ct.transpose() * S.inverse();
Vector Delta = InnovationLift * K * delta_vec;
X_hat = G<N>::exp(Delta) * X_hat;
Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma;
}
/**
* Computes linearized continuous time state matrix
* @param u Angular velocity
* @return Linearized state matrix
* Uses Matrix zero and Identity functions
*/
template <size_t N>
Matrix EqF<N>::stateMatrixA(const Input& u) const {
Matrix3 W0 = velocityAction(X_hat.inv(), u).W();
Matrix A1 = Matrix::Zero(6, 6);
A1.block<3, 3>(0, 3) = -I_3x3;
A1.block<3, 3>(3, 3) = W0;
Matrix A2 = repBlock(W0, N);
return blockDiag(A1, A2);
}
/**
* Computes the discrete time state transition matrix
* @param u Angular velocity
* @param dt time step
* @return State transition matrix in discrete time
*/
template <size_t N>
Matrix EqF<N>::stateTransitionMatrix(const Input& u, double dt) const {
Matrix3 W0 = velocityAction(X_hat.inv(), u).W();
Matrix Phi1 = Matrix::Zero(6, 6);
Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0);
Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0;
Phi1.block<3, 3>(0, 0) = I_3x3;
Phi1.block<3, 3>(0, 3) = Phi12;
Phi1.block<3, 3>(3, 3) = Phi22;
Matrix Phi2 = repBlock(Phi22, N);
return blockDiag(Phi1, Phi2);
}
/**
* Computes the input uncertainty propagation matrix
* @return
* Uses the blockdiag matrix
*/
template <size_t N>
Matrix EqF<N>::inputMatrixBt() const {
Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix());
Matrix B2(3 * N, 3 * N);
for (size_t i = 0; i < N; ++i) {
B2.block<3, 3>(3 * i, 3 * i) = X_hat.B[i].matrix();
}
return blockDiag(B1, B2);
}
/**
* Computes the linearized measurement matrix. The structure depends on whether
* the sensor has a calibration state
* @param d reference direction
* @param idx Calibration index
* @return Measurement matrix
* Uses the matrix zero, Rot3 hat and the Unitvector functions
*/
template <size_t N>
Matrix EqF<N>::measurementMatrixC(const Unit3& d, int idx) const {
Matrix Cc = Matrix::Zero(3, 3 * N);
// If the measurement is related to a sensor that has a calibration state
if (idx >= 0) {
// Set the correct 3x3 block in Cc
Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.unitVector());
}
Matrix3 wedge_d = Rot3::Hat(d.unitVector());
// Create the combined matrix
Matrix temp(3, 6 + 3 * N);
temp.block<3, 3>(0, 0) = wedge_d;
temp.block<3, 3>(0, 3) = Matrix3::Zero();
temp.block(0, 6, 3, 3 * N) = Cc;
return wedge_d * temp;
}
/**
* Computes the measurement uncertainty propagation matrix
* @param idx Calibration index
* @return Returns B[idx] for calibrated sensors, A for uncalibrated
*/
template <size_t N>
Matrix EqF<N>::outputMatrixDt(int idx) const {
// If the measurement is related to a sensor that has a calibration state
if (idx >= 0) {
if (idx >= static_cast<int>(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 <size_t N>
struct traits<abc_eqf_lib::EqF<N>>
: internal::LieGroupTraits<abc_eqf_lib::EqF<N>> {};
} // namespace gtsam
#endif // ABC_EQF_H

444
examples/ABC_EQF_Demo.cpp Normal file
View File

@ -0,0 +1,444 @@
/**
* @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 gtsam;
constexpr size_t N = 1; // Number of calibration states
using M = abc_eqf_lib::State<N>;
using Group = abc_eqf_lib::G<N>;
using EqFilter = abc_eqf_lib::EqF<N>;
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<Measurement> y; /// Output measurements
int n_meas; /// Number of measurements
double t; /// Time
double dt; /// Time step
};
//========================================================================
// 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
*
*/
std::vector<Data> loadDataFromCSV(const std::string& filename, int startRow = 0,
int maxRows = -1, int downsample = 1);
/// Process data with EqF and print summary results
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& data_list,
int printInterval = 10);
//========================================================================
// Data Processing Functions Implementation
//========================================================================
/*
* 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<Data> loadDataFromCSV(const std::string& filename, int startRow,
int maxRows, int downsample) {
std::vector<Data> 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<double> 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::array<Rot3, N> 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<Measurement> 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 << " 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;
}
/// Takes in the data and runs an EqF on it and reports the results
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& 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<double> att_errors;
std::vector<double> bias_errors;
std::vector<double> 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<double> 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;
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> 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;
}

5
gtsam/geometry/SimpleCamera.h
View File

@ -31,6 +31,11 @@ namespace gtsam {
/// Convenient aliases for Pinhole camera classes with different calibrations.
/// Also needed as forward declarations in the wrapper.
using PinholePoseCal3_S2 = gtsam::PinholePose<gtsam::Cal3_S2>;
using PinholePoseCal3Bundler = gtsam::PinholePose<gtsam::Cal3Bundler>;
using PinholePoseCal3DS2 = gtsam::PinholePose<gtsam::Cal3DS2>;
using PinholePoseCal3Unified = gtsam::PinholePose<gtsam::Cal3Unified>;
using PinholePoseCal3Fisheye = gtsam::PinholePose<gtsam::Cal3Fisheye>;
using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;

41
gtsam/geometry/geometry.i
View File

@ -694,6 +694,45 @@ class Unit3 {
bool equals(const gtsam::Unit3& expected, double tol) const;
};
#include <gtsam/geometry/OrientedPlane3.h>
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<Eigen::MatrixXd> Hp,
Eigen::Ref<Eigen::MatrixXd> Hr) const;
gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other) const;
gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> H) const;
gtsam::Vector3 localCoordinates(const gtsam::OrientedPlane3& s) const;
gtsam::Vector planeCoefficients() const;
gtsam::Unit3 normal() const;
gtsam::Unit3 normal(Eigen::Ref<Eigen::MatrixXd> H) const;
double distance() const;
double distance(Eigen::Ref<Eigen::MatrixXd> H) const;
};
#include <gtsam/geometry/EssentialMatrix.h>
class EssentialMatrix {
// Standard Constructors
@ -1278,7 +1317,7 @@ class Similarity3 {
double scale() const;
};
template <T>
template <T = {gtsam::PinholePoseCal3_S2}>
class CameraSet {
CameraSet();

7
gtsam/geometry/triangulation.h
View File

@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
}
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetPinholePoseCal3Bundler = CameraSet<PinholePose<Cal3Bundler>>;
using CameraSetPinholePoseCal3_S2 = CameraSet<PinholePose<Cal3_S2>>;
using CameraSetPinholePoseCal3DS2 = CameraSet<PinholePose<Cal3DS2>>;
using CameraSetPinholePoseCal3Fisheye = CameraSet<PinholePose<Cal3Fisheye>>;
using CameraSetPinholePoseCal3Unified = CameraSet<PinholePose<Cal3Unified>>;
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam

391
gtsam/inference/doc/Shortcuts.ipynb Normal file
View File

@ -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": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Title: G Pages: 1 -->\n",
"<svg width=\"168pt\" height=\"188pt\"\n",
" viewBox=\"0.00 0.00 167.97 188.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 184)\">\n",
"<title>G</title>\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-184 163.97,-184 163.97,4 -4,4\"/>\n",
"<!-- 0 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>0</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"79.49\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"79.49\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">r</text>\n",
"</g>\n",
"<!-- 1 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>1</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"35.49\" cy=\"-90\" rx=\"35.49\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"35.49\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">b, c : r</text>\n",
"</g>\n",
"<!-- 0&#45;&gt;1 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>0&#45;&gt;1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M69.5,-145.12C64.29,-136.82 57.78,-126.46 51.85,-117.03\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"54.83,-115.19 46.54,-108.59 48.9,-118.92 54.83,-115.19\"/>\n",
"</g>\n",
"<!-- 3 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>3</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"124.49\" cy=\"-90\" rx=\"35.49\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"124.49\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">d, e : r</text>\n",
"</g>\n",
"<!-- 0&#45;&gt;3 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>0&#45;&gt;3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M89.7,-145.12C95.09,-136.73 101.83,-126.24 107.94,-116.73\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"110.72,-118.88 113.18,-108.58 104.83,-115.1 110.72,-118.88\"/>\n",
"</g>\n",
"<!-- 2 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>2</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"35.49\" cy=\"-18\" rx=\"27.3\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"35.49\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">a : b</text>\n",
"</g>\n",
"<!-- 1&#45;&gt;2 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>1&#45;&gt;2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M35.49,-71.7C35.49,-64.41 35.49,-55.73 35.49,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"38.99,-47.62 35.49,-37.62 31.99,-47.62 38.99,-47.62\"/>\n",
"</g>\n",
"<!-- 4 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>4</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"124.49\" cy=\"-18\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"124.49\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">f : e</text>\n",
"</g>\n",
"<!-- 3&#45;&gt;4 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>3&#45;&gt;4</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M124.49,-71.7C124.49,-64.41 124.49,-55.73 124.49,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"127.99,-47.62 124.49,-37.62 120.99,-47.62 127.99,-47.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x10796f1a0>"
]
},
"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
}

2
gtsam/linear/linear.i
View File

@ -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;

8
gtsam/nonlinear/nonlinear.i
View File

@ -549,7 +549,7 @@ class ISAM2 {
gtsam::Values calculateEstimate() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3f, gtsam::Cal3Bundler,
gtsam::Cal3f, gtsam::Cal3Bundler, gtsam::imuBias::ConstantBias,
gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
@ -626,6 +626,7 @@ template <T = {double,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::NavState,
gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior,
@ -669,17 +670,17 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
};
#include <gtsam/nonlinear/FixedLagSmoother.h>
// 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 +740,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;

5
gtsam/nonlinear/values.i
View File

@ -12,6 +12,7 @@ namespace gtsam {
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
@ -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<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& 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<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& 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<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@ -216,6 +220,7 @@ class Values {
gtsam::EssentialMatrix,
gtsam::FundamentalMatrix,
gtsam::SimpleFundamentalMatrix,
gtsam::OrientedPlane3,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3f>,
gtsam::PinholeCamera<gtsam::Cal3_S2>,

10
gtsam/slam/KarcherMeanFactor.h
View File

@ -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 <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
typename std::enable_if<traits<T>::IsLieGroup, T>::type
FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &elements);
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
/// FindKarcherMean version from initializer list
template <class T>
typename std::enable_if<traits<T>::IsLieGroup, T>::type
FindKarcherMean(std::initializer_list<T> &&elements);
/**
* The KarcherMeanFactor creates a constraint on all SO(n) variables with

2
gtsam/slam/PlanarProjectionFactor.h
View File

@ -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) {}

24
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -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!
*
* <b>Note on Template Parameter `CAMERA`:</b>
* 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<CALIBRATION>` or `SphericalCamera`.
* - Using `CAMERA` types where `dimension != 6`, such as
* `PinholeCamera<CALIBRATION>` (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<CALIBRATION>` 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<CAMERA>::dimension` for downstream calculations.
*
* @ingroup slam
*/
template <class CAMERA>

322
gtsam/slam/doc/BetweenFactor.ipynb Normal file
View File

@ -0,0 +1,322 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# BetweenFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`BetweenFactor<VALUE>` 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<VALUE>` 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/BetweenFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 2.50.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"206pt\" height=\"84pt\"\n",
" viewBox=\"0.00 0.00 206.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
"<polygon fill=\"white\" stroke=\"transparent\" points=\"-4,4 -4,-79.6 202,-79.6 202,4 -4,4\"/>\n",
"<!-- var8646911284551352320 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var8646911284551352320</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x0</text>\n",
"</g>\n",
"<!-- factor0 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>factor0</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"75\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M40.37,-41.61C52.8,-27.68 69.99,-8.41 74.09,-3.81\"/>\n",
"</g>\n",
"<!-- var8646911284551352321 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var8646911284551352321</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x1</text>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor0 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M91.67,-40.17C85.49,-26.32 77.36,-8.1 75.43,-3.76\"/>\n",
"</g>\n",
"<!-- factor1 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>factor1</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"122\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor1 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M106.03,-40.17C111.94,-26.32 119.74,-8.1 121.59,-3.76\"/>\n",
"</g>\n",
"<!-- var8646911284551352322 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var8646911284551352322</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"171\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x2</text>\n",
"</g>\n",
"<!-- var8646911284551352322&#45;&#45;factor1 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8646911284551352322&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M157.61,-41.9C144.92,-27.96 127.17,-8.48 122.94,-3.83\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x2622f1f9fd0>"
]
},
"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
}

212
gtsam/slam/doc/EssentialMatrixConstraint.ipynb Normal file
View File

@ -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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/EssentialMatrixConstraint.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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
}

392
gtsam/slam/doc/EssentialMatrixFactor.ipynb Normal file
View File

@ -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<CALIBRATION>`: Constrains an `EssentialMatrix` and a *shared* `CALIBRATION` variable using a point correspondence given in *pixel* coordinates.\n",
"* `EssentialMatrixFactor5<CALIBRATION>`: 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/EssentialMatrixFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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<CALIBRATION>`"
]
},
{
"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<CALIBRATION>`"
]
},
{
"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
}

257
gtsam/slam/doc/FrobeniusFactor.ipynb Normal file
View File

@ -0,0 +1,257 @@
{
"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<T>`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n",
"* `FrobeniusFactor<T>`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T1 - T2||_F^2$.\n",
"* `FrobeniusBetweenFactor<T>`: 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<Rot3>`) 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/FrobeniusFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"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<Rot3>`"
]
},
{
"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": 4,
"metadata": {
"id": "fprior_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"FrobeniusPriorRot3: keys = { r0 }\n",
" noise model: unit (9) \n",
"\n",
"FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\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}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "ffactor_header_md"
},
"source": [
"## 1. `FrobeniusFactor<Rot3>`"
]
},
{
"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": 7,
"metadata": {
"id": "ffactor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"FrobeniusFactorRot3: keys = { r0 r1 }\n",
" noise model: unit (9) \n",
"\n",
"FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n"
]
}
],
"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(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}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "fbetween_header_md"
},
"source": [
"## 3. `FrobeniusBetweenFactor<Rot3>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "fbetween_desc_md"
},
"source": [
"Acts like `BetweenFactor<Rot3>` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error."
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"id": "fbetween_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<class gtsam::Rot3>(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",
"FrobeniusBetweenFactor error: 1.925929944387236e-34\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}\")"
]
}
],
"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
}

223
gtsam/slam/doc/GeneralSFMFactor.ipynb Normal file
View File

@ -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<CAMERA, LANDMARK>`:\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<Cal3Bundler>`).\n",
"- Error: `camera.project(landmark) - measured`\n",
"\n",
"`GeneralSFMFactor2<CALIBRATION>`:\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<CALIBRATION>(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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/GeneralSFMFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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<CAMERA, LANDMARK>`"
]
},
{
"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<CALIBRATION>`"
]
},
{
"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
}

256
gtsam/slam/doc/InitializePose3.ipynb Normal file
View File

@ -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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/InitializePose3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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
}

228
gtsam/slam/doc/KarcherMeanFactor.ipynb Normal file
View File

@ -0,0 +1,228 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# Karcher Mean Factors"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"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`: 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<T>`: 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/KarcherMeanFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n",
"from gtsam.symbol_shorthand import 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 = FindKarcherMeanRot3(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<Rot3>`"
]
},
{
"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": 6,
"metadata": {
"id": "factor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n",
"sqrt(beta): 10.0\n",
"\n",
"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"
]
}
],
"source": [
"keys = [R(0), R(1), R(2)]\n",
"beta = 100.0 # Strength of the constraint\n",
"\n",
"k_factor = KarcherMeanFactorRot3(keys, 3, beta)\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",
"\n",
"# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n",
"sqrt_beta = np.sqrt(beta)\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}\")"
]
}
],
"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": 0
}

222
gtsam/slam/doc/OrientedPlane3Factor.ipynb Normal file
View File

@ -0,0 +1,222 @@
{
"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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/OrientedPlane3Factor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 2,
"metadata": {
"id": "imports_code"
},
"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.symbol_shorthand import X, P"
]
},
{
"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": 3,
"metadata": {
"id": "factor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"OrientedPlane3Factor: \n",
"OrientedPlane3Factor Factor (x0, p0)\n",
"Measured Plane : 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"
]
}
],
"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.planeCoefficients()\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 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": 4,
"metadata": {
"id": "prior_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"OrientedPlane3DirectionPrior: \n",
"OrientedPlane3DirectionPrior: Prior Factor on p0\n",
"Measured Plane : 0 0 1 0\n",
"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(2, 0.02)\n",
"\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",
"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": "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": 0
}

284
gtsam/slam/doc/PlanarProjectionFactor.ipynb Normal file
View File

@ -0,0 +1,284 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PlanarProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"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`: 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",
"\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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PlanarProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 2,
"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.symbol_shorthand import X, L, C, O"
]
},
{
"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.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",
"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",
"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(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(X(0), 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 = { 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": [
"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(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(L(0), 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": 5,
"metadata": {
"id": "factor3_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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: 92.30212176019934\n"
]
}
],
"source": [
"offset_key = O(0)\n",
"calib_key = C(0)\n",
"\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(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",
"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": "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": 0
}

202
gtsam/slam/doc/PoseRotationPrior.ipynb Normal file
View File

@ -0,0 +1,202 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PoseRotationPrior"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`PoseRotationPrior<POSE>` 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseRotationPrior.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPrior3D\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": 2,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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"
]
}
],
"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. 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 = PoseRotationPrior3D(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": 3,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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"
]
}
],
"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
}

204
gtsam/slam/doc/PoseTranslationPrior.ipynb Normal file
View File

@ -0,0 +1,204 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PoseTranslationPrior"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`PoseTranslationPrior<POSE>` 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",
"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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseTranslationPrior.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPrior3D\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": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"PoseTranslationPrior: PoseTranslationPrior keys = { x0 }\n",
"isotropic dim=3 sigma=0.5\n",
"Measured Translation[\n",
"\t10;\n",
"\t20;\n",
"\t5\n",
"]\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 = 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 = PoseTranslationPrior3D(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": 5,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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"
]
}
],
"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
}

238
gtsam/slam/doc/ProjectionFactor.ipynb Normal file
View File

@ -0,0 +1,238 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# GenericProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`GenericProjectionFactor<POSE, LANDMARK, CALIBRATION>` 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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
}

221
gtsam/slam/doc/ReferenceFrameFactor.ipynb Normal file
View File

@ -0,0 +1,221 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# ReferenceFrameFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`ReferenceFrameFactor<POINT, TRANSFORM>` 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ReferenceFrameFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"['ReferenceFrameFactorPoint3Pose3']\n"
]
}
],
"source": [
"import gtsam\n",
"print([k for k in gtsam.__dict__.keys() if \"ReferenceFrame\" in k])"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "imports_code"
},
"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.symbol_shorthand import L, T, O"
]
},
{
"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": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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 = 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",
"\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": 5,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Expected local landmark: [ 2. -4. 1.]\n",
"\n",
"Error at ground truth: 4500.0 (Should be zero)\n",
"Error with noisy local landmark: 4621.125\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
}

225
gtsam/slam/doc/RotateFactor.ipynb Normal file
View File

@ -0,0 +1,225 @@
{
"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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/RotateFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"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": 2,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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",
"\n",
"Error at ground truth R: 700.0\n",
"Error at noisy R: 699.2869223608442\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": 8,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"RotateDirectionsFactor: keys = { r0 }\n",
"isotropic dim=2 sigma=0.01\n",
"RotateDirectionsFactor:\n",
"p:1\n",
"0\n",
"0\n",
"z:0\n",
"1\n",
"0\n",
"\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"
]
}
],
"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 = \\n{gt_R_dir.rotate(z_body)}\")\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
}

246
gtsam/slam/doc/SmartProjectionFactor.ipynb Normal file
View File

@ -0,0 +1,246 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionFactor<CAMERA>` 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.\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."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 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",
" SmartProjectionFactorPinholeCameraCal3_S2,\n",
" PinholeCameraCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import C"
]
},
{
"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": 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 = 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"
]
}
],
"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": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulated point result:\n",
"Status.BEHIND_CAMERA\n",
"\n",
"Triangulation failed, error calculation depends on degeneracyMode.\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.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",
"else:\n",
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
]
}
],
"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": 0
}

229
gtsam/slam/doc/SmartProjectionParams.ipynb Normal file
View File

@ -0,0 +1,229 @@
{
"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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartFactorParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 3,
"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": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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"
]
}
],
"source": [
"# Default parameters\n",
"default_params = SmartProjectionParams()\n",
"print(\"Default Parameters:\")\n",
"default_params.print()\n",
"\n",
"# Custom parameters\n",
"custom_params = SmartProjectionParams(\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.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()"
]
},
{
"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": 5,
"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 = 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",
" keys = { }\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 = 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": "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": 0
}

321
gtsam/slam/doc/SmartProjectionPoseFactor.ipynb Normal file
View File

@ -0,0 +1,321 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionPoseFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionPoseFactor<CALIBRATION>` 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`.\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."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"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"
]
},
{
"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": 3,
"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 = 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 = { x0 x1 x2 }\n"
]
}
],
"source": [
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams() # Use default params\n",
"K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
"\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": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"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)): 120243.1626\n"
]
}
],
"source": [
"# Create Values containing Pose3 objects\n",
"values = Values()\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",
"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: {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",
"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": 5,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (showing HessianFactor structure):\n",
"\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"
]
}
],
"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.HessianFactor(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": "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": 0
}

586
gtsam/slam/doc/SmartProjectionRigFactor.ipynb Normal file
View File

@ -0,0 +1,586 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionRigFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionRigFactor<CAMERA>` 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., `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 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<CAMERA>::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<CALIBRATION>` or `SphericalCamera`.\n",
"- Using `CAMERA` types where `dimension != 6`, such as `PinholeCamera<CALIBRATION>` (dim = 6 + CalDim), **will cause compilation errors**.\n",
"- **Recommendation:** For standard pinhole cameras in a fixed rig, **use `PinholePose<CALIBRATION>`** 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"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionRigFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"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": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"from gtsam import (\n",
" Values,\n",
" Point2,\n",
" Point3,\n",
" Pose3,\n",
" Rot3,\n",
" SmartProjectionParams,\n",
" LinearizationMode,\n",
" DegeneracyMode,\n",
" # Use PinholePose variant for wrapping\n",
" SmartProjectionRigFactorPinholePoseCal3_S2,\n",
" PinholePoseCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Constructor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "constructor_desc_md"
},
"source": [
"You create a `SmartProjectionRigFactor` by providing:\n",
"1. A noise model for the 2D pixel measurements (typically `noiseModel.Isotropic`).\n",
"2. A `CameraSet<CAMERA>` 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": "constructor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"SmartFactorRig: SmartProjectionRigFactor: \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",
" keys = { }\n"
]
}
],
"source": [
"# Define the Camera Rig (using PinholePose)\n",
"K = Cal3_S2(500, 500, 0, 320, 240)\n",
"body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n",
"cam0 = PinholePoseCal3_S2(body_P_cam0, K)\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",
"# Noise model and parameters\n",
"noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams(\n",
" linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY\n",
")\n",
"\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": "add_header_md"
},
"source": [
"## `add(measurement, poseKey, cameraId)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "add_desc_md"
},
"source": [
"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": "add_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart factor involves 2 measurements from 2 unique poses.\n"
]
}
],
"source": [
"# --- 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() # 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",
"# Need to check triangulation first, as error calculation depends on it\n",
"point_result = smart_factor.point(values)\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",
" triangulated_point = point_result.get()\n",
" print(f\"Triangulated Point: {triangulated_point}\")\n",
"else:\n",
" 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<CAMERA>` 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()}\")"
]
}
],
"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": 0
}

262
gtsam/slam/doc/StereoFactor.ipynb Normal file
View File

@ -0,0 +1,262 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# GenericStereoFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`GenericStereoFactor<POSE, LANDMARK>` 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/StereoFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"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",
"# 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",
"\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": 2,
"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": 3,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"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"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Example values\n",
"pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\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",
"error_no_offset = factor_no_offset.error(values)\n",
"print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be near zero)\")\n",
"\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 near zero)\")\n",
"\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\"\\nError (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
}

283
gtsam/slam/doc/TriangulationFactor.ipynb Normal file
View File

@ -0,0 +1,283 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# TriangulationFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`TriangulationFactor<CAMERA>` 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": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/TriangulationFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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<double,-1,1,0,-1,1>)\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
}

252
gtsam/slam/doc/dataset.ipynb Normal file
View File

@ -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<T>`/`parseMeasurements<T>`/`parseFactors<T>`: Lower-level parsing functions (less commonly used directly)."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/dataset.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"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 = <KernelFunctionType.KernelFunctionTypeNONE: 0>) -> 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
}

265
gtsam/slam/doc/lago.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

203
gtsam/slam/slam.i
View File

@ -152,6 +152,30 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const;
};
// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/slam/SmartFactorBase.h>
// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
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<gtsam::Point2>& measured() const;
std::vector<CAMERA> 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 <gtsam/slam/SmartProjectionFactor.h>
/// Linearization mode: what factor to linearize to
@ -162,16 +186,86 @@ 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 <
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<CAMERA> {
SmartProjectionFactor();
SmartProjectionFactor(
const gtsam::noiseModel::Base* sharedNoiseModel,
const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
bool decideIfTriangulate(const gtsam::CameraSet<CAMERA>& cameras) const;
gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet<CAMERA>& cameras) const;
bool triangulateForLinearize(const gtsam::CameraSet<CAMERA>& cameras) const;
gtsam::HessianFactor createHessianFactor(
const gtsam::CameraSet<CAMERA>& cameras, const double lambda = 0.0,
bool diagonalDamping = false) const;
gtsam::JacobianFactor createJacobianQFactor(
const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
gtsam::JacobianFactor createJacobianQFactor(
const gtsam::Values& values, double lambda) const;
gtsam::JacobianFactor createJacobianSVDFactor(
const gtsam::CameraSet<CAMERA>& 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<CAMERA>& 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<CAMERA>& 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<CAMERA>& 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 <gtsam/slam/SmartProjectionPoseFactor.h>
template <CALIBRATION>
// We are not deriving from SmartProjectionFactor yet - too complicated in wrapper
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K);
@ -195,8 +289,30 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
gtsam::TriangulationResult point(const gtsam::Values& values) const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
SmartProjectionPose3Factor;
#include <gtsam/slam/SmartProjectionRigFactor.h>
// Only for PinholePose cameras -> PinholeCamera is not supported
template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2,
gtsam::PinholePoseCal3Bundler,
gtsam::PinholePoseCal3Fisheye,
gtsam::PinholePoseCal3Unified}>
virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
SmartProjectionRigFactor();
SmartProjectionRigFactor(
const gtsam::noiseModel::Base* sharedNoiseModel,
const gtsam::CameraSet<CAMERA>* cameraRig,
const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
void add(const gtsam::Point2& measured, const gtsam::Key& poseKey,
const size_t& cameraId = 0);
void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys,
const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
const gtsam::KeyVector& nonUniqueKeys() const;
const gtsam::CameraSet<CAMERA>& cameraRig() const;
const gtsam::FastVector<size_t>& cameraIds() const;
};
#include <gtsam/slam/StereoFactor.h>
template <POSE, LANDMARK>
@ -227,11 +343,72 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>
GenericStereoFactor3D;
#include <gtsam/slam/ReferenceFrameFactor.h>
template<LANDMARK = {gtsam::Point3}, POSE = {gtsam::Pose3}>
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 <gtsam/slam/RotateFactor.h>
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 <gtsam/slam/OrientedPlane3Factor.h>
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 <gtsam/slam/PoseTranslationPrior.h>
template <POSE>
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 +421,10 @@ typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
#include <gtsam/slam/PoseRotationPrior.h>
template <POSE>
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;
};
@ -396,14 +575,25 @@ template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
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 = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
T FindKarcherMean(const std::vector<T>& elements);
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
size_t d);
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
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 <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
@ -464,6 +654,7 @@ typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Unified>>
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

54
gtsam/slam/slam.md Normal file
View File

@ -0,0 +1,54 @@
# 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.
- [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.
- [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
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.
- [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.
- [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).

2
gtsam/symbolic/symbolic.md
View File

@ -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.

4
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -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

3
myst.yml
View File

@ -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

16431
python/gtsam/examples/EKF_SLAM.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

812
python/gtsam/examples/PlanarSLAMExample.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

97
python/gtsam/examples/PlanarSLAMExample.py
View File

@ -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()

712
python/gtsam/examples/Pose2SLAMExample.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

102
python/gtsam/examples/Pose2SLAMExample.py
View File

@ -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()

519
python/gtsam/examples/gtsam_plotly.py Normal file
View File

@ -0,0 +1,519 @@
# gtsam_plotly.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 # Optional progress bar
import gtsam
# --- Dataclass for History ---
@dataclass
class SlamFrameData:
"""Holds all data needed for a single frame of the SLAM animation."""
step_index: int
results: gtsam.Values # Estimates for variables active at this step
marginals: Optional[gtsam.Marginals] # Marginals for variables active at this step
graph_dot_string: Optional[str] = None # Graphviz DOT string for visualization
# --- 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 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)
eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues
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
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 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 * (
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_p[0]},{y_p[0]} "
+ " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_p[1:], y_p[1:]))
+ " Z"
)
return path
# --- Plotly Element Generators ---
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
return go.Scatter(
x=landmarks_gt[0, :],
y=landmarks_gt[1, :],
mode="markers",
marker=dict(color="black", size=8, symbol="star"),
name="Landmarks GT",
)
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
return go.Scatter(
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",
)
def create_est_path_trace(
est_path_x: List[float], est_path_y: List[float]
) -> go.Scatter:
"""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="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 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",
)
def _create_ellipse_shape_dict(
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)
return dict(
type="path",
path=path,
xref="x",
yref="y",
fillcolor=fillcolor,
line_color=line_color,
opacity=0.7, # Make ellipses slightly transparent
)
def create_pose_ellipse_shape(
pose_mean_xy: np.ndarray, pose_cov: np.ndarray, scale: float
) -> Dict[str, Any]:
"""Creates shape dict for a pose covariance ellipse."""
return _create_ellipse_shape_dict(
cx=pose_mean_xy[0],
cy=pose_mean_xy[1],
cov=pose_cov,
scale=scale,
fillcolor="rgba(255,0,255,0.2)", # Magenta fill
line_color="rgba(255,0,255,0.5)", # Magenta line
)
def create_landmark_ellipse_shape(
lm_mean_xy: np.ndarray, lm_cov: np.ndarray, scale: float
) -> Dict[str, Any]:
"""Creates shape dict for a landmark covariance ellipse."""
return _create_ellipse_shape_dict(
cx=lm_mean_xy[0],
cy=lm_mean_xy[1],
cov=lm_cov,
scale=scale,
fillcolor="rgba(0,0,255,0.1)", # Blue fill
line_color="rgba(0,0,255,0.3)", # Blue line
)
def dot_string_to_base64_svg(
dot_string: Optional[str], engine: str = "neato"
) -> Optional[str]:
"""Renders a DOT string to a base64 encoded SVG using graphviz."""
if not dot_string:
return None
try:
source = graphviz.Source(dot_string, engine=engine)
svg_bytes = source.pipe(format="svg")
encoded_string = base64.b64encode(svg_bytes).decode("utf-8")
return f"data:image/svg+xml;base64,{encoded_string}"
except graphviz.backend.execute.CalledProcessError as e:
print(f"Graphviz rendering error ({engine}): {e}")
return None
except Exception as e:
print(f"Unexpected error during Graphviz SVG generation: {e}")
return None
# --- Frame Content Generation ---
def generate_frame_content(
frame_data: SlamFrameData,
X: Callable[[int], int],
L: Callable[[int], int],
max_landmark_index: int,
ellipse_scale: float = 2.0,
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."""
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] = []
frame_shapes: List[Dict[str, Any]] = []
layout_image: Optional[Dict[str, Any]] = None
# 1. Estimated Path (Full History or Partial)
est_path_x = []
est_path_y = []
current_pose_est = None
# 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) # 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 (Only for items in step_results AND step_marginals)
if step_marginals:
# Current Pose Ellipse
pose_key = X(k)
# 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: # Covariance might not be available
if verbose:
print(
f"Warn: LM {gtsam.Symbol(lm_key).index()} cov not in marginals @ step {k}"
)
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
img_src = dot_string_to_base64_svg(
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 dynamic elements for this frame
return frame_dynamic_traces, frame_shapes, layout_image
# --- Figure Configuration ---
def configure_figure_layout(
fig: go.Figure,
num_steps: int,
world_size: float,
initial_shapes: List[Dict[str, Any]],
initial_image: Optional[Dict[str, Any]],
) -> None:
"""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,
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
],
)
]
updatemenus = [
dict(
type="buttons",
showactive=False,
direction="left",
pad={"r": 10, "t": 87},
x=plot_domain[0],
xanchor="left",
y=0,
yanchor="top",
buttons=[
dict(
label="Play",
method="animate",
args=[
None,
dict(
mode="immediate",
frame=dict(duration=100, redraw=True),
transition=dict(duration=0),
fromcurrent=True,
),
],
),
dict(
label="Pause",
method="animate",
args=[
[None],
dict(
mode="immediate",
frame=dict(duration=0, redraw=False),
transition=dict(duration=0),
),
],
),
],
)
]
fig.update_layout(
title="Factor Graph SLAM Animation (Graph Left, Results Right)",
xaxis=dict(
range=[-world_size / 2 - 2, world_size / 2 + 2],
domain=plot_domain,
constrain="domain",
),
yaxis=dict(
range=[-world_size / 2 - 2, world_size / 2 + 2],
scaleanchor="x",
scaleratio=1,
domain=[0, 1],
),
width=1000,
height=600,
hovermode="closest",
updatemenus=updatemenus,
sliders=sliders,
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 ---
def create_slam_animation(
history: List[SlamFrameData],
X: Callable[[int], int],
L: Callable[[int], int],
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",
verbose_cov_errors: bool = False,
) -> go.Figure:
"""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. Create static GT traces ONCE
gt_traces = []
gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array)
if gt_lm_trace:
gt_traces.append(gt_lm_trace)
gt_path_trace = create_gt_path_trace(poses_gt)
if gt_path_trace:
gt_traces.append(gt_path_trace)
# 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 (GT + dynamic frame 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 = []
steps_iterable = range(num_steps + 1)
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)
# Generate dynamic content specific to this frame
frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content(
frame_data,
X,
L,
max_landmark_index,
ellipse_scale,
graphviz_engine,
verbose_cov_errors,
)
# 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, # GT must be in each frame's data
name=str(k),
layout=go.Layout(
shapes=frame_shapes, # Replaces shapes list for this frame
images=(
[layout_image] if layout_image else []
), # Replaces image list
),
)
)
# 5. Assign frames to the figure
fig.update(frames=frames)
# 6. Configure overall layout (sliders, buttons, axes, etc.)
configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image)
print("Plotly animation generated.")
return fig

137
python/gtsam/examples/simulation.py Normal file
View File

@ -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 / 2
):
# 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,
)

6
python/gtsam/tests/test_KarcherMeanFactor.py
View File

@ -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)))

6
python/gtsam/tests/test_backwards_compatibility.py
View File

@ -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)

28
python/gtsam/tests/test_lago.py
View File

@ -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()