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

Merge pull request #2126 from borglab/feature/smartFactorDocs 

Smart factors docs
release/4.3a0
Frank Dellaert 2025-05-08 13:09:11 -04:00 committed by GitHub
parent e41c2eef12 2ba3bc360d
commit 0c9f87d75c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 438 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

431
gtsam/linear/RegularHessianFactor.h
View File

@ -9,209 +9,332 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file RegularHessianFactor.h * @file RegularHessianFactor.h
* @brief HessianFactor class with constant sized blocks * @brief Specialized HessianFactor class for regular problems (fixed-size blocks).
* @author Sungtae An * @details This factor is specifically designed for quadratic cost functions
* @date March 2014 * where all variables involved have the same, fixed dimension `D`.
*/ * It stores the Hessian and gradient terms and provides optimized
* methods for operations like Hessian-vector products, crucial for
* iterative solvers like Conjugate Gradient. It ensures that all
* involved blocks have the expected dimension `D`.
* @author Sungtae An
* @author Frank Dellaert
* @date March 2014
*/
#pragma once #pragma once
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h> #include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/linear/VectorValues.h> // For VectorValues and Scatter
#include <vector> #include <vector>
#include <stdexcept> // For std::invalid_argument
namespace gtsam { namespace gtsam {
template<size_t D> /**
class RegularHessianFactor: public HessianFactor { * @brief A HessianFactor where all variables have the same dimension D.
* @details Represents a quadratic factor \f$ E(x) = 0.5 x^T G x - g^T x + 0.5 f \f$,
public: * corresponding to a Gaussian probability density \f$ P(x) \propto \exp(-E(x)) \f$.
* Here, G is the Hessian (or information matrix), g is the gradient vector,
typedef Eigen::Matrix<double, D, 1> VectorD; * and f is a constant term (negative log-likelihood at x=0).
typedef Eigen::Matrix<double, D, D> MatrixD; *
* This class is templated on the dimension `D` and enforces that all variables
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the * associated with this factor have this dimension during construction.
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces * It inherits from HessianFactor but provides specialized, efficient implementations
* of the linear vector term, and f the constant term. * for operations like `multiplyHessianAdd` using raw memory access, assuming
* a regular block structure. This can significantly improve performance in
* iterative optimization algorithms when dealing with many variables of the
* same type and dimension (e.g., Pose3 variables in SLAM).
*
* It can be constructed directly from Hessian blocks, from a
* RegularJacobianFactor, or by linearizing a NonlinearFactorGraph.
*
* @tparam D The dimension of each variable block involved in the factor.
*/ */
RegularHessianFactor(const KeyVector& js, template<size_t D>
class RegularHessianFactor : public HessianFactor {
public:
typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, D, D> MatrixD;
/**
* @brief Construct an n-way factor from supplied components.
* @details Represents the energy function \f$ E(x) = 0.5 \sum_{i,j} x_i^T G_{ij} x_j - \sum_i g_i^T x_i + 0.5 f \f$.
* Note that the keys in `js` must correspond order-wise to the blocks in `Gs` and `gs`.
* @param js Vector of keys involved in the factor.
* @param Gs Vector of upper-triangular Hessian blocks (row-major order, e.g., G11, G12, G13, G22, G23, G33 for a ternary factor).
* @param gs Vector of gradient vector blocks (-J^T b).
* @param f Constant term \f$ 0.5*f = 0.5*\|b\|^2 \f$.
*/
RegularHessianFactor(const KeyVector& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) : const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
HessianFactor(js, Gs, gs, f) { HessianFactor(js, Gs, gs, f) {
checkInvariants(); checkInvariants();
} }
/** Construct a binary factor. Gxx are the upper-triangle blocks of the /** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector * quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term. * term, and f the constant term.
*/ */
RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12, RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
} }
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the /** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector * quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term. * term, and f the constant term.
*/ */
RegularHessianFactor(Key j1, Key j2, Key j3, RegularHessianFactor(Key j1, Key j2, Key j3,
const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1, const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
const MatrixD& G22, const MatrixD& G23, const VectorD& g2, const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
const MatrixD& G33, const VectorD& g3, double f) : const MatrixD& G33, const VectorD& g3, double f) :
HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
} }
/** Constructor with an arbitrary number of keys and with the augmented information matrix /**
* specified as a block matrix. */ * @brief Constructor with an arbitrary number of keys and the augmented information matrix
template<typename KEYS> * specified as a block matrix.
RegularHessianFactor(const KEYS& keys, * @details The augmented information matrix contains the Hessian blocks G and the
* gradient blocks g, arranged as \f$ [ G, -g ; -g^T, f ] \f$.
* @param keys Container of keys specifying the variables involved and their order.
* @param augmentedInformation The augmented information matrix [H -g; -g' f], laid out as a SymmetricBlockMatrix.
*/
template<typename KEYS>
RegularHessianFactor(const KEYS& keys,
const SymmetricBlockMatrix& augmentedInformation) : const SymmetricBlockMatrix& augmentedInformation) :
HessianFactor(keys, augmentedInformation) { HessianFactor(keys, augmentedInformation) {
checkInvariants(); checkInvariants();
} }
/// Construct from RegularJacobianFactor /**
RegularHessianFactor(const RegularJacobianFactor<D>& jf) * @brief Construct a RegularHessianFactor from a RegularJacobianFactor.
: HessianFactor(jf) {} * @details Computes \f$ G = J^T J \f$, \f$ g = J^T b \f$, and \f$ f = b^T b \f$.
* Requires that the JacobianFactor also has regular block sizes matching `D`.
* @param jf The RegularJacobianFactor to convert.
*/
RegularHessianFactor(const RegularJacobianFactor<D>& jf)
: HessianFactor(jf) {
}
/// Construct from a GaussianFactorGraph /**
RegularHessianFactor(const GaussianFactorGraph& factors, * @brief Construct from a GaussianFactorGraph combined using a Scatter.
const Scatter& scatter) * @details This is typically used internally by optimizers. It sums the Hessian
* contributions from multiple factors in the graph.
* @param factors The graph containing factors to combine.
* @param scatter A Scatter structure indicating the layout of variables.
*/
RegularHessianFactor(const GaussianFactorGraph& factors,
const Scatter& scatter)
: HessianFactor(factors, scatter) { : HessianFactor(factors, scatter) {
checkInvariants(); checkInvariants();
} }
/// Construct from a GaussianFactorGraph /**
RegularHessianFactor(const GaussianFactorGraph& factors) * @brief Construct from a GaussianFactorGraph.
* @details This is typically used internally by optimizers. It sums the Hessian
* contributions from multiple factors in the graph. Assumes keys are ordered 0..n-1.
* @param factors The graph containing factors to combine.
*/
RegularHessianFactor(const GaussianFactorGraph& factors)
: HessianFactor(factors) { : HessianFactor(factors) {
checkInvariants(); checkInvariants();
} }
private: private:
/// Check invariants after construction /**
void checkInvariants() { * @brief Check if the factor has regular block structure.
if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D) * @details Verifies that the dimensions of the stored augmented information matrix
throw std::invalid_argument( * `info_` correspond to the expected size based on the number of keys and the
"RegularHessianFactor constructor was given non-regular factors"); * fixed block dimension `D`. Throws `std::invalid_argument` if the structure
} * is not regular. This is called internally by constructors.
*/
void checkInvariants() const {
if (info_.cols() != 0 && // Allow zero-key factors (e.g. priors on anchor nodes)
info_.cols() != 1 + (info_.nBlocks() - 1) * static_cast<DenseIndex>(D))
throw std::invalid_argument(
"RegularHessianFactor constructor was given non-regular factors or "
"incorrect template dimension D");
}
// Use Eigen magic to access raw memory // Use Eigen magic to access raw memory for efficiency in Hessian products
typedef Eigen::Map<VectorD> DMap; typedef Eigen::Map<VectorD> DMap;
typedef Eigen::Map<const VectorD> ConstDMap; typedef Eigen::Map<const VectorD> ConstDMap;
// Scratch space for multiplyHessianAdd // Scratch space for multiplyHessianAdd to avoid re-allocation
// According to link below this is thread-safe. // According to link below this is thread-safe.
// http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
mutable std::vector<VectorD> y_; mutable std::vector<VectorD, Eigen::aligned_allocator<VectorD>> y_;
public: public:
/** y += alpha * A'*A*x */ /**
void multiplyHessianAdd(double alpha, const VectorValues& x, * @brief Multiply the Hessian part of the factor times a VectorValues `x` and add the result to `y`.
* @details Computes `y += alpha * H * x`.
* @param alpha Scalar multiplier.
* @param x VectorValues containing the vector to multiply with.
* @param[in,out] y VectorValues to store the result (accumulates).
*/
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const override { VectorValues& y) const override {
HessianFactor::multiplyHessianAdd(alpha, x, y); // Note: This implementation just calls the base class. The raw memory versions
} // below are specifically optimized for the regular structure of this class.
// Consider using those directly or ensuring the base class implementation
// is efficient enough for your use case if calling this version.
HessianFactor::multiplyHessianAdd(alpha, x, y);
}
/** y += alpha * A'*A*x */ /**
void multiplyHessianAdd(double alpha, const double* x, * @brief Multiply the Hessian part of the factor times a raw vector `x` and add the result to `y`. (Raw memory version)
* @details Computes `y += alpha * H * x`, optimized for regular block structure.
* Assumes `x` and `yvalues` are pre-allocated, contiguous memory blocks
* where variable `j` corresponds to memory chunk `x + keys_[j] * D`
* and `yvalues + keys_[j] * D`.
* @param alpha Scalar multiplier.
* @param x Raw pointer to the input vector data.
* @param[in,out] yvalues Raw pointer to the output vector data (accumulates).
*/
void multiplyHessianAdd(double alpha, const double* x,
double* yvalues) const { double* yvalues) const {
// Create a vector of temporary y_ values, corresponding to rows i // Ensure scratch space is properly sized
y_.resize(size()); const size_t n = size();
for(VectorD & yi: y_) if (y_.size() != n) {
yi.setZero(); y_.resize(n);
}
for (VectorD& yi : y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive // Loop over columns (j) of the Hessian H=info_
// So we will loop over columns to access x only once per column // Accessing x only once per column j
// And fill the above temporary y_ values, to be added into yvalues after // Fill temporary y_ vector corresponding to rows i
VectorD xj(D); for (DenseIndex j = 0; j < static_cast<DenseIndex>(n); ++j) {
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { Key key_j = keys_[j];
Key key = keys_[j]; ConstDMap xj(x + key_j * D); // Map to the j-th block of x
const double* xj = x + key * D;
DenseIndex i = 0; DenseIndex i = 0;
for (; i < j; ++i) // Off-diagonal blocks G_ij * x_j for i < j
y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj); for (; i < j; ++i) {
// blocks on the diagonal are only half y_[i] += info_.aboveDiagonalBlock(i, j) * xj;
y_[i] += info_.diagonalBlock(j) * ConstDMap(xj); }
// for below diagonal, we take transpose block from upper triangular part // Diagonal block G_jj * x_j
for (i = j + 1; i < (DenseIndex) size(); ++i) y_[i] += info_.diagonalBlock(j) * xj;
y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj); // Off-diagonal blocks G_ij * x_j for i > j (using transpose G_ji^T * x_j)
for (i = j + 1; i < static_cast<DenseIndex>(n); ++i) {
y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
}
}
// Add accumulated results from y_ to the output yvalues
for (DenseIndex i = 0; i < static_cast<DenseIndex>(n); ++i) {
Key key_i = keys_[i];
DMap map_yi(yvalues + key_i * D); // Map to the i-th block of yvalues
map_yi += alpha * y_[i];
}
} }
// copy to yvalues /**
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { * @brief Multiply the Hessian part of the factor times a raw vector `x` and add the result to `y`.
Key key = keys_[i]; * @details Computes `y += alpha * H * x`, optimized for regular block structure,
DMap(yvalues + key * D) += alpha * y_[i]; * but handles potentially non-contiguous or scattered memory layouts
} * for `x` and `yvalues` as defined by the `offsets` vector.
} * `offsets[k]` should give the starting index of variable `k` in the
* raw memory arrays. `offsets[k+1] - offsets[k]` should equal `D`.
* @param alpha Scalar multiplier.
* @param x Raw pointer to the input vector data.
* @param[in,out] yvalues Raw pointer to the output vector data (accumulates).
* @param offsets Vector mapping variable keys to their starting index in `x` and `yvalues`.
*/
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
const std::vector<size_t>& offsets) const {
// Ensure scratch space is properly sized
const size_t n = size();
if (y_.size() != n) {
y_.resize(n);
}
for (VectorD& yi : y_)
yi.setZero();
/// Raw memory version, with offsets TODO document reasoning // Loop over columns (j) of the Hessian H=info_
void multiplyHessianAdd(double alpha, const double* x, double* yvalues, for (DenseIndex j = 0; j < static_cast<DenseIndex>(n); ++j) {
std::vector<size_t> offsets) const { Key key_j = keys_[j];
size_t offset_j = offsets[key_j];
// Ensure block size matches D (redundant if checkInvariants worked, but safe)
size_t dim_j = offsets[key_j + 1] - offset_j;
if (dim_j != D) throw std::runtime_error("RegularHessianFactor::multiplyHessianAdd: Mismatched dimension in offset map.");
ConstDMap xj(x + offset_j); // Map to the j-th block of x using offset
// Create a vector of temporary y_ values, corresponding to rows i DenseIndex i = 0;
y_.resize(size()); // Off-diagonal blocks G_ij * x_j for i < j
for(VectorD & yi: y_) for (; i < j; ++i) {
yi.setZero(); y_[i] += info_.aboveDiagonalBlock(i, j) * xj;
}
// Diagonal block G_jj * x_j
y_[i] += info_.diagonalBlock(j) * xj;
// Off-diagonal blocks G_ij * x_j for i > j (using transpose G_ji^T * x_j)
for (i = j + 1; i < static_cast<DenseIndex>(n); ++i) {
y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
}
}
// Accessing the VectorValues one by one is expensive // Add accumulated results from y_ to the output yvalues using offsets
// So we will loop over columns to access x only once per column for (DenseIndex i = 0; i < static_cast<DenseIndex>(n); ++i) {
// And fill the above temporary y_ values, to be added into yvalues after Key key_i = keys_[i];
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { size_t offset_i = offsets[key_i];
DenseIndex i = 0; size_t dim_i = offsets[key_i + 1] - offset_i;
for (; i < j; ++i) if (dim_i != D) throw std::runtime_error("RegularHessianFactor::multiplyHessianAdd: Mismatched dimension in offset map.");
y_[i] += info_.aboveDiagonalBlock(i, j) DMap map_yi(yvalues + offset_i); // Map to the i-th block of yvalues using offset
* ConstDMap(x + offsets[keys_[j]], map_yi += alpha * y_[i];
offsets[keys_[j] + 1] - offsets[keys_[j]]); }
// blocks on the diagonal are only half
y_[i] += info_.diagonalBlock(j)
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y_[i] += info_.aboveDiagonalBlock(j, i).transpose()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
} }
// copy to yvalues /**
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) * @brief Return the diagonal of the Hessian for this factor (Raw memory version).
DMap(yvalues + offsets[keys_[i]], * @details Adds the diagonal elements of the Hessian matrix \f$ H = G \f$ associated
offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i]; * with this factor to the pre-allocated raw memory block `d`.
} * Assumes `d` has the standard contiguous layout `d + keys_[i] * D`.
* @param[in,out] d Raw pointer to the memory block where Hessian diagonals should be added.
/** Return the diagonal of the Hessian for this factor (raw memory version) */ */
void hessianDiagonal(double* d) const override { void hessianDiagonal(double* d) const override {
// Loop over all variables (diagonal blocks) in the factor
// Loop over all variables in the factor const size_t n = size();
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { for (DenseIndex pos = 0; pos < static_cast<DenseIndex>(n); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal // Get the diagonal block G_jj and add its diagonal elements to d
DMap(d + D * j) += info_.diagonal(pos); DMap(d + D * j) += info_.diagonal(pos);
}
} }
}
/// Add gradient at zero to d TODO: is it really the goal to add ?? /**
void gradientAtZero(double* d) const override { * @brief Add the gradient vector \f$ -g \f$ (gradient at zero) to a raw memory block `d`.
* @details Adds the gradient vector \f$ -g \f$ (where the factor represents
// Loop over all variables in the factor * \f$ 0.5 x^T G x - g^T x + 0.5 f \f$) to the pre-allocated raw
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { * memory block `d`. Assumes `d` has the standard contiguous layout
Key j = keys_[pos]; * `d + keys_[i] * D`.
// Get the diagonal block, and insert its diagonal * @param[in,out] d Raw pointer to the memory block where the gradient should be added.
DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());; */
void gradientAtZero(double* d) const override {
// The linear term g is stored as the last block column of info_ (negated).
// We add (-g) to d.
const size_t n = size();
for (DenseIndex pos = 0; pos < static_cast<DenseIndex>(n); ++pos) {
Key j = keys_[pos];
// info_.aboveDiagonalBlock(pos, n) accesses the block corresponding to g_j
DMap(d + D * j) += info_.aboveDiagonalBlock(pos, n);
}
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
}; }; // end class RegularHessianFactor
// end class RegularHessianFactor
// traits // traits
template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable< template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
RegularHessianFactor<D> > { RegularHessianFactor<D> > {
}; };
}
} // namespace gtsam

160
gtsam/slam/doc/SmartFactors.md Normal file
View File

@ -0,0 +1,160 @@
# Smart Factors
Smart factors in GTSAM provide an efficient way to handle constraints involving landmarks (like 3D points) in Structure from Motion (SfM) or SLAM problems without explicitly including the landmark variables in the optimized state. Instead, the landmark is implicitly represented and marginalized out, leading to factors that directly constrain only the camera-related variables (poses and/or calibrations). This often results in a smaller state space and can significantly speed up optimization, especially when using iterative linear solvers.
The core idea is based on the **Schur complement**. If we consider a factor graph involving cameras $C_i$ and a single landmark $p$, the Hessian matrix of the linearized system has a block structure:
```math
H = \begin{bmatrix} H_{pp} & H_{pc} \\ H_{cp} & H_{cc} \end{bmatrix}
```
where $H_{pp}$ relates to the landmark, $H_{cc}$ relates to the cameras, and $H_{pc}$ ($H_{cp}$) are the off-diagonal blocks. Marginalizing out the landmark variable $\delta_p$ corresponds to solving the system using the Schur complement $S = H_{cc} - H_{cp} H_{pp}^{-1} H_{pc}$. Smart factors effectively represent or compute this Schur complement system directly.
Key Reference:
{cite}`10.1109/ICRA.2014.6907483`.
## Mathematical Details
### 1. Triangulation
The core internal operation of a smart factor is triangulation. Given a set of cameras $C_i$ (which include pose and calibration) and corresponding 2D measurements $z_i$, the factor finds the most likely 3D landmark position $p^*$ by solving:
```math
p^* = \underset{p}{\arg \min} \sum_i \| \Pi(C_i, p) - z_i \|_{\Sigma_i}^2
```
where $\Pi(C_i, p)$ is the projection function of the $i$-th camera, and $\Sigma_i$ is the noise covariance for the $i$-th measurement (though typically a shared noise model $\Sigma$ is used). GTSAM uses the robust `gtsam.triangulateSafe` function internally, which returns a `gtsam.TriangulationResult` containing the point estimate $p^*$ and status information (valid, degenerate, behind camera, etc.).
### 2. Error Function
The nonlinear error minimized by the smart factor is the sum of squared reprojection errors using the *implicitly triangulated* point $p^*$:
```math
\text{error}( \{C_i\} ) = \frac{1}{2} \sum_i \| \Pi(C_i, p^*) - z_i \|_{\Sigma}^2
```
Note that $p^*$ itself is a function of the cameras $\{C_i\}$.
### 3. Linearization
When a smart factor is linearized (e.g., during optimization), it computes a Gaussian factor that only involves the camera variables $\{\delta_{C_i}\}$. This is achieved by implicitly performing the Schur complement.
Let the standard projection factor error for camera $i$ be $e_i(\delta_{C_i}, \delta_p) = F_i \delta_{C_i} + E_i \delta_p - b_i$, where $F_i$ and $E_i$ are the Jacobians with respect to the camera and point, evaluated at the current estimates $C_i^0$ and the triangulated point $p^*$, and $b_i = z_i - \Pi(C_i^0, p^*)$ is the residual. The combined linearized system is:
```math
\sum_i \| F_i \delta_{C_i} + E_i \delta_p - b_i \|_{\Sigma}^2 =
\left\| \begin{bmatrix} F & E \end{bmatrix} \begin{bmatrix} \delta_C \\ \delta_p \end{bmatrix} - b \right\|_{\Sigma}^2
```
where $F = \text{blkdiag}(F_i)$, $E = [E_1^T, ..., E_m^T]^T$, $b = [b_1^T, ..., b_m^T]^T$, and $\delta_C = [\delta_{C_1}^T, ..., \delta_{C_m}^T]^T$. The noise model $\Sigma$ is typically block-diagonal with identical blocks.
Marginalizing $\delta_p$ yields a Gaussian factor on $\delta_C$ with Hessian $H_S$ and information vector $\eta_S$:
```math
H_S = F^T \Sigma^{-1} F - F^T \Sigma^{-1} E (E^T \Sigma^{-1} E)^{-1} E^T \Sigma^{-1} F
```
```math
\eta_S = F^T \Sigma^{-1} b - F^T \Sigma^{-1} E (E^T \Sigma^{-1} E)^{-1} E^T \Sigma^{-1} b
```
Let $P = (E^T \Sigma^{-1} E)^{-1}$ be the point covariance and define the projection matrix $Q = \Sigma^{-1} (I - E P E^T \Sigma^{-1})$. Then the system simplifies to:
```math
H_S = F^T Q F
```
```math
\eta_S = F^T Q b
```
The smart factor's linearization computes or represents this Schur complement system $(H_S, \eta_S)$.
## The Smart Factor Family
GTSAM provides several types of smart factors, primarily for projection measurements. They inherit common functionality from `gtsam.SmartFactorBase`.
### `gtsam.SmartProjectionFactor`
This factor is used when **both camera poses and camera calibration** are unknown and being optimized. It connects multiple `CAMERA` variables, where the `CAMERA` type (e.g., `gtsam.PinholeCameraCal3_S2`) encapsulates both the pose and the intrinsic calibration.
See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorBase.h], [SmartProjectionFactor Notebook](SmartProjectionFactor.ipynb).
### `gtsam.SmartProjectionPoseFactor`
This factor is optimized for the common case where **camera calibration is known and fixed**, and only the camera **poses** (`gtsam.Pose3`) are being optimized. It assumes a single, shared calibration object (`gtsam.Cal3_S2`, `gtsam.Cal3Bundler`, etc.) for all measurements within the factor.
See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartProjectionPoseFactor.h], [SmartProjectionPoseFactor Notebook](SmartProjectionPoseFactor.ipynb).
### `gtsam.SmartProjectionRigFactor`
This factor extends the concept to **multi-camera rigs** where the *relative* poses and calibrations of the cameras within the rig are fixed, but the **rig's body pose** (`gtsam.Pose3`) is being optimized. It allows multiple measurements from different cameras within the rig, potentially associated with the same body pose key.
**Note:** Due to implementation details, the `CAMERA` template parameter used with this factor should typically be `gtsam.PinholePose` rather than `gtsam.PinholeCamera`.
See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartProjectionRigFactor.h], [SmartProjectionRigFactor Notebook](SmartProjectionRigFactor.ipynb).
## Configuration
The behavior of smart factors is controlled by `gtsam.SmartProjectionParams`. Key parameters include:
* `linearizationMode`: Selects which type of `GaussianFactor` is returned by `linearize`, default is `HESSIAN`, see below.
* `degeneracyMode`: Defines how to handle cases where triangulation fails or is ill-conditioned (e.g., collinear cameras). Options include ignoring it, returning a zero-information factor, or treating the point as being at infinity.
* `triangulation`: Parameters passed to `triangulateSafe`, such as rank tolerance for SVD and outlier rejection thresholds.
* `retriangulationThreshold`: If the camera poses change significantly between linearizations (measured by Frobenius norm), the point is re-triangulated.
* `throwCheirality`/`verboseCheirality`: Control behavior when the triangulated point ends up behind one or more cameras.
See also: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorParams.h], [SmartFactorParams Notebook](SmartProjectionParams.ipynb).
## Linear Factor Representations
Depending on the `linearizationMode` specified in `gtsam.SmartProjectionParams`, the `linearize` method of a smart factor returns different types of `gtsam.GaussianFactor` objects, all representing the same underlying Schur complement system but optimized for different solvers:
### 1. `gtsam.RegularHessianFactor` (`HESSIAN` mode, default)
When a smart factor is linearized using the `HESSIAN` mode, it computes and returns a `gtsam.RegularHessianFactor`. In this specific context, this factor explicitly represents the dense **Schur complement** system obtained after marginalizing out the 3D landmark.
* **Defining Feature:** This factor requires that all involved camera variables have the *same, fixed dimension D* (e.g., $D=6$ for `gtsam.Pose3`), specified via a template parameter. This contrasts with the base `gtsam.HessianFactor` which handles variable dimensions.
* **Stored Information:** It stores the blocks of the effective Hessian matrix $H_S$ (denoted internally as $G_{ij}$) and the effective information vector $\eta_S$ (denoted internally as $g_i$) that act *only* on the camera variables.
* **Optimized Operators (Benefit for CG):** Crucially, this regularity allows `RegularHessianFactor` to provide highly optimized implementations for key linear algebra operations, especially the raw-memory (`double*`) version of `multiplyHessianAdd`. This operation computes the Hessian-vector product ($y \leftarrow y + \alpha H_S x$) extremely efficiently by leveraging `Eigen::Map` and fixed-size matrix operations, making it ideal for **iterative solvers like Conjugate Gradient (CG)** which rely heavily on this product. The base `HessianFactor`'s implementations are more general and typically involve more overhead.
* **Formation Cost:** Creating this factor from a smart factor can still be computationally expensive (forming the Schur complement $H_S = F^T Q F$), especially for landmarks observed by many cameras.
* **Solver Suitability:** While efficient for CG (using the `double*` methods), it is also suitable for **direct linear solvers** (like Cholesky or QR factorization) as it provides the explicit system matrix $H_S$.
Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/linear/RegularHessianFactor.h].
### 2. `gtsam.RegularImplicitSchurFactor` (`IMPLICIT_SCHUR` mode)
This factor *stores* the ingredients $F_i$, $E_i$, $P$, and $b_i$ (or rather, their whitened versions) instead of the final Hessian $H_S$. Its key feature is an efficient `multiplyHessianAdd` method that computes the Hessian-vector product $y \leftarrow y + \alpha H_S x$ *without ever forming* the potentially large $H_S$ matrix. It computes this via:
```math
v_i = F_i x_i \\
w = E^T \Sigma^{-1} v \\
d = P w \\
u_i = E_i d \\
y_i \leftarrow y_i + \alpha F_i^T \Sigma^{-1} (v_i - u_i)
```
This is highly efficient for iterative linear solvers like Conjugate Gradient (CG), which primarily rely on Hessian-vector products.
Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/RegularImplicitSchurFactor.h].
### 3. `gtsam.JacobianFactorQ` (`JACOBIAN_Q` mode)
This factor represents the Schur complement system as a Jacobian factor. It computes a projection matrix $Q_{proj} = \Sigma^{-1/2} (I - E P E^T \Sigma^{-1}) \Sigma^{-1/2}$ (where $\Sigma = \text{blkdiag}(\Sigma_i)$) and represents the error term $\| Q_{proj}^{1/2} (F \delta_C - b) \|^2$. It stores the projected Jacobians $A_i = Q_{proj, i}^{1/2} F_i$ and the projected right-hand side $b' = Q_{proj}^{1/2} b$.
Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/JacobianFactorQ.h].
### 4. `gtsam.JacobianFactorSVD` (`JACOBIAN_SVD` mode)
This factor uses the "Nullspace Trick". It computes $E_{null}$, a matrix whose columns form an orthonormal basis for the left nullspace of $E$ (i.e., $E_{null}^T E = 0$). Multiplying the original linearized system by $E_{null}^T \Sigma^{-1}$ yields a smaller system involving only $\delta_C$:
```math
\| E_{null}^T \Sigma^{-1} F \delta_C - E_{null}^T \Sigma^{-1} b \|^2_{(E_{null}^T \Sigma^{-1} E_{null})^{-1}}
```
The factor stores the projected Jacobian $A = E_{null}^T \Sigma^{-1} F$, the projected right-hand side $b' = E_{null}^T \Sigma^{-1} b$, and an appropriate noise model derived from $(E_{null}^T \Sigma^{-1} E_{null})^{-1}$. This method is mathematically equivalent to the Schur complement but can offer computational advantages in certain scenarios.
Source: [https://github.com/borglab/gtsam/blob/develop/gtsam/slam/JacobianFactorSVD.h].
## Conclusion
Smart factors provide a powerful mechanism for efficiently handling landmark-based constraints in SLAM and SfM. By implicitly marginalizing landmarks, they reduce the size of the state space and enable the use of specialized linear factor representations like `gtsam.RegularImplicitSchurFactor`, which are highly effective when combined with iterative solvers like Conjugate Gradient. Understanding the underlying mathematical connection to the Schur complement and the different linearization options allows users to choose the most appropriate configuration for their specific problem and solver.

2
python/gtsam/examples/EKF_SLAM.ipynb
View File

@ -30,7 +30,7 @@
"id": "colab-button-cell-fls", "id": "colab-button-cell-fls",
"metadata": {}, "metadata": {},
"source": [ "source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/examples/EKF_SLAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>" "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/EKF_SLAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
] ]
}, },
{ {