def __init__(self, initial_state, covariance, alphas, beta):
self._initial_state = gtsam.Pose2(initial_state[0], initial_state[1], initial_state[2])
self._prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([covariance, covariance, covariance]))
# self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([beta[0] ** 2, np.deg2rad(beta[1]) ** 2]))
#self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([beta[0] ** 2, np.deg2rad(beta[1]) ** 2]))
self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array(([np.deg2rad(beta[1]) ** 2, (beta[0] / 100) ** 2])))
self.beta = beta
self.alphas = alphas ** 2
self.pose_num = 0
self.observation_num = 1000
self.landmark_indexes = list()
self.states_new = np.array([[]])
self.observation_new = np.array([[]])
self.graph = gtsam.NonlinearFactorGraph()
self.estimations = gtsam.Values()
self.result = gtsam.Values()
self.parameters = gtsam.ISAM2Params()
self.parameters.setRelinearizeThreshold(1e-4)
self.parameters.setRelinearizeSkip(1)
self.slam = gtsam.ISAM2(self.parameters)
self.graph.add(gtsam.PriorFactorPose2(self.pose_num, self._initial_state, self._prior_noise))
self.estimations.insert(self.pose_num, self._initial_state)
def action_step(self, action, action_noise):
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
# Setting path and initiating DA realization.
path_length = len(self.daRealization)
self.daRealization.append(0)
# Advancing the belief by camera
self.graph.add(
gtsam.BetweenFactorPose3(self.X(path_length),
self.X(path_length + 1), action,
action_noise))
graph_add.add(
gtsam.BetweenFactorPose3(self.X(path_length),
self.X(path_length + 1), action,
action_noise))
# Setting initial values, save an initial value vector for the propagated pose, need it later
if self.initial.exists(self.X(path_length + 1)) is False:
self.initial.insert(
self.X(path_length + 1),
self.initial.atPose3(self.X(path_length)).compose(action))
# self.initial.insert(self.X(path_length + 1), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
if initial_add.exists(self.X(path_length + 1)) is False:
initial_add.insert(
self.X(path_length + 1),
self.initial.atPose3(self.X(path_length)).compose(action))
# initial_add.insert(self.X(path_length + 1), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
# Setting propagated belief object for weight computation
self.prop_belief = self.graph.clone()
self.prop_initial = gtsam.Values()
for key in list(self.initial.keys()):
self.prop_initial.insert(key, self.initial.atPose3(key))
# self.optimize() #TODO: SEE IF WORKS
#print(initial_add.exists(self.X(2)))
self.isam.update(graph_add, initial_add)
self.optimize()
# non-existent object issue is resolved.
self.marginals = gtsam.Marginals(self.prop_belief, self.result)
# Setting resultsFlag to false to require optimization
self.resultsFlag = False
# Multi robot flag for twin measurements
self.sim_measurement_flag = list()
def setUp(self):
"""Set up a small Karcher mean optimization example."""
# Grabbed from KarcherMeanFactor unit tests.
R = Rot3.Expmap(np.array([0.1, 0, 0]))
rotations = {R, R.inverse()} # mean is the identity
self.expected = Rot3()
def check(actual):
# Check that optimizing yields the identity
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
# Check that logging output prints out 3 lines (exact intermediate values differ by OS)
self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3)
# reset stdout catcher
self.capturedOutput.truncate(0)
self.check = check
self.graph = gtsam.NonlinearFactorGraph()
for R in rotations:
self.graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
self.initial = gtsam.Values()
self.initial.insert(KEY, R)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def create_initial_estimate(self):
"""Create initial estimate with landmark map.
Parameters:
pose_estimates - list, pose estimates by measurements
landmark_map - list, A map of landmarks and their correspondence
"""
initial_estimate = gtsam.Values()
# Initial estimate for landmarks
for landmark_idx, observation_list in enumerate(self._landmark_map):
key_point = observation_list[0][1]
pose_idx = observation_list[0][0]
pose = self._pose_estimates[pose_idx]
landmark_3d_point = self.back_projection(key_point, pose,
self._depth)
initial_estimate.insert(P(landmark_idx), landmark_3d_point)
# Filter valid poses
valid_pose_indices = set()
for observation_list in self._landmark_map:
for observation in observation_list:
pose_idx = observation[0]
valid_pose_indices.add(pose_idx)
# Initial estimate for poses
for pose_idx in valid_pose_indices:
initial_estimate.insert(X(pose_idx),
self._pose_estimates[pose_idx])
return initial_estimate
def test_Pose2SLAMExample(self):
# Assumptions
# - All values are axis aligned
# - Robot poses are facing along the X axis (horizontal, to the right in images)
# - We have full odometry for measurements
# - The robot is on a grid, moving 2 meters each step
# Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
# Add prior
# gaussian for prior
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3,
0.1]))
# add directly to graph
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry
# general noisemodel for odometry
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2.0, 0.0, 0.0),
odometryNoise))
graph.add(
gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2.0, 0.0, pi / 2),
odometryNoise))
graph.add(
gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2.0, 0.0, pi / 2),
odometryNoise))
graph.add(
gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2.0, 0.0, pi / 2),
odometryNoise))
# Add pose constraint
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2),
model))
# Initialize to noisy points
initialEstimate = gtsam.Values()
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
# Optimize using Levenberg-Marquardt optimization with an ordering from
# colamd
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimizeSafely()
# Plot Covariance Ellipses
marginals = gtsam.Marginals(graph, result)
P = marginals.marginalCovariance(1)
pose_1 = result.atPose2(1)
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
def test_factor(self):
"""Check that the InnerConstraint factor leaves the mean unchanged."""
# Make a graph with two variables, one between, and one InnerConstraint
# The optimal result should satisfy the between, while moving the other
# variable to make the mean the same as before.
# Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
# R*R*R, i.e. geodesic length is 3 rather than 2.
graph = gtsam.NonlinearFactorGraph()
R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector()
keys.append(1)
keys.append(2)
graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values()
initial.insert(1, R.inverse())
initial.insert(2, R)
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean(
gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2)))
def create_initial_estimate(self, pose_indices):
"""Create initial estimate from ???."""
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # camera to world rotation
depth = 20
initial_estimate = gtsam.Values()
# Create dictionary for initial estimation
for img_idx, features in enumerate(self._image_features):
for kp_idx, keypoint in enumerate(features.keypoints):
if self.image_points[img_idx].kp_3d_exist(kp_idx):
landmark_id = self.image_points[img_idx].kp_3d(kp_idx)
# Filter invalid landmarks
if self._landmark_estimates[
landmark_id].seen >= self._min_landmark_seen:
key_point = Point2(keypoint[0], keypoint[1])
key = P(landmark_id)
if not initial_estimate.exists(key):
# do back-projection
pose = Pose3(wRc,
Point3(0, self.delta_z * img_idx, 2))
landmark_3d_point = self.back_projection(
key_point, pose, depth)
initial_estimate.insert(P(landmark_id),
landmark_3d_point)
# Initial estimate for poses
for idx in pose_indices:
pose_i = Pose3(wRc, Point3(0, self.delta_z * idx, 2))
initial_estimate.insert(X(idx), pose_i)
return initial_estimate
def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'):
"""
circlePose3 generates a set of poses in a circle. This function
returns those poses inside a gtsam.Values object, with sequential
keys starting from 0. An optional character may be provided, which
will be stored in the msb of each key (i.e. gtsam.Symbol).
We use aerospace/navlab convention, X forward, Y right, Z down
First pose will be at (R,0,0)
^y ^ X
| |
z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
Vehicle at p0 is looking towards y axis (X-axis points towards world y)
"""
values = gtsam.Values()
theta = 0.0
dtheta = 2 * pi / numPoses
gRo = gtsam.Rot3(
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
for i in range(numPoses):
key = gtsam.symbol(symbolChar, i)
gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
oRi = gtsam.Rot3.Yaw(
-theta) # negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gRo.compose(oRi), gti)
values.insert(key, gTi)
theta = theta + dtheta
return values
def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
""""Returns global rotations and unit translation directions between 8 cameras
that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
and the unit translations directions between some camera pairs are computed from their
global translations. """
fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0))
# Rotations of the cameras in the world frame.
wRc_values = gtsam.Values()
# Normalized translation directions from camera i to camera j
# in the coordinate frame of camera i.
i_iZj_list = []
for i in range(0, len(wTc_list) - 2):
# Add the rotation.
wRi = wTc_list[i].rotation()
wRc_values.insert(i, wRi)
# Create unit translation measurements with next two poses.
for j in range(i + 1, i + 3):
# Compute the translation from pose i to pose j, in the world reference frame.
w_itj = wTc_list[j].translation() - wTc_list[i].translation()
# Obtain the translation in the camera i's reference frame.
i_itj = wRi.unrotate(w_itj)
# Compute the normalized unit translation direction.
i_iZj = gtsam.Unit3(i_itj)
i_iZj_list.append(
gtsam.BinaryMeasurementUnit3(
i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
# Add the last two rotations.
wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation())
wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation())
return wRc_values, i_iZj_list
def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
wRc_values: gtsam.Values) -> gtsam.Values:
"""Estimate poses given rotations and normalized translation directions between cameras.
Args:
i_iZj_list: List of normalized translation direction measurements between camera pairs,
Z here refers to measurements. The measurements are of camera j with reference
to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit
vector to j in i's frame and is not a transformation.
wRc_values: Rotations of the cameras in the world frame.
Returns:
gtsam.Values: Estimated poses.
"""
# Convert the translation direction measurements to world frame using the rotations.
w_iZj_list = gtsam.BinaryMeasurementsUnit3()
for i_iZj in i_iZj_list:
w_iZj = gtsam.Unit3(
wRc_values.atRot3(i_iZj.key1()).rotate(i_iZj.measured().point3()))
w_iZj_list.append(
gtsam.BinaryMeasurementUnit3(i_iZj.key1(), i_iZj.key2(), w_iZj,
i_iZj.noiseModel()))
# Remove the outliers in the unit translation directions.
w_iZj_inliers = filter_outliers(w_iZj_list)
# Run the optimizer to obtain translations for normalized directions.
wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
wTc_values = gtsam.Values()
for key in wRc_values.keys():
wTc_values.insert(
key, gtsam.Pose3(wRc_values.atRot3(key), wtc_values.atPoint3(key)))
return wTc_values
def main():
"""Main runner."""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first point, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose3() # prior at origin
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
# Add GPS factors
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose3())
print("\nInitial Estimate:\n{}".format(initial))
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
def __init__(self,
p,
q,
r,
alphas=np.array([0.001, 0.0001]),
sensor_offset=np.zeros(2)):
# Add noise models
self.odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(q)
self.measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(r)
self.alphas = alphas
self.sensor_offset = sensor_offset
# Create graph and initilize newest pose
self.graph = gtsam.NonlinearFactorGraph()
self.poses = gtsam.Values()
# To enumerate all poses and landmarks
self.kx = 1
self.kl = 1
self.landmarks = np.empty(0)
# Initilize graph with prior
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(p)
self.graph.add(
gtsam.PriorFactorPose2(X(self.kx), gtsam.Pose2(0.0, 0.0, 0.0),
prior_noise))
def init_smoother(request):
"""
Runs a batch fixed smoother on an agent with two odometry
sensors that is simply moving along the x axis in constant
speed.
"""
global SMOOTHER_BATCH
# Define a batch fixed lag smoother, which uses
# Levenberg-Marquardt to perform the nonlinear optimization
lag = request.lag
smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values()
new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
prior_mean = request.prior_mean
prior_noise = request.prior_noise
X1 = 0
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean)
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
SMOOTHER_BATCH = smoother_batch
SMOOTHER_BATCH.update(new_factors, new_values, new_timestamps)
return X1
def test_jacobian(self):
"""Evaluate jacobian at optical axis"""
obj_point_on_axis = np.array([0, 0, 1])
img_point = np.array([0.0, 0.0])
pose = gtsam.Pose3()
camera = gtsam.Cal3Unified()
state = gtsam.Values()
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
state.insert_cal3unified(camera_key, camera)
state.insert_point3(landmark_key, obj_point_on_axis)
state.insert_pose3(pose_key, pose)
g = gtsam.NonlinearFactorGraph()
noise_model = gtsam.noiseModel.Unit.Create(2)
factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model,
pose_key, landmark_key,
camera_key)
g.add(factor)
f = g.error(state)
gaussian_factor_graph = g.linearize(state)
H, z = gaussian_factor_graph.jacobian()
self.assertAlmostEqual(f, 0)
self.gtsamAssertEquals(z, np.zeros(2))
self.gtsamAssertEquals(H @ H.T, 4 * np.eye(2))
Dcal = np.zeros((2, 10), order='F')
Dp = np.zeros((2, 2), order='F')
camera.calibrate(img_point, Dcal, Dp)
self.gtsamAssertEquals(
Dcal,
np.array([[0., 0., 0., -1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]]))
self.gtsamAssertEquals(Dp, np.array([[1., -0.], [-0., 1.]]))
def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1):
""" priorMean and priorCov should be in 1 dimensional array
"""
# init the graph
self.graph = gtsam.NonlinearFactorGraph()
# init the iSam2 solver
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(relinearizeThreshold)
parameters.setRelinearizeSkip(relinearizeSkip)
self.isam = gtsam.ISAM2(parameters)
# init container for initial values
self.initialValues = gtsam.Values()
# setting the current position
self.currentKey = 1
# current estimate
self.currentEst = False
self.currentPose = [0, 0, 0]
return
def test_perturbPose2(self):
"""Test perturbPose2."""
values = gtsam.Values()
values.insert(0, gtsam.Pose2())
values.insert(1, gtsam.Point2(1, 1))
gtsam.utilities.perturbPose2(values, 1, 1)
self.assertTrue(values.atPose2(0) != gtsam.Pose2())
def log_pdf_value(self, measurement, point_x, point_xo):
"""
:type point_x: gtsam.Pose3
:type point_xo: gtsam.Pose3
:type measurement: gtsam.Pose3
"""
meas_x = measurement[2] * np.cos(measurement[0] / 180 * np.pi) \
* np.cos(measurement[1] / 180 * np.pi)
meas_y = measurement[2] * np.sin(measurement[0] / 180 * np.pi) \
* np.cos(measurement[1] / 180 * np.pi)
meas_z = measurement[2] * np.sin(measurement[1] / 180 * np.pi)
pose_meas = gtsam.Pose3(gtsam.Rot3.Ypr(0.0, 0.0, 0.0),
np.array([meas_x, meas_y, meas_z]))
# Create an auxilary factor and compute his log-pdf value (without the normalizing constant)
aux_factor = gtsam.BetweenFactorPose3(1, 2, pose_meas,
self.model_noise)
poses = gtsam.Values()
poses.insert(1, point_x)
poses.insert(2, point_xo)
return aux_factor.error(poses)
def step5_add_odometry_and_landmark_observations():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X2 = X(2)
X3 = X(3)
L2 = L(2)
# Update the list with the new variable keys.
pose_variables.append(X3)
landmark_variables.append(L2)
# Add the odometry measurement.
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
def test_extractPoint3(self):
"""Test extractPoint3."""
initial = gtsam.Values()
point3 = gtsam.Point3(0.0, 0.1, 0.0)
initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
initial.insert(2, point3)
np.testing.assert_equal(gtsam.utilities.extractPoint3(initial),
point3.reshape(-1, 3))
def test_perturbPoint3(self):
"""Test perturbPoint3."""
values = gtsam.Values()
point3 = gtsam.Point3(0, 0, 0)
values.insert(0, gtsam.Pose2())
values.insert(1, point3)
gtsam.utilities.perturbPoint3(values, 1)
self.assertTrue(not np.allclose(values.atPoint3(1), point3))
def test_perturbPoint2(self):
"""Test perturbPoint2."""
values = gtsam.Values()
values.insert(0, gtsam.Pose3())
values.insert(1, gtsam.Point2(1, 1))
gtsam.utilities.perturbPoint2(values, 1.0)
self.assertTrue(
not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1)))
def test_extractPose3(self):
"""Test extractPose3."""
initial = gtsam.Values()
pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.])
initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
initial.insert(2, gtsam.Pose3())
np.testing.assert_allclose(gtsam.utilities.extractPose3(initial),
pose3.reshape(-1, 12))
def __init__(self, IMU_PARAMS=None, BIAS_COVARIANCE=None):
"""
Define factor graph parameters (e.g. noise, camera calibrations, etc) here
"""
self.graph = gtsam.NonlinearFactorGraph()
self.initial_estimate = gtsam.Values()
self.IMU_PARAMS = IMU_PARAMS
self.BIAS_COVARIANCE = BIAS_COVARIANCE
def add_mr_object_measurement(self,
stack,
class_realization,
new_input_object_prior=None,
new_input_object_covariance=None):
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
idx = 0
for obj in stack:
cov_noise_model = stack[obj]['covariance']
exp_gtsam = gtsam.Pose3(
gtsam.Rot3.Ypr(stack[obj]['expectation'][2],
stack[obj]['expectation'][1],
stack[obj]['expectation'][0]),
gtsam.Point3(stack[obj]['expectation'][3],
stack[obj]['expectation'][4],
stack[obj]['expectation'][5]))
pose_rotation_matrix = exp_gtsam.rotation().matrix()
cov_noise_model_rotated = self.rotate_cov_6x6(
cov_noise_model, np.transpose(pose_rotation_matrix))
cov_noise_model_rotated = gtsam.noiseModel.Gaussian.Covariance(
cov_noise_model_rotated)
#updating the graph
self.graph.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
graph_add.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
self.prop_belief.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
if self.prop_initial.exists(self.XO(obj)) is False:
self.prop_initial.insert(
self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
# if obj in new_input_object_prior and obj not in self.classRealization:
# self.graph.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj],
# new_input_object_covariance[obj]))
# self.prop_belief.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj],
# new_input_object_covariance[obj]))
self.classRealization[obj] = class_realization[obj]
if self.isam.value_exists(self.XO(obj)) is False:
initial_add.insert(self.XO(obj),
gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
idx += 1
if stack[obj]['weight'] > -322:
self.logWeight += stack[obj]['weight'] # + 150
else:
self.logWeight = -math.inf
def estimate_global(self):
np.random.seed(2)
n0 = 0.000001
n03 = np.array([n0, n0, n0])
nNoiseFactor3 = np.array(
[0.2, 0.2,
0.2]) # TODO: something about floats and major row? check cython
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose, setting it to the origin
priorMean = self.odom_global[0]
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(n03)
graph.add(gtsam.PriorFactorPose2(self.X(1), priorMean, priorNoise))
# Add odometry factors
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(nNoiseFactor3)
for i, pose in enumerate(self.odom_relative):
graph.add(
gtsam.BetweenFactorPose2(self.X(i + 1), self.X(i + 2), pose,
odometryNoise))
# Add visual factors
visualNoise = gtsam.noiseModel_Diagonal.Sigmas(nNoiseFactor3)
for i, pose in enumerate(self.visual_relative):
graph.add(
gtsam.BetweenFactorPose2(self.X(i + 1), self.X(i + 2), pose,
visualNoise))
# set initial guess to odometry estimates
initialEstimate = gtsam.Values()
for i, pose in enumerate(self.odom_global):
initialEstimate.insert(self.X(i + 1), pose)
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
# gtsam_quadrics.setMaxIterations(params, iter)
# gtsam_quadrics.setRelativeErrorTol(params, 1e-8)
# gtsam_quadrics.setAbsoluteErrorTol(params, 1e-8)
# graph.print_('graph')
# initialEstimate.print_('initialEstimate ')
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate,
params)
# parameters = gtsam.GaussNewtonParams()
# parameters.relativeErrorTol = 1e-8
# parameters.maxIterations = 300
# optimizer = gtsam.GaussNewtonOptimizer(graph, initialEstimate, parameters)
result = optimizer.optimize()
# result.print_('result ')
# self.draw_trajectories([self.odom_global, self.visual_global], ['b', 'r'], 2)
return self.unwrap_results(result)
def test_SFMExample(self):
options = generator.Options()
options.triangle = False
options.nrCameras = 10
[data, truth] = generator.generate_data(options)
measurementNoiseSigma = 1.0
pointNoiseSigma = 0.1
poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
graph = gtsam.NonlinearFactorGraph()
# Add factors for all measurements
measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma)
for i in range(len(data.Z)):
for k in range(len(data.Z[i])):
j = data.J[i][k]
graph.add(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], measurementNoise,
X(i), P(j), data.K))
posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(X(0),
truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(P(0),
truth.points[0], pointPriorNoise))
# Initial estimate
initialEstimate = gtsam.Values()
for i in range(len(truth.cameras)):
pose_i = truth.cameras[i].pose()
initialEstimate.insert(X(i), pose_i)
for j in range(len(truth.points)):
point_j = truth.points[j]
initialEstimate.insert(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
for i in range(5):
optimizer.iterate()
result = optimizer.values()
# Marginalization
marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(P(0))
marginals.marginalCovariance(X(0))
# Check optimized results, should be equal to ground truth
for i in range(len(truth.cameras)):
pose_i = result.atPose3(X(i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(P(j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
def test_insertBackprojections(self):
"""Test insertBackprojections."""
values = gtsam.Values()
cam = gtsam.PinholeCameraCal3_S2()
gtsam.utilities.insertBackprojections(
values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
10)
np.testing.assert_allclose(values.atPoint3(0),
gtsam.Point3(200, 200, 10))
def __init__(self):
self.gtsam_graph = gtsam.NonlinearFactorGraph()
self.factor_edges = {} # adjacency list (dict)
# Variables
self.values = gtsam.Values()
self.vars = {}
self.n_variables = 0
def test_extractPose2(self):
"""Test extractPose2."""
initial = gtsam.Values()
pose2 = np.asarray((0.0, 0.1, 0.1))
initial.insert(1, gtsam.Pose2(*pose2))
initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0))
np.testing.assert_allclose(gtsam.utilities.extractPose2(initial),
pose2.reshape(-1, 3))
def test_allPose3s(self):
"""Test allPose3s."""
initial = gtsam.Values()
initial.insert(0, gtsam.Pose3())
initial.insert(2, gtsam.Point2(1, 1))
initial.insert(1, gtsam.Pose3())
initial.insert(3, gtsam.Point3(1, 2, 3))
result = gtsam.utilities.allPose3s(initial)
self.assertEqual(result.size(), 2)
