def __reprojection_factors(
self, initial_data: GtsfmData,
is_fisheye_calibration: bool) -> NonlinearFactorGraph:
"""Generate reprojection factors using the tracks."""
graph = NonlinearFactorGraph()
# noise model for measurements -- one pixel in u and v
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
IMG_MEASUREMENT_DIM, MEASUREMENT_NOISE_SIGMA)
if self._robust_measurement_noise:
measurement_noise = gtsam.noiseModel.Robust(
gtsam.noiseModel.mEstimator.Huber(1.345), measurement_noise)
sfm_factor_class = GeneralSFMFactor2Cal3Fisheye if is_fisheye_calibration else GeneralSFMFactor2Cal3Bundler
for j in range(initial_data.number_tracks()):
track = initial_data.get_track(j) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.numberMeasurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.push_back(
sfm_factor_class(
uv,
measurement_noise,
X(i),
P(j),
K(self.__map_to_calibration_variable(i)),
))
return graph
def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
"""Add a prior on the navigation state at time `i`."""
state = self.scenario.navState(i)
graph.push_back(
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
graph.push_back(
gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
def _between_factors(self, relative_pose_priors: Dict[Tuple[int, int],
PosePrior],
cameras_to_model: List[int]) -> NonlinearFactorGraph:
"""Add between factors on poses between cameras and IMUs.
1. For the same timestamp, add a prior factor between each camera and cam2.
2. For different timestamps, the between factors are between cam2s only.
"""
graph = NonlinearFactorGraph()
# translate the relative pose priors between cams to IMUs, and add if not already present
between_factors: Dict[Tuple[int, int], BetweenFactorPose3] = {}
for (i1, i2), i2Ti1_prior in relative_pose_priors.items():
if i1 not in cameras_to_model or i2 not in cameras_to_model:
continue
b1: int = self.__get_rig_idx(i1)
b2: int = self.__get_rig_idx(i2)
cam_type_i1: int = self.__get_camera_type(i1)
cam_type_i2: int = self.__get_camera_type(i2)
if (b1 == b2 and (cam_type_i1 == UPWARD_FACING_CAM_TYPE
or cam_type_i2 == UPWARD_FACING_CAM_TYPE)) or (
cam_type_i1 == UPWARD_FACING_CAM_TYPE
and cam_type_i2 == UPWARD_FACING_CAM_TYPE):
between_factors[(i1, i2)] = BetweenFactorPose3(
X(i2), X(i1), i2Ti1_prior.value,
gtsam.noiseModel.Diagonal.Sigmas(i2Ti1_prior.covariance))
logger.info("Added %d between factors for BA", len(between_factors))
for factor in between_factors.values():
graph.push_back(factor)
return graph
def test_optimize(self):
"""Do trivial test with three optimizer variants."""
fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
optimal_values = Values()
optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, fg.error(optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
initial_values = Values()
initial_values.insert(KEY1, x0)
self.assertEqual(9.0, fg.error(initial_values), 1e-3)
# optimize parameters
ordering = Ordering()
ordering.push_back(KEY1)
# Gauss-Newton
gnParams = GaussNewtonParams()
gnParams.setOrdering(ordering)
actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
self.assertAlmostEqual(0, fg.error(actual1))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(ordering)
actual2 = LevenbergMarquardtOptimizer(fg, initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual2 = LevenbergMarquardtOptimizer(fg, initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)
actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
self.assertAlmostEqual(0, fg.error(actual3))
def create_graph(microphones, simulatedTOA):
"""Create factor graph."""
graph = NonlinearFactorGraph()
# Create a noise model for the TOA error
model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
K = len(microphones)
key = 0
for toa in simulatedTOA:
for i in range(K):
factor = TOAFactor(key, microphones[i], toa[i], model)
graph.push_back(factor)
key += 1
return graph
def _between_factors(self, relative_pose_priors: Dict[Tuple[int, int],
PosePrior],
cameras_to_model: List[int]) -> NonlinearFactorGraph:
"""Generate BetweenFactors on relative poses for pose variables."""
graph = NonlinearFactorGraph()
for (i1, i2), i2Ti1_prior in relative_pose_priors.items():
if i1 not in cameras_to_model or i2 not in cameras_to_model:
continue
graph.push_back(
BetweenFactorPose3(
X(i1),
X(i2),
i2Ti1_prior.value.inverse(),
gtsam.noiseModel.Diagonal.Sigmas(i2Ti1_prior.covariance),
))
return graph
def __calibration_priors(
self, initial_data: GtsfmData, cameras_to_model: List[int],
is_fisheye_calibration: bool) -> NonlinearFactorGraph:
"""Generate prior factors on calibration parameters of the cameras."""
graph = NonlinearFactorGraph()
calibration_prior_factor_class = PriorFactorCal3Fisheye if is_fisheye_calibration else PriorFactorCal3Bundler
calibration_prior_factor_dof = CAM_CAL3FISHEYE_DOF if is_fisheye_calibration else CAM_CAL3BUNDLER_DOF
calibration_prior_noise_sigma = (CAM_CAL3FISHEYE_PRIOR_NOISE_SIGMA
if is_fisheye_calibration else
CAM_CAL3BUNDLER_PRIOR_NOISE_SIGMA)
if self._shared_calib:
graph.push_back(
calibration_prior_factor_class(
K(self.__map_to_calibration_variable(cameras_to_model[0])),
initial_data.get_camera(cameras_to_model[0]).calibration(),
gtsam.noiseModel.Isotropic.Sigma(
calibration_prior_factor_dof,
calibration_prior_noise_sigma),
))
else:
for i in cameras_to_model:
graph.push_back(
calibration_prior_factor_class(
K(self.__map_to_calibration_variable(i)),
initial_data.get_camera(i).calibration(),
gtsam.noiseModel.Isotropic.Sigma(
calibration_prior_factor_dof,
calibration_prior_noise_sigma),
))
return graph
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
self.fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
self.optimal_values = Values()
self.optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
self.initial_values = Values()
self.initial_values.insert(KEY1, x0)
self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
# optimize parameters
self.ordering = Ordering()
self.ordering.push_back(KEY1)
def __pose_priors(
self,
absolute_pose_priors: List[Optional[PosePrior]],
initial_data: GtsfmData,
camera_for_origin: gtsfm_types.CAMERA_TYPE,
) -> NonlinearFactorGraph:
"""Generate prior factors (in the world frame) on pose variables."""
graph = NonlinearFactorGraph()
# TODO(Ayush): start using absolute prior factors.
num_priors_added = 0
if num_priors_added == 0:
# Adding a prior to fix origin as no absolute prior exists.
graph.push_back(
PriorFactorPose3(
X(camera_for_origin),
initial_data.get_camera(camera_for_origin).pose(),
gtsam.noiseModel.Isotropic.Sigma(
CAM_POSE3_DOF, CAM_POSE3_PRIOR_NOISE_SIGMA),
))
return graph
def setUp(self):
model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
# We consider a small graph:
# symbolic FG
# x2 0 1
# / | \ 1 2
# / | \ 2 3
# x3 | x1 2 0
# \ | / 0 3
# \ | /
# x0
#
p0 = Point3(0, 0, 0)
self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
p1 = Point3(1, 2, 0)
self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
p2 = Point3(0, 2, 0)
self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
p3 = Point3(-1, 1, 0)
self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
pose0 = Pose3(self.R0, p0)
pose1 = Pose3(self.R1, p1)
pose2 = Pose3(self.R2, p2)
pose3 = Pose3(self.R3, p3)
g = NonlinearFactorGraph()
g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
g.add(gtsam.PriorFactorPose3(x0, pose0, model))
self.graph = g
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
Each variable in the system (poses and landmarks) must be identified with a unique key.
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
Here we will use Symbols
In GTSAM, measurement functions are represented as 'factors'. Several common factors
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
Here we will use Projection factors to model the camera's landmark observations.
Also, we will initialize the robot at some location using a Prior factor.
When the factors are created, we will add them to a Factor Graph. As the factors we are using
are nonlinear factors, we will need a Nonlinear Factor Graph.
Finally, once all of the factors have been added to our factor graph, we will want to
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
to update the linearization point. This happens repeatedly until the solver converges
to a consistent set of variable values. This requires us to specify an initial guess
for each variable, held in a Values container.
"""
# Define the camera calibration parameters
K = 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 a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. 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]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1 * np.random.randn(6, 1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1 * np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
class TestScenario(GtsamTestCase):
"""Do trivial test with three optimizer variants."""
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
self.fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
self.optimal_values = Values()
self.optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
self.initial_values = Values()
self.initial_values.insert(KEY1, x0)
self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
# optimize parameters
self.ordering = Ordering()
self.ordering.push_back(KEY1)
def test_gauss_newton(self):
gnParams = GaussNewtonParams()
gnParams.setOrdering(self.ordering)
actual = GaussNewtonOptimizer(self.fg, self.initial_values,
gnParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_levenberg_marquardt(self):
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(self.ordering)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_levenberg_marquardt_pcg(self):
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_dogleg(self):
dlParams = DoglegParams()
dlParams.setOrdering(self.ordering)
actual = DoglegOptimizer(self.fg, self.initial_values,
dlParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_graduated_non_convexity(self):
gncParams = GncLMParams()
actual = GncLMOptimizer(self.fg, self.initial_values,
gncParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_iteration_hook(self):
# set up iteration hook to track some testable values
iteration_count = 0
final_error = 0
final_values = None
def iteration_hook(iter, error_before, error_after):
nonlocal iteration_count, final_error, final_values
iteration_count = iter
final_error = error_after
final_values = optimizer.values()
# optimize
params = LevenbergMarquardtParams.CeresDefaults()
params.setOrdering(self.ordering)
params.iterationHook = iteration_hook
optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
params)
actual = optimizer.optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
self.gtsamAssertEquals(final_values, actual)
self.assertEqual(self.fg.error(actual), final_error)
self.assertEqual(optimizer.iterations(), iteration_count)
def __construct_factor_graph(
self,
cameras_to_model: List[int],
initial_data: GtsfmData,
absolute_pose_priors: List[Optional[PosePrior]],
relative_pose_priors: Dict[Tuple[int, int], PosePrior],
) -> NonlinearFactorGraph:
"""Construct the factor graph with reprojection factors, BetweenFactors, and prior factors."""
is_fisheye_calibration = isinstance(
initial_data.get_camera(cameras_to_model[0]),
PinholeCameraCal3Fisheye)
graph = NonlinearFactorGraph()
# Create a factor graph
graph.push_back(
self.__reprojection_factors(
initial_data=initial_data,
is_fisheye_calibration=is_fisheye_calibration))
graph.push_back(
self._between_factors(relative_pose_priors=relative_pose_priors,
cameras_to_model=cameras_to_model))
graph.push_back(
self.__pose_priors(
absolute_pose_priors=absolute_pose_priors,
initial_data=initial_data,
camera_for_origin=cameras_to_model[0],
))
graph.push_back(
self.__calibration_priors(initial_data, cameras_to_model,
is_fisheye_calibration))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
gtsam.PriorFactorPoint3(
P(0),
initial_data.get_track(0).point3(),
gtsam.noiseModel.Isotropic.Sigma(POINT3_DOF, 0.1)))
return graph
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()
l = np.append(points[i*2], points[i*2 + 1])
lines_array.append(l)
return [points, lines, semantics]
K = Cal3_S2(320., 320., 0.0, 320., 240.)
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3])) # RPY - XYZ
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
pl = createPointsLines()
poses = createPoses(K)
points = pl[0]
sems = pl[2]
# Create a factor graph
graph = NonlinearFactorGraph()
#Add Pose Prior
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)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
# print(measurement)
# factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)
factor = gtsam.CustomFactor(measurement_noise, [X(i),L(j)], partial(error_point_landmarks,measurement, K, sems))
graph.push_back(factor)
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
camera = get_camera(radius)
pose_0 = camera.pose()
delta_t = 1.0 / 18 # makes for 10 degrees per step
angular_velocity = math.radians(180) # rad/sec
scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
# Create a factor graph
graph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = Values()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = B(0)
biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
biasnoise)
graph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
graph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t # simulation time
if i == 0: # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))
elif i >= 2: # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))
if i > 0:
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(),
BIAS_COVARIANCE)
graph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
graph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(graph, initialEstimate)
result = isam.calculateEstimate()
plot.plot_incremental_trajectory(0,
result,
start=i,
scale=3,
time_interval=0.01)
# reset
graph = NonlinearFactorGraph()
initialEstimate.clear()
plt.show()
