def test_between(self):
"""Test between method."""
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
expected = T2.inverse().compose(T3)
actual = T2.between(T3)
self.gtsamAssertEquals(actual, expected, 1e-6)
def run(
self, num_images: int,
i2Ri1_dict: Dict[Tuple[int, int],
Optional[Rot3]]) -> List[Optional[Rot3]]:
"""Run the rotation averaging.
Args:
num_images: number of poses.
i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1.
Returns:
Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is
`num_images`. The list may contain `None` where the global rotation could not be computed (either
underconstrained system or ill-constrained system).
"""
if len(i2Ri1_dict) == 0:
return [None] * num_images
# create the random seed using relative rotations
seed_rotation = next(iter(i2Ri1_dict.values()))
np.random.seed(int(1000 * seed_rotation.xyz()[0]) % (2 ^ 32))
# TODO: do not assign values where we do not have any edge
# generate dummy rotations
wRi_list = []
for _ in range(num_images):
random_vector = np.random.rand(3) * 2 * np.pi
wRi_list.append(
Rot3.Rodrigues(random_vector[0], random_vector[1],
random_vector[2]))
return wRi_list
def test_transformFrom(self):
"""Test transformFrom method."""
pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi / 2), Point3(2, 4, 0))
actual = pose.transformFrom(Point3(2, 1, 10))
expected = Point3(3, 2, 10)
self.gtsamAssertEquals(actual, expected, 1e-6)
# multi-point version
points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
actual_array = pose.transformFrom(points)
self.assertEqual(actual_array.shape, (3, 2))
expected_array = np.stack([expected, expected]).T
np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
def preintegration_parameters():
# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
I = np.eye(3)
PARAMS.setAccelerometerCovariance(I * 0.1)
PARAMS.setGyroscopeCovariance(I * 0.1)
PARAMS.setIntegrationCovariance(I * 0.1)
PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), Point3(0.05, -0.10, 0.20))
return PARAMS, BIAS_COVARIANCE, DELTA
def test_align_squares(self):
"""Test if Align method can align 2 squares."""
square = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]],
float).T
sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
transformed = sTt.transformTo(square)
st_pairs = Point3Pairs()
for j in range(4):
st_pairs.append((square[:, j], transformed[:, j]))
# Recover the transformation sTt
estimated_sTt = Pose3.Align(st_pairs)
self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
# Matrix version
estimated_sTt = Pose3.Align(square, transformed)
self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
def test_transform_to(self):
"""Test transformTo method."""
transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
actual = transform.transformTo(Point3(3, 2, 10))
expected = Point3(2, 1, 10)
self.gtsamAssertEquals(actual, expected, 1e-6)
def test_transform_to(self):
"""Test transform_to method."""
transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
actual = transform.transform_to(Point3(3, 2, 10))
expected = Point3(2, 1, 10)
self.assertTrue(actual.equals(expected, 1e-6))
def test__between(self):
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
expected = T2.inverse().compose(T3)
actual = T2.between(T3)
self.assertTrue(actual.equals(expected, 1e-6))
def main():
"""
A structure-from-motion example with landmarks
- The landmarks form a 10 meter cube
- The robot rotates around the landmarks, always facing towards the cube
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a NonlinearISAM object which will relinearize and reorder the variables
# every "reorderInterval" updates
isam = NonlinearISAM(reorderInterval=3)
# Create a Factor Graph and Values to hold the new data
graph = NonlinearFactorGraph()
initial_estimate = Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise,
X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM.
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j] + noise
initial_estimate.insert(L(j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
