InitializePose3 - GTSAM Docs

import gtsam
import numpy as np
from gtsam import NonlinearFactorGraph, Values, Pose3, Rot3, Point3, PriorFactorPose3, BetweenFactorPose3
from gtsam import InitializePose3
from gtsam import symbol_shorthand

X = symbol_shorthand.X

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. The core idea is to first estimate the orientations (Rot3) independently and then use these estimates to initialize a linear system for the translations.

Key static methods:

buildPose3graph(graph): Extracts the subgraph containing only Pose3 BetweenFactor and PriorFactor constraints from a larger NonlinearFactorGraph.
computeOrientationsChordal(pose3Graph): Estimates rotations using chordal relaxation on the rotation constraints.
computeOrientationsGradient(pose3Graph, initialGuess): Estimates rotations using gradient descent on the manifold.
initializeOrientations(graph): Convenience function combining buildPose3graph and computeOrientationsChordal.
computePoses(initialRot, poseGraph): Computes translations given estimated rotations by solving a linear system (performing one Gauss-Newton iteration on poses).
initialize(graph): Performs the full initialization pipeline (extract graph, estimate rotations via chordal, compute translations).
initialize(graph, givenGuess, useGradient): Full pipeline allowing specification of an initial guess and choosing between chordal or gradient descent for rotations.

Example Initialization Pipeline¶

We’ll create a simple 3D pose graph and use the InitializePose3.initialize method to get an initial estimate.

# 1. Create a NonlinearFactorGraph with Pose3 factors
graph = NonlinearFactorGraph()

# Add a prior on the first pose
prior_mean = Pose3(Rot3.Yaw(0.1), Point3(0.1, 0, 0))
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.05]*3))
graph.add(PriorFactorPose3(X(0), prior_mean, prior_noise))

# Add odometry factors
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05]*3 + [0.2]*3))
odometry1 = Pose3(Rot3.Yaw(0.5), Point3(1.0, 0.1, 0))
odometry2 = Pose3(Rot3.Yaw(0.4), Point3(0.9, -0.1, 0))
graph.add(BetweenFactorPose3(X(0), X(1), odometry1, odometry_noise))
graph.add(BetweenFactorPose3(X(1), X(2), odometry2, odometry_noise))

# Add a loop closure factor (less certain)
loop_mean = Pose3(Rot3.Yaw(-0.9), Point3(-1.8, 0.05, 0))
loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1]*3 + [0.4]*3))
graph.add(BetweenFactorPose3(X(2), X(0), loop_mean, loop_noise))

graph.print("Original Factor Graph:\n")

# 2. Perform initialization using the default chordal relaxation method
initial_estimate = InitializePose3.initialize(graph)

print("\nInitial Estimate (using InitializePose3.initialize):\n")
initial_estimate.print()

# 3. (Optional) Perform initialization step-by-step
# pose3graph = InitializePose3.buildPose3graph(graph)
# initial_orientations = InitializePose3.initializeOrientations(graph)
# initial_estimate_manual = InitializePose3.computePoses(initial_orientations, pose3graph)
# print("\nInitial Estimate (Manual Steps):\n")
# initial_estimate_manual.print()

Original Factor Graph:
size: 4

Factor 0: PriorFactor on x0
  prior mean:  R: [
	0.995004, -0.0998334, 0;
	0.0998334, 0.995004, 0;
	0, 0, 1
]
t: 0.1   0   0
  noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.05; 0.05; 0.05];

Factor 1: BetweenFactor(x0,x1)
  measured:  R: [
	0.877582562, -0.479425539, 0;
	0.479425539, 0.877582562, 0;
	0, 0, 1
]
t:   1 0.1   0
  noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];

Factor 2: BetweenFactor(x1,x2)
  measured:  R: [
	0.921060994, -0.389418342, 0;
	0.389418342, 0.921060994, 0;
	0, 0, 1
]
t:  0.9 -0.1    0
  noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];

Factor 3: BetweenFactor(x2,x0)
  measured:  R: [
	0.621609968, 0.78332691, 0;
	-0.78332691, 0.621609968, 0;
	0, 0, 1
]
t: -1.8 0.05    0
  noise model: diagonal sigmas [0.1; 0.1; 0.1; 0.4; 0.4; 0.4];



Initial Estimate (using InitializePose3.initialize):

Values with 3 values:
Value x0: (class gtsam::Pose3)
R: [
	0.995004165, -0.0998334166, 0;
	0.0998334166, 0.995004165, 0;
	0, 0, 1
]
t:             0.1 -7.63278329e-15               0

Value x1: (class gtsam::Pose3)
R: [
	0.825335615, -0.564642473, 0;
	0.564642473, 0.825335615, 0;
	0, 0, 1
]
t: 0.956742586 0.343109526           0

Value x2: (class gtsam::Pose3)
R: [
	0.540302306, -0.841470985, 0;
	0.841470985, 0.540302306, 0;
	0, 0, 1
]
t:  1.62773065 0.912529884           0

Notes¶

The quality of the initial estimate depends heavily on the quality and connectivity of the pose graph factors.
Chordal relaxation (computeOrientationsChordal) is generally faster and often sufficient.
Gradient descent (computeOrientationsGradient) might provide slightly more accurate orientations but is slower and requires a good initial guess.
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).