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 test_StereoVOExample(self):
## Assumptions
# - For simplicity this example is in the camera's coordinate frame
# - X: right, Y: down, Z: forward
# - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
# - x1 is fixed with a constraint, x2 is initialized with noisy values
# - No noise on measurements
## Create keys for variables
x1 = symbol('x', 1)
x2 = symbol('x', 2)
l1 = symbol('l', 1)
l2 = symbol('l', 2)
l3 = symbol('l', 3)
## Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
## add a constraint on the starting pose
first_pose = gtsam.Pose3()
graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
## Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline
K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
stereo_model = gtsam.noiseModel.Diagonal.Sigmas(
np.array([1.0, 1.0, 1.0]))
## Add measurements
# pose 1
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440),
stereo_model, x1, l1, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440),
stereo_model, x1, l2, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140),
stereo_model, x1, l3, K))
#pose 2
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490),
stereo_model, x2, l1, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(70, 20, 490),
stereo_model, x2, l2, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115),
stereo_model, x2, l3, K))
## Create initial estimate for camera poses and landmarks
initialEstimate = gtsam.Values()
initialEstimate.insert(x1, first_pose)
# noisy estimate for pose 2
initialEstimate.insert(
x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, -.1, 1.1)))
expected_l1 = gtsam.Point3(1, 1, 5)
initialEstimate.insert(l1, expected_l1)
initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
initialEstimate.insert(l3, gtsam.Point3(0, -.5, 5))
## optimize
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
## check equality for the first pose and point
pose_x1 = result.atPose3(x1)
self.gtsamAssertEquals(pose_x1, first_pose, 1e-4)
point_l1 = result.atPoint3(l1)
self.gtsamAssertEquals(point_l1, expected_l1, 1e-4)
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