def add_point(self, pointsInitial, measurements, octave):
if pointsInitial[-1] > self.depth_threshold:
information = self.inv_lvl_sigma2[octave] * np.identity(2)
stereo_model = gt.noiseModel_Diagonal.Information(information)
huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaMono)
robust_model = gt.noiseModel_Robust(huber, stereo_model)
factor = gt.GenericProjectionFactorCal3_S2(
gt.Point2(measurements[0], measurements[2]), robust_model,
X(1), L(self.counter), self.K_mono)
self.is_stereo.append(False)
else:
information = self.inv_lvl_sigma2[octave] * np.identity(3)
stereo_model = gt.noiseModel_Diagonal.Information(information)
huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaStereo)
robust_model = gt.noiseModel_Robust(huber, stereo_model)
factor = gt.GenericStereoFactor3D(
gt.StereoPoint2(*tuple(measurements)), robust_model, X(1),
L(self.counter), self.K_stereo)
self.is_stereo.append(True)
self.graph.add(
gt.NonlinearEqualityPoint3(L(self.counter),
gt.Point3(pointsInitial)))
self.initialEstimate.insert(L(self.counter), gt.Point3(pointsInitial))
self.graph.add(factor)
self.octave.append(octave)
self.counter += 1
def plot_2d(result, isam, ax, seen):
idx = np.ix_((0, -1), (0, -1))
ax.cla()
# plot cameras
i = 0
cameras = []
while result.exists(X(i)):
# get all data from pose
pose = result.atPose3(X(i))
# R = pose.rotation().matrix()
# cov = isam.marginalCovariance(X(i))[3:6,3:6]
# cov = ( R@[email protected] )[idx]
t = [result.atPose3(X(i)).x(), result.atPose3(X(i)).z()]
# do we want pose covariances?
# ax.add_patch(cov_patch(t, cov, 'b'))
cameras.append(t)
i += 1
# plot landmarks
landmarks = []
for i in seen:
# get all data from pose
pose = result.atPose3(L(i))
R = pose.rotation().matrix()
cov = isam.marginalCovariance(L(i))[3:6, 3:6]
cov = (R @ cov @ R.T)[idx]
t = [result.atPose3(L(i)).x(), result.atPose3(L(i)).z()]
ax.add_patch(cov_patch(t, cov, 'r'))
landmarks.append(t)
cameras = np.array(cameras)
landmarks = np.array(landmarks)
ax.plot(cameras[:, 0],
cameras[:, 1],
label="Camera Poses",
c='b',
marker='o')
ax.scatter(landmarks[:, 0], landmarks[:, 1], label="Landmarks", c='r')
ax.legend()
ax.set_xlabel("X")
ax.set_ylabel("Z")
ax.set_aspect('equal')
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 plot_3d(result, ax, seen):
ax.cla()
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3_on_axes(ax, pose_i, 8)
i += 1
# plot landmarks
if isinstance(seen, int):
seen = np.arange(seen)
for i in seen:
try:
pose_i = result.atPose3(L(i))
gtsam_plot.plot_pose3_on_axes(ax, pose_i, 4)
except:
pass
# draw
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
set_axes_equal(ax)
ax.view_init(-55, -85)
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 __init__(self):
# Model initialization
joint_lambda_pose_factor_fake_1d.initialize_python_objects()
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
def __init__(self, exp_add_1, exp_add_2, exp_add_3, exp_add_4, exp_add_5,
rinf_add_1, rinf_add_2, rinf_add_3, rinf_add_4, rinf_add_5):
# Model initialization
joint_lambda_pose_factor_2d.initialize_python_objects(
exp_add_1, exp_add_2, exp_add_3, exp_add_4, exp_add_5, rinf_add_1,
rinf_add_2, rinf_add_3, rinf_add_4, rinf_add_5)
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
def __init__(self,
geo_model,
da_model,
lambda_model,
prior_mean,
prior_noise,
lambda_prior_mean=(0., 0.),
lambda_prior_noise=((0.5, 0.), (0., 0.5)),
cls_enable=True):
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
# Enable Lambda inference
self.cls_enable = cls_enable
# Camera pose prior
self.graph = gtsam.NonlinearFactorGraph()
self.graph.add(
gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise))
# Setting initial values
self.initial = gtsam.Values()
self.initial.insert(self.X(0), prior_mean)
self.prev_step_camera_pose = prior_mean
self.daRealization = list()
# Setting up models
self.geoModel = geo_model
self.daModel = da_model
self.lambdaModel = lambda_model
# Setting up ISAM2
params2 = gtsam.ISAM2Params()
#params2.relinearize_threshold = 0.01
#params2.relinearize_skip = 1
self.isam = gtsam.ISAM2(params2)
self.isam.update(self.graph, self.initial)
# Set custom lambda
if type(lambda_prior_mean) is np.ndarray:
self.lambda_prior_mean = lambda_prior_mean
else:
self.lambda_prior_mean = np.array(lambda_prior_mean)
self.lambda_prior_cov = gtsam.noiseModel.Gaussian.Covariance(
np.matrix(lambda_prior_noise))
self.num_cls = len(self.lambda_prior_mean) + 1
# Initialize object last lambda database
self.object_lambda_dict = dict()
def test_generic_factor(self):
"""Evaluate residual using pose and point as state variables"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
pose_key, point_key = P(0), L(0)
k = self.stereographic
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(point_key, self.obj_point)
factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
def test_sfm_factor2(self):
"""Evaluate residual with camera, pose and point as state variables"""
r = np.linalg.norm(self.obj_point)
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
k = self.stereographic
state.insert_cal3unified(camera_key, k)
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(landmark_key, self.obj_point)
factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
def evaluate_jacobian(obj_point, img_point):
"""Evaluate jacobian at given object point"""
pose = gtsam.Pose3()
camera = gtsam.Cal3Fisheye()
state = gtsam.Values()
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
state.insert_point3(landmark_key, obj_point)
state.insert_pose3(pose_key, pose)
g = gtsam.NonlinearFactorGraph()
noise_model = gtsam.noiseModel.Unit.Create(2)
factor = gtsam.GenericProjectionFactorCal3Fisheye(
img_point, noise_model, pose_key, landmark_key, camera)
g.add(factor)
f = g.error(state)
gaussian_factor_graph = g.linearize(state)
H, z = gaussian_factor_graph.jacobian()
return f, z, H
def step4_add_new_landmark_observation():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X2 = X(2)
L1 = L(1)
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Update ISAM2 with the new factor graph.
# There are no new variables this update, so no new initial values.
isam.update(graph, gtsam.Values())
def step2_add_landmark_observation():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X1 = X(1)
L1 = L(1)
# Update the list with the new landmark variable key.
landmark_variables.append(L1)
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), measurement_noise))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
from gtsam.symbol_shorthand import X, L
# Create noise models
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = X(1)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(gtsam.BetweenFactorPose2(
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(gtsam.BearingRangeFactor2D(
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_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 an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
# to maintain proper linearization and efficient variable ordering, iSAM2
# performs partial relinearization/reordering at each step. A parameter
# structure is available that allows the user to set various properties, such
# as the relinearization threshold and type of linear solver. For this
# example, we we set the relinearization threshold small so the iSAM2 result
# will approach the batch result.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.01)
parameters.setRelinearizeSkip(1)
isam = gtsam.ISAM2(parameters)
# Create a Factor Graph and Values to hold the new data
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
# Add factors for each landmark observation
for j, point in enumerate(points):
camera = gtsam.PinholeCameraCal3_S2(pose, K)
measurement = camera.project(point)
graph.push_back(
gtsam.GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
initial_estimate.insert(
X(i),
pose.compose(
gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25),
gtsam.Point3(0.05, -0.10, 0.20))))
# 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
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3,
0.3])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
# Add a prior on landmark l0
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
graph.push_back(
gtsam.PriorFactorPoint3(L(0), points[0],
point_noise)) # add directly to graph
# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initial_estimate.insert(
L(j),
gtsam.Point3(point[0] - 0.25, point[1] + 0.20,
point[2] + 0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
# If accuracy is desired at the expense of time, update(*) can be called additional
# times to perform multiple optimizer iterations every step.
isam.update()
current_estimate = isam.calculateEstimate()
print("****************************************************")
print("Frame", i, ":")
for j in range(i + 1):
print(X(j), ":", current_estimate.atPose3(X(j)))
for j in range(len(points)):
print(L(j), ":", current_estimate.atPoint3(L(j)))
visual_ISAM2_plot(current_estimate)
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
plt.ioff()
plt.show()
import numpy as np
import gtsam
from gtsam.symbol_shorthand import X, O, L
import lambda_prior_factor
#import joint_lambda_pose_factor_2d as joint_lambda_pose_factor_fake_2d
import joint_lambda_pose_factor_2d as joint_lambda_pose_factor_fake_2d
#import libJointLambdaPoseFactor
#import libJointLambdaPoseFactorFake
#import gaussianb_jlp
# libLambdaPriorFactor
# Symbol initialization
#X = lambda i: X(i)
XO = lambda j: O(j)
Lambda = lambda k: L(k)
# joint_lambda_pose_factor_fake_2d.initialize_python_objects()
joint_lambda_pose_factor_fake_2d.initialize_python_objects(
'../exp_model_Book Jacket.pt', '../exp_model_Digital Clock.pt',
'../exp_model_Packet.pt', '../exp_model_Pop Bottle.pt',
'../exp_model_Soap Dispenser.pt', '../rinf_model_Book Jacket.pt',
'../rinf_model_Digital Clock.pt', '../rinf_model_Packet.pt',
'../rinf_model_Pop Bottle.pt', '../rinf_model_Soap Dispenser.pt')
# Graph initialization
graph_1 = gtsam.NonlinearFactorGraph()
# Add prior lambda
prior_noise = gtsam.noiseModel.Diagonal.Covariance(
np.array([[3.2, 0.0, 0.0, 0.0], [0.0, 3.2, 0.0, 0.0], [0.0, 0.0, 3.2, 0.0],
[0.0, 0.0, 0.0, 3.2]]))
def batch_factorgraph_example():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create the keys for the poses.
X1 = X(1)
X2 = X(2)
X3 = X(3)
pose_variables = [X1, X2, X3]
# Create keys for the landmarks.
L1 = L(1)
L2 = L(2)
landmark_variables = [L1, L2]
# Add a prior on pose X1 at the origin.
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise))
# Add odometry factors between X1,X2 and X2,X3, respectively
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1,
0.1]))
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add Range-Bearing measurements to two different landmarks L1 and L2
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Create an optimizer.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
# Solve the MAP problem.
result = optimizer.optimize()
# Calculate marginal covariances for all variables.
marginals = gtsam.Marginals(graph, result)
# Extract marginals
pose_marginals = []
for var in pose_variables:
pose_marginals.append(
MultivariateNormalParameters(result.atPose2(var),
marginals.marginalCovariance(var)))
landmark_marginals = []
for var in landmark_variables:
landmark_marginals.append(
MultivariateNormalParameters(result.atPoint2(var),
marginals.marginalCovariance(var)))
# You can extract the joint marginals like this.
joint_all = marginals.jointMarginalCovariance(
gtsam.KeyVector(pose_variables + landmark_variables))
print("Joint covariance over all variables:")
print(joint_all.fullMatrix())
# Plot the marginals.
plot_result(pose_marginals, landmark_marginals)
def add_keypoints(self, vision_data, measured_poses, n_skip, depth):
measured_poses = np.reshape(measured_poses, (-1, 4, 4))
# pose_key = X(0)
# self.initial_estimate.insert(pose_key, gtsam.Pose3(measured_poses[0]))
# R_rect = np.array([[9.999239e-01, 9.837760e-03, -7.445048e-03, 0.],
# [ -9.869795e-03, 9.999421e-01, -4.278459e-03, 0.],
# [ 7.402527e-03, 4.351614e-03, 9.999631e-01, 0.],
# [ 0., 0., 0., 1.]])
# R_cam_velo = np.array([[7.533745e-03, -9.999714e-01, -6.166020e-04],
# [ 1.480249e-02, 7.280733e-04, -9.998902e-01],
# [ 9.998621e-01, 7.523790e-03, 1.480755e-02]])
# R_velo_imu = np.array([[9.999976e-01, 7.553071e-04, -2.035826e-03],
# [-7.854027e-04, 9.998898e-01, -1.482298e-02],
# [2.024406e-03, 1.482454e-02, 9.998881e-01]])
# t_cam_velo = np.array([-4.069766e-03, -7.631618e-02, -2.717806e-01])
# t_velo_imu = np.array([-8.086759e-01, 3.195559e-01, -7.997231e-01])
# T_velo_imu = np.zeros((4,4))
# T_cam_velo = np.zeros((4,4))
# T_velo_imu[3,3] = 1.
# T_cam_velo[3,3] = 1.
# T_velo_imu[:3,:3] = R_velo_imu
# T_velo_imu[:3,3] = t_velo_imu
# T_cam_velo[:3,:3] = R_cam_velo
# T_cam_velo[:3,3] = t_cam_velo
# cam_to_imu = R_rect @ T_cam_velo @ T_velo_imu
# CAM_TO_IMU_POSE = gtsam.Pose3(cam_to_imu)
# imu_to_cam = np.linalg.inv(cam_to_imu)
# IMU_TO_CAM_POSE = gtsam.Pose3(imu_to_cam)
# K_np = np.array([[9.895267e+02, 0.000000e+00, 7.020000e+02],
# [0.000000e+00, 9.878386e+02, 2.455590e+02],
# [0.000000e+00, 0.000000e+00, 1.000000e+00]])
K_np = self.K
K = gtsam.Cal3_S2(K_np[0, 0], K_np[1, 1], 0., K_np[0, 2], K_np[1, 2])
valid_track = np.zeros(vision_data.shape[0], dtype=bool)
N = vision_data.shape[1]
for i in range(vision_data.shape[0]):
track_length = N - np.sum(vision_data[i, :, 0] == -1)
if track_length > 1 and track_length < 0.5 * N:
valid_track[i] = True
count = 0
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 10.0)
for i in range(0, vision_data.shape[0], 20):
if not valid_track[i]:
continue
key_point_initialized = False
for j in range(vision_data.shape[1] - 1):
if vision_data[i, j, 0] >= 0:
zp = float(depth[j * n_skip][vision_data[i, j, 1],
vision_data[i, j, 0]])
if zp == 0:
continue
self.graph.push_back(
gtsam.GenericProjectionFactorCal3_S2(
vision_data[i, j, :], measurement_noise, X(j),
L(i), K, gtsam.Pose3(np.eye(4))))
if not key_point_initialized:
count += 1
# Initialize landmark 3D coordinates
fx = K_np[0, 0]
fy = K_np[1, 1]
cx = K_np[0, 2]
cy = K_np[1, 2]
# Depth:
zp = float(depth[j * n_skip][vision_data[i, j, 1],
vision_data[i, j, 0]])
xp = float(vision_data[i, j, 0] - cx) / fx * zp
yp = float(vision_data[i, j, 1] - cy) / fy * zp
# Convert to global
Xg = measured_poses[j * n_skip] @ np.array(
[xp, yp, zp, 1])
self.initial_estimate.insert(L(i), Xg[:3])
key_point_initialized = True
print('==> Using ', count, ' tracks')
def main():
"""Main runner"""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = X(1)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
# Print graph
print("Factor Graph:\n{}".format(graph))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Print
print("Initial Estimate:\n{}".format(initial_estimate))
# Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
(L2, "L2")]:
print("{} covariance:\n{}\n".format(s,
marginals.marginalCovariance(key)))
def optimize(self, flag_verbose=False):
# optimize
edge_outlier = np.full(self.counter - 1, False)
error_th_stereo = [7.815, 7.815, 5, 5]
error_th_mono = [5.991, 5.991, 3.5, 3.5]
# error_th_stereo = [7.815, 7.815, 7.815, 7.815]
# error_th_mono = [5.991, 5.991, 5.991, 5.991]
for iteration in range(4):
if flag_verbose:
errors = []
optimizer = gt.LevenbergMarquardtOptimizer(self.graph,
self.initialEstimate)
result = optimizer.optimize()
n_bad = 0
if flag_verbose:
print(
f"Number of Factors: {self.graph.nrFactors()-self.graph.size()//2, self.graph.size()//2}"
)
error_s = error_th_stereo[iteration]
error_m = error_th_mono[iteration]
for idx in range(1, self.graph.size(), 2):
try:
if self.is_stereo[idx]:
factor = gt.dynamic_cast_GenericStereoFactor3D_NonlinearFactor(
self.graph.at(idx))
else:
factor = gt.dynamic_cast_GenericProjectionFactorCal3_S2_NonlinearFactor(
self.graph.at(idx))
except:
if flag_verbose:
errors.append(0)
continue
error = factor.error(result)
# print(error)
if flag_verbose:
errors.append(error)
# if error > 7.815:
if (self.is_stereo[idx]
and error > error_s) or (not self.is_stereo[idx]
and error > error_m):
edge_outlier[idx // 2] = True
self.graph.remove(idx)
n_bad += 1
else:
edge_outlier[idx // 2] = False
if iteration == 2:
if self.is_stereo[idx]:
information = self.inv_lvl_sigma2[self.octave[
idx // 2]] * np.identity(3)
stereo_model = gt.noiseModel_Diagonal.Information(
information)
new_factor = gt.GenericStereoFactor3D(
factor.measured(), stereo_model, X(1),
L(idx // 2 + 1), self.K_stereo)
else:
information = self.inv_lvl_sigma2[self.octave[
idx // 2]] * np.identity(2)
stereo_model = gt.noiseModel_Diagonal.Information(
information)
new_factor = gt.GenericProjectionFactorCal3_S2(
factor.measured(), stereo_model, X(1),
L(idx // 2 + 1), self.K_mono)
self.graph.replace(idx, new_factor)
if flag_verbose:
fig, ax = plt.subplots()
ax.bar(np.arange(0, len(errors)).tolist(), errors)
plt.show()
print("NUM BADS: ", n_bad)
pose = result.atPose3(X(1))
# marginals = gt.Marginals(self.graph, result)
# cov = marginals.marginalCovariance(gt.X(1))
return pose, edge_outlier # self.graph, result
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()