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 __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 __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 __init__(self,
class_probability_prior,
geo_model,
da_model,
cls_model,
prior_mean,
prior_noise,
cls_enable=True):
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
# Camera pose prior
self.graph = gtsam.NonlinearFactorGraph()
self.graph.add(
gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise))
# Class realization and prior probabilities. if one of the class probabilities is 0 it will
# set the logWeight to -inf
self.cls_enable = cls_enable
# 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()
# self.initial.print()
# Setting up models
self.geoModel = geo_model
self.daModel = da_model
self.clsModel = cls_model
# self.classifierModel = classifierModel
# Setting up ISAM2 TODO: make ISAM2 work. As of now, it doesn't take the initial values at optimize_isam2 method
params2 = gtsam.ISAM2Params()
# params2.relinearize_threshold = 0.01
# params2.relinearize_skip = 1
self.isam = gtsam.ISAM2(params2)
self.isam.update(self.graph, self.initial)
# Setting up weight memory
self.weight_memory = list()
self.normalized_weight = 0
# Setting up class realization
self.classRealization = dict()
self.classProbabilityPrior = class_probability_prior
def __init__(self,
pose0=np.eye(4),
pose0_to_pose1_range=1.0,
K=np.eye(3),
relinearizeThreshold=0.1,
relinearizeSkip=10,
proj_noise_val=1.0):
self.graph = NonlinearFactorGraph()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
x0factor = PriorFactorPose3(iSAM2Wrapper.get_key('x', 0),
gtsam.gtsam.Pose3(pose0), pose_noise)
self.graph.push_back(x0factor)
# Set scale between pose 0 and pose 1 to Unity
x0x1_noise = gtsam.noiseModel_Isotropic.Sigma(1, 0.1)
x1factor = RangeFactorPose3(iSAM2Wrapper.get_key('x', 0),
iSAM2Wrapper.get_key('x', 1),
pose0_to_pose1_range, x0x1_noise)
self.graph.push_back(x1factor)
iS2params = gtsam.ISAM2Params()
iS2params.setRelinearizeThreshold(relinearizeThreshold)
iS2params.setRelinearizeSkip(relinearizeSkip)
self.isam2 = gtsam.ISAM2(iS2params)
self.projection_noise = gtsam.noiseModel_Isotropic.Sigma(
2, proj_noise_val)
self.K = gtsam.Cal3_S2(iSAM2Wrapper.CameraMatrix_to_Cal3_S2(K))
#self.opt_params = gtsam.DoglegParams()
#self.opt_params.setVerbosity('Error')
#self.opt_params.setErrorTol(0.1)
self.initial_estimate = gtsam.Values()
self.initial_estimate.insert(iSAM2Wrapper.get_key('x', 0),
gtsam.gtsam.Pose3(pose0))
self.lm_factor_ids = set()
def __init__(self, config, pose_3rd):
quat = R_scipy.from_matrix(pose_3rd[:3, :3]).as_quat()
quat = np.roll(quat, 1)
t = pose_3rd[:3, -1]
init_pose = np.array([*t, *quat])
self.result = None
self.marginals = None
self.node_idx = 0
self.odom_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["odom_noise"])
self.skip_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["skip_noise"])
self.prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["prior_noise"])
self.gps_pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["gps_pose_noise"])
self.gps_noise = gtsam.noiseModel.Isotropic.Sigmas(config["gps_noise"])
self.skip_num = config["skip_num"]
# self.visualize=config["visualize"]
self.graph = gtsam.NonlinearFactorGraph()
self.initial_estimate = gtsam.Values()
self.init_pose = gen_pose(
init_pose
) #gtsam.Pose3(gtsam.Rot3.Rodrigues(*init_pose[3:]), gtsam.Point3(*init_pose[:3]))#gen_pose([0,0,0])
self.parameters = gtsam.ISAM2Params()
self.parameters.setRelinearizeThreshold(0.01)
self.parameters.setRelinearizeSkip(10)
self.isam = gtsam.ISAM2(self.parameters)
#! Add a prior on pose x0
self.graph.push_back(
gtsam.PriorFactorPose3(X(self.node_idx), self.init_pose,
self.prior_noise))
self.initial_estimate.insert(X(self.node_idx), self.init_pose)
self.current_estimate = None
self.last_transform = None
def clone(self):
"""
:type new_object: GaussianBelief
"""
new_object = copy.copy(self)
new_object.graph = self.graph.clone()
new_object.daRealization = self.daRealization.copy()
new_object.classRealization = self.classRealization.copy()
params3 = gtsam.ISAM2Params()
#params3.relinearize_threshold = 0.01
#params3.relinearize_skip = 1
new_object.isam = gtsam.ISAM2(params3)
new_object.isam.update(self.graph, self.initial)
new_object.initial = gtsam.Values()
for key in self.initial.keys():
try:
new_object.initial.insert(key, self.initial.atPose3(key))
except:
new_object.initial.insert(key, self.initial.atVector(key))
return new_object
def incremental_factorgraph_example():
# Create an ISAM2 object.
# You can balance computation time vs accuracy by changing the parameters to ISAM2 in the ISAM2Params struct
# (see https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/ISAM2Params.h).
# The parameters are here set to default.
isam_params = gtsam.ISAM2Params()
isam = gtsam.ISAM2(isam_params)
# We will keep the variables in lists for simplicity.
pose_variables = []
landmark_variables = []
# Step 1: Initialize with a prior on the first pose X1.
def step1_initialize():
# Create an empty nonlinear factor graph.
# We will need to do this for every update.
graph = gtsam.NonlinearFactorGraph()
# Create a key for the first pose.
X1 = X(1)
# Update the list with the new pose variable key.
pose_variables.append(X1)
# 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))
# Set an initial estimate for the first pose.
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
# Update ISAM2 with the initial factor graph.
isam.update(graph, initial_estimate)
step1_initialize()
# Compute the current MAP estimate for all variables.
result = isam.calculateEstimate()
# Define a function that extracts the marginals (which we will reuse below).
# Notice that we compute the marginal covariance directly from the isam object.
def extract_marginals():
# Extract the marginal distributions for each variable
X_marginals = []
for var in pose_variables:
X_marginals.append(
MultivariateNormalParameters(result.atPose2(var),
isam.marginalCovariance(var)))
L_marginals = []
for var in landmark_variables:
L_marginals.append(
MultivariateNormalParameters(result.atPoint2(var),
isam.marginalCovariance(var)))
return X_marginals, L_marginals
pose_marginals, landmark_marginals = extract_marginals()
# Show current results.
plot_result(pose_marginals, landmark_marginals)
# Step 2: Add a landmark observation from the pose X1
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)
step2_add_landmark_observation()
# ISAM2 performs only one iteration of the iterative nonlinear solver per update() call.
# We can use "free time" until the next update from the sensors to perform additional iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
# Step 3: Add a new pose with a relative odometry constraint.
def step3_add_pose_from_odometry():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X1 = X(1)
X2 = X(2)
# Update the list with the new pose variable key.
pose_variables.append(X2)
# Add an odometry measurement.
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))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
step3_add_pose_from_odometry()
# Run a few additional update iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
# Step 4: Add new observation of the landmark L1 from X2.
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())
step4_add_new_landmark_observation()
# Run a few additional update iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
# Step 5: Add both new odometry and landmark observations for X3.
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)
step5_add_odometry_and_landmark_observations()
# Run a few additional update iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
def optimize(gps_measurements: List[GpsMeasurement],
imu_measurements: List[ImuMeasurement],
sigma_init_x: gtsam.noiseModel.Diagonal,
sigma_init_v: gtsam.noiseModel.Diagonal,
sigma_init_b: gtsam.noiseModel.Diagonal,
noise_model_gps: gtsam.noiseModel.Diagonal,
kitti_calibration: KittiCalibration, first_gps_pose: int,
gps_skip: int) -> gtsam.ISAM2:
"""Run ISAM2 optimization on the measurements."""
# Set initial conditions for the estimated trajectory
# initial pose is the reference frame (navigation frame)
current_pose_global = Pose3(gtsam.Rot3(),
gps_measurements[first_gps_pose].position)
# the vehicle is stationary at the beginning at position 0,0,0
current_velocity_global = np.zeros(3)
current_bias = gtsam.imuBias.ConstantBias() # init with zero bias
imu_params = getImuParams(kitti_calibration)
# Set ISAM2 parameters and create ISAM2 solver object
isam_params = gtsam.ISAM2Params()
isam_params.setFactorization("CHOLESKY")
isam_params.relinearizeSkip = 10
isam = gtsam.ISAM2(isam_params)
# Create the factor graph and values object that will store new factors and
# values to add to the incremental graph
new_factors = gtsam.NonlinearFactorGraph()
# values storing the initial estimates of new nodes in the factor graph
new_values = gtsam.Values()
# Main loop:
# (1) we read the measurements
# (2) we create the corresponding factors in the graph
# (3) we solve the graph to obtain and optimal estimate of robot trajectory
print("-- Starting main loop: inference is performed at each time step, "
"but we plot trajectory every 10 steps")
j = 0
included_imu_measurement_count = 0
for i in range(first_gps_pose, len(gps_measurements)):
# At each non=IMU measurement we initialize a new node in the graph
current_pose_key = X(i)
current_vel_key = V(i)
current_bias_key = B(i)
t = gps_measurements[i].time
if i == first_gps_pose:
# Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global)
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
new_factors.addPriorPose3(current_pose_key, current_pose_global,
sigma_init_x)
new_factors.addPriorVector(current_vel_key,
current_velocity_global, sigma_init_v)
new_factors.addPriorConstantBias(current_bias_key, current_bias,
sigma_init_b)
else:
t_previous = gps_measurements[i - 1].time
# Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = gtsam.PreintegratedImuMeasurements(
imu_params, current_bias)
while (j < len(imu_measurements)
and imu_measurements[j].time <= t):
if imu_measurements[j].time >= t_previous:
current_summarized_measurement.integrateMeasurement(
imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope, imu_measurements[j].dt)
included_imu_measurement_count += 1
j += 1
# Create IMU factor
previous_pose_key = X(i - 1)
previous_vel_key = V(i - 1)
previous_bias_key = B(i - 1)
new_factors.push_back(
gtsam.ImuFactor(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key,
current_summarized_measurement))
# Bias evolution as given in the IMU metadata
sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([
np.sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma
] * 3 + [
np.sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma
] * 3))
new_factors.push_back(
gtsam.BetweenFactorConstantBias(previous_bias_key,
current_bias_key,
gtsam.imuBias.ConstantBias(),
sigma_between_b))
# Create GPS factor
gps_pose = Pose3(current_pose_global.rotation(),
gps_measurements[i].position)
if (i % gps_skip) == 0:
new_factors.addPriorPose3(current_pose_key, gps_pose,
noise_model_gps)
new_values.insert(current_pose_key, gps_pose)
print(f"############ POSE INCLUDED AT TIME {t} ############")
print(gps_pose.translation(), "\n")
else:
new_values.insert(current_pose_key, current_pose_global)
# Add initial values for velocity and bias based on the previous
# estimates
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
# Update solver
# =======================================================================
# We accumulate 2*GPSskip GPS measurements before updating the solver at
# first so that the heading becomes observable.
if i > (first_gps_pose + 2 * gps_skip):
print(f"############ NEW FACTORS AT TIME {t:.6f} ############")
new_factors.print()
isam.update(new_factors, new_values)
# Reset the newFactors and newValues list
new_factors.resize(0)
new_values.clear()
# Extract the result/current estimates
result = isam.calculateEstimate()
current_pose_global = result.atPose3(current_pose_key)
current_velocity_global = result.atVector(current_vel_key)
current_bias = result.atConstantBias(current_bias_key)
print(f"############ POSE AT TIME {t} ############")
current_pose_global.print()
print("\n")
return isam
def Pose2SLAM_ISAM2_example():
"""Perform 2D SLAM given the ground truth changes in pose as well as
simple loop closure detection."""
plt.ion()
# Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters.
prior_xy_sigma = 0.3
# Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees.
prior_theta_sigma = 5
# Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters.
odometry_xy_sigma = 0.2
# Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees.
odometry_theta_sigma = 5
# Although this example only uses linear measurements and Gaussian noise models, it is important
# to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
# simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma,
prior_xy_sigma,
prior_theta_sigma*np.pi/180]))
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma,
odometry_xy_sigma,
odometry_theta_sigma*np.pi/180]))
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
# update calls are required to perform the relinearization.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.1)
parameters.relinearizeSkip = 1
isam = gtsam.ISAM2(parameters)
# Create the ground truth odometry measurements of the robot during the trajectory.
true_odometry = [(2, 0, 0),
(2, 0, math.pi/2),
(2, 0, math.pi/2),
(2, 0, math.pi/2),
(2, 0, math.pi/2)]
# Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements.
odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance())
for true_odom in true_odometry]
# Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
# iSAM2 incremental optimization.
graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
# Initialize the current estimate which is used during the incremental inference loop.
current_estimate = initial_estimate
for i in range(len(true_odometry)):
# Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise.
noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i]
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25)
# Add a binary factor in between two existing states if loop closure is detected.
# Otherwise, add a binary factor between a newly observed state and the previous state.
if loop:
graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop,
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
else:
graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2,
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
# Compute and insert the initialization estimate for the current pose using the noisy odometry measurement.
computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x,
noisy_odom_y,
noisy_odom_theta))
initial_estimate.insert(i + 2, computed_estimate)
# Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
isam.update(graph, initial_estimate)
current_estimate = isam.calculateEstimate()
# Report all current state estimates from the iSAM2 optimzation.
report_on_progress(graph, current_estimate, i)
initial_estimate.clear()
# Print the final covariance matrix for each pose after completing inference on the trajectory.
marginals = gtsam.Marginals(graph, current_estimate)
i = 1
for i in range(1, len(true_odometry)+1):
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
plt.ioff()
plt.show()
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.3, 0.3, 0.3, 0.1, 0.1,
0.1])) # 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.x() - 0.25,
point.y() + 0.20,
point.z() + 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()
def initialize(data, truth, options):
# Initialize iSAM
params = gtsam.ISAM2Params()
if options.alwaysRelinearize:
params.relinearizeSkip = 1
isam = gtsam.ISAM2(params=params)
# Add constraints/priors
# TODO: should not be from ground truth!
newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = gtsam.Values()
for i in range(2):
ii = symbol('x', i)
if i == 0:
if options.hardConstraint: # add hard constraint
newFactors.add(
gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
else:
newFactors.add(
gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
data.noiseModels.posePrior))
initialEstimates.insert(ii, truth.cameras[i].pose())
nextPoseIndex = 2
# Add visual measurement factors from two first poses and initialize
# observed landmarks
for i in range(2):
ii = symbol('x', i)
for k in range(len(data.Z[i])):
j = data.J[i][k]
jj = symbol('l', j)
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], data.noiseModels.measurement, ii, jj,
data.K))
# TODO: initial estimates should not be from ground truth!
if not initialEstimates.exists(jj):
initialEstimates.insert(jj, truth.points[j])
if options.pointPriors: # add point priors
newFactors.add(
gtsam.PriorFactorPoint3(jj, truth.points[j],
data.noiseModels.pointPrior))
# Add odometry between frames 0 and 1
newFactors.add(
gtsam.BetweenFactorPose3(symbol('x', 0), symbol('x', 1),
data.odometry[1], data.noiseModels.odometry))
# Update ISAM
if options.batchInitialization: # Do a full optimize for first two poses
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
newFactors, initialEstimates)
fullyOptimized = batchOptimizer.optimize()
isam.update(newFactors, fullyOptimized)
else:
isam.update(newFactors, initialEstimates)
# figure(1)tic
# t=toc plot(frame_i,t,'r.') tic
result = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')
return isam, result, nextPoseIndex
def __init__(self):
self.oculus = OculusProperty()
# Create a new factor when
# - |ti - tj| > min_duration and
# - |xi - xj| > max_translation or
# - |ri - rj| > max_rotation
self.keyframe_duration = None
self.keyframe_translation = None
self.keyframe_rotation = None
# List of keyframes
self.keyframes = []
# Current (non-key)frame with real-time pose update
# FIXME propagate cov from previous keyframe
self.current_frame = None
self.isam_params = gtsam.ISAM2Params()
self.graph = gtsam.NonlinearFactorGraph()
self.values = gtsam.Values()
# [x, y, theta]
self.prior_sigmas = None
# Noise model without ICP
# [x, y, theta]
self.odom_sigmas = None
# Downsample point cloud for ICP and publishing
self.point_resolution = 0.5
# Noise radius in overlap
self.point_noise = 0.5
self.ssm_params = SMParams()
self.ssm_params.initialization = True
self.ssm_params.initialization_params = 50, 1, 0.01
self.ssm_params.min_st_sep = 1
self.ssm_params.min_points = 50
self.ssm_params.max_translation = 2.0
self.ssm_params.max_rotation = np.pi / 6
self.ssm_params.target_frames = 3
# Don't use ICP covariance
self.ssm_params.cov_samples = 0
self.nssm_params = SMParams()
self.nssm_params.initialization = True
self.nssm_params.initialization_params = 100, 5, 0.01
self.nssm_params.min_st_sep = 10
self.nssm_params.min_points = 100
self.nssm_params.max_translation = 6.0
self.nssm_params.max_rotation = np.pi / 2
self.nssm_params.source_frames = 5
self.nssm_params.cov_samples = 30
self.icp = pcl.ICP()
# Pairwise consistent measurement
self.nssm_queue = []
self.pcm_queue_size = 5
self.min_pcm = 3
# Use fixed noise model in two cases
# - Sequential scan matching
# - ICP cov is too small in non-sequential scan matching
# [x, y, theta]
self.icp_odom_sigmas = None
# FIXME Can't save fig in online mode
self.save_fig = False
self.save_data = False
for elem in xVals:
val = elem.split(',')
x[val[0]] = val[1]
elif data[i][0:2] == 'l0':
i+=1
lString = data[i].split('[')[1].split(']')[0]
lVals = lString[1:-1].split('), (')
for elem in lVals:
val = elem.split(',')
l[val[0]] = val[1]
parameters = gtsam.ISAM2Params()
parameters.relinearize_threshold = 0.01
parameters.relinearize_skip = 1
isam = gtsam.ISAM2(parameters)
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.000, 0.0, 0.0]))
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0.000, 0.0, 0.0), priorNoise))
initialEstimate.insert(1, gtsam.Pose2(0.0, 0.0, 0.0))
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.000, 0.0, 0.0]))
graph.add(gtsam.PriorFactorPose2(5, gtsam.Pose2(2.034, 0.0, 0.0), priorNoise))
initialEstimate.insert(5, gtsam.Pose2(2.034, 0.0, 0.0))
def __init__(self, params):
""" Initialize variables """
self.imu_meas_predict = [] # IMU measurements for pose prediction
self.imu_opt_meas = [] # IMU measurements for pose prediction between measurement updates
self.imu_samples = [] # imu samples
self.opt_meas = [] # optimizes measurements
self.__opt_meas_buffer_time = params['opt_meas_buffer_time']
""" IMU Preintegration """
# IMU preintegration parameters
self.imu_preint_params = gtsam.PreintegrationParams(np.asarray(params['g']))
self.imu_preint_params.setAccelerometerCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2))
self.imu_preint_params.setGyroscopeCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2))
self.imu_preint_params.setIntegrationCovariance(np.eye(3) * params['int_cov_sigma']**2)
self.imu_preint_params.setUse2ndOrderCoriolis(params['setUse2ndOrderCoriolis'])
self.imu_preint_params.setOmegaCoriolis(np.array(params['omega_coriolis']))
""" Initialize Parameters """
# ISAM2 initialization
isam2_params = gtsam.ISAM2Params()
isam2_params.setRelinearizeThreshold(params['relinearize_th'])
isam2_params.setRelinearizeSkip(params['relinearize_skip'])
isam2_params.setFactorization(params['factorization'])
# self.isam2 = gtsam.ISAM2(isam2_params)
self.isam2 = gtsam.ISAM2()
self.new_factors = gtsam.NonlinearFactorGraph()
self.new_initial_ests = gtsam.Values()
self.min_imu_sample = 2 # minimum imu sample count needed for integration
# ISAM2 keys
self.poseKey = gtsam.symbol(ord('x'), 0)
self.velKey = gtsam.symbol(ord('v'), 0)
self.biasKey = gtsam.symbol(ord('b'), 0)
""" Set initial variables """
# Initial state
self.last_opt_time = self.current_time = 0
self.current_global_pose = numpy_pose_to_gtsam(
params['init_pos'],
params['init_ori'])
self.current_global_vel = np.asarray(
params['init_vel'])
self.current_bias = gtsam.imuBias_ConstantBias(
np.asarray(params['init_acc_bias']),
np.asarray(params['init_gyr_bias']))
self.imu_accum = gtsam.PreintegratedImuMeasurements(self.imu_preint_params)
# uncertainty of the initial state
self.init_pos_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_ori_cov'], params['init_ori_cov'], params['init_ori_cov'],
params['init_pos_cov'], params['init_pos_cov'], params['init_pos_cov']]))
self.init_vel_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_vel_cov'], params['init_vel_cov'], params['init_vel_cov']]))
self.init_bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_acc_bias_cov'],
params['init_gyr_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov']]))
# measurement noise
self.dvl_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray(
params['dvl_cov']))
self.bar_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray(
params['bar_cov']))
self.bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.concatenate((
params['sigma_acc_bias_evol'],
params['sigma_gyr_bias_evol'])))
""" Set initial state """
# Initial factors
prior_pose_factor = gtsam.PriorFactorPose3(
self.poseKey,
self.current_global_pose,
self.init_pos_cov)
self.new_factors.add(prior_pose_factor)
prior_vel_factor = gtsam.PriorFactorVector(
self.velKey,
self.current_global_vel,
self.init_vel_cov)
self.new_factors.add(prior_vel_factor)
prior_bias_factor = gtsam.PriorFactorConstantBias(
self.biasKey,
self.current_bias,
self.init_bias_cov)
self.new_factors.add(prior_bias_factor)
# Initial estimates
self.new_initial_ests.insert(self.poseKey, self.current_global_pose)
self.new_initial_ests.insert(self.velKey, self.current_global_vel)
self.new_initial_ests.insert(self.biasKey, self.current_bias)
