Feature: parameter for whether `BetweenFactor` is world fixed or body fixed relative to the first state.

[mattbrown11]

First, thanks for developing and supporting such a great library and releasing it as BSD. Copyleft is a deal breaker for my use case, and all other SLAM tools seemed to be copyleft.

Feature

My understanding is that when BetweenFactor is applied with references to two instances of Pose3, the measurement is an SE3 (rotation and transformation) defined relative to the world. However, it would be useful to have a factor like BetweenFactor (assuming one doesn't already exist and I just missed it) where the SE3 measurement is defined relative to the coordinate system of the first Pose3. It seems something similar is supported via PreintegratedImuMeasurements and ImuFactor, where the integrated results yields a change in velocity and a rotation defined relative to the first Pose3, but it isn't exactly the same.

Motivation

I am exploring using GTSAM for SLAM in my ADAPT Payload Project, where we have a hardware INS that works decently well for the most part (generally unobstructed view of the sky). But, I want to incorporate visual odometry, which I have been able to do successfully using GTSAM with the INS-reported solution used as a PriorFactorPose3 on the pose associated with each image. The resulting solution is consistent with both the INS measurements and the image measurements in terms of number of sigmas of deviation for each factor being small. But, I can see some high-frequency jitter in the image-to-image pose that is objectively unrealistic. So, I wanted to the additionally constraint that the error in the INS-reported pose changes relatively slowly, e.g., the dead reckoning estimate from the INS over 1/10 second should be much more accurate than the absolute-position covariance. So, I wanted to use a BetweenFactor between poses to smooth out the unrealistic oscillations. But, the current BetweenFactor defines the relative poses in terms of an SE3 transformation relative to the world coordinate system, when I really need it relative to the initial pose in the pair.

Pitch

Add a parameter to indicate whether the BetweenFactor is world fixed or body fixed relative to the first state.

[dellaert]

Thanks! BetweenFactor is generic, not specific to Pose3, but when applied to Pose3 it will only care about the relative pose between the two poses. Hence, which coordinate frame they are defined in should theoretically not matter. Maybe I’m misunderstanding.

[mattbrown11]

I had initially done an experiment with the code that led me to believe the BetweenFactorPose3 was defined relative to the world coordinate system, but it seems that experiment was invalid. My experiment below confirms that BetweenFactorPose3 is indeed an SE3 relative to the first state. Thanks for confirming.

import gtsam
from gtsam import NonlinearFactorGraph, Pose3, PriorFactorPoint3, Rot3, \
    Values, PriorFactorPose3, BetweenFactorPose3
import transformations
import numpy as np
from gtsam import symbol_shorthand
X = symbol_shorthand.X

# Basically imply certainty.
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(6)*1e-6)

# Define the initial orientation.
y1 = np.pi
p1 = np.pi/2
r1 = 0
pose1 = Pose3(Rot3(transformations.euler_matrix(y1, p1, r1, axes='rzyx')[:3, :3]),
              np.array([0, 0, 0]))

So, we start with an orientation given by the matrix: [[ 0 0 -1 ] [ 0 -1 0 ] [-1 0 0 ]] i.e., moving coordinate system axes x,y,z relative to world X,Y,Z: x=-Z, y=-Y, z=-X

# Initialize to identity, but it will be updated.
pose2 = Pose3()

# Define the between rotation. Here, a small positive rotation around z.
y2 = 0.1
p2 = 0
r2 = 0
dpose = Pose3(Rot3(transformations.euler_matrix(y2, p2, r2, axes='rzyx')[:3, :3]),
              np.array([0, 0, 0]))

dpose is a small rotation around the z axis, but we'll use it to interrogate which z axis (body z or world Z).

# Define factor graph
graph = NonlinearFactorGraph()
initial_estimate = Values()
initial_estimate.insert(X(0), pose1)
initial_estimate.insert(X(1), pose2)
factor = PriorFactorPose3(X(0), pose1, pose_noise)
graph.push_back(factor)
factor = BetweenFactorPose3(X(0), X(1), dpose, pose_noise)
graph.push_back(factor)

# Solve
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
result = optimizer.optimize()

R1 = pose1.rotation().matrix()
dR = dpose.rotation().matrix()
R2 = result.atPose3(X(1)).rotation().matrix()

print('Initial orientation\n', R1)
print('applied rotation applied\n', dR)
print('with rotation applied\n', R2)

print('Result if dR is a rotation relative to the world coordinate system\n',
      np.dot(dR, R1))

gtsam returns R1 = [[ 0 0 -1 ] R2 = [[ 0, 0, -1 ], [ 0 -1 0 ] -> [-0.0998..., -0.995..., 0 ], [-1 0 0 ]] [-0.995..., 0.0998..., 0 ]] which is a small rotation around pose-1 z-axis, which was at world -X.

[dellaert]

Yay!