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_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 bundle_adjustment(self):
"""
Parameters:
calibration - gtsam.Cal3_S2, camera calibration
landmark_map - list, A map of landmarks and their correspondence
"""
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initial_estimate = self.create_initial_estimate()
# Create Projection Factors
# """
# Measurement noise for bundle adjustment:
# sigma = 1.0
# measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# """
for landmark_idx, observation_list in enumerate(self._landmark_map):
for obersvation in observation_list:
pose_idx = obersvation[0]
key_point = obersvation[1]
# To test indeterminate system
# if(landmark_idx == 2 or landmark_idx == 480):
# continue
# if(pose_idx == 6):
# print("yes2")
# continue
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
key_point, self._measurement_noise, X(pose_idx),
P(landmark_idx), self._calibration))
# Create priors for first two poses
# """
# Pose prior noise:
# rotation_sigma = np.radians(60)
# translation_sigma = 1
# pose_noise_sigmas = np.array([rotation_sigma, rotation_sigma, rotation_sigma,
# translation_sigma, translation_sigma, translation_sigma])
# """
for idx in (0, 1):
pose_i = initial_estimate.atPose3(X(idx))
graph.add(
gtsam.PriorFactorPose3(X(idx), pose_i, self._pose_prior_noise))
# Optimization
# Using QR rather than Cholesky decomposition
# params = gtsam.LevenbergMarquardtParams()
# params.setLinearSolverType("MULTIFRONTAL_QR")
# optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
sfm_result = optimizer.optimize()
# Check if factor covariances are under constrain
marginals = gtsam.Marginals( # pylint: disable=unused-variable
graph, sfm_result)
return sfm_result
def add_GenericProjectionFactorCal3_S2_factor(self, pt_uv, X_id, L_id):
if pt_uv.ndim == 1:
raise ValueError(
"Supplied point is 1-dimensional, required Nx2 array")
if pt_uv.shape[1] != 2:
raise ValueError(
"2nd dimension on supplied point is not 2, required Nx2 array")
for pt, l in zip(pt_uv, L_id):
fact = gtsam.GenericProjectionFactorCal3_S2(
gtsam.gtsam.Point2(*pt), self.projection_noise,
self.get_key('x', X_id), self.get_key('l', l), self.K)
self.graph.push_back(fact)
self.lm_factor_ids.update(L_id)
def trajectory_estimator(self, features, visible_map):
""" Estimate current pose with matched features through GTSAM and update the trajectory
Parameters:
features: A Feature Object. Stores all matched Superpoint features.
visible_map: A Map Object. Stores all matched Landmark points.
pose: gtsam.pose3 Object. The pose at the last state from the atrium map trajectory.
Returns:
current_pose: gtsam.pose3 Object. The current estimate pose.
Use input matched features as the projection factors of the graph.
Use input visible_map (match landmark points of the map) as the landmark priors of the graph.
Use the last time step pose from the trajectory as the initial estimate value of the current pose
"""
assert len(self.atrium_map.trajectory) != 0, "Trajectory is empty."
pose = self.atrium_map.trajectory[len(self.atrium_map.trajectory)-1]
# Initialize factor graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add projection factors with matched feature points for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(
2, measurementNoiseSigma)
for i, feature in enumerate(features.key_point_list):
graph.add(gtsam.GenericProjectionFactorCal3_S2(
feature, measurementNoise,
X(0), P(i), self.calibration))
# Create initial estimate for the pose
# Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose.
initialEstimate.insert(X(0), pose)
# Create priors for visual
# Because the map is known, we use the landmarks from the visible map with nearly zero error as priors.
pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01)
for i, landmark in enumerate(visible_map.landmark_list):
graph.add(gtsam.PriorFactorPoint3(P(i),
landmark, pointPriorNoise))
initialEstimate.insert(P(i), landmark)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
# Add current pose to the trajectory
current_pose = result.atPose3(symbol(ord('x'), 0))
self.atrium_map.append_trajectory(current_pose)
return current_pose
def step(data, isam, result, truth, currPoseIndex):
'''
Do one step isam update
@param[in] data: measurement data (odometry and visual measurements and their noiseModels)
@param[in/out] isam: current isam object, will be updated
@param[in/out] result: current result object, will be updated
@param[in] truth: ground truth data, used to initialize new variables
@param[currPoseIndex]: index of the current pose
'''
# iSAM expects us to give it a new set of factors
# along with initial estimates for any new variables introduced.
newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = gtsam.Values()
# Add odometry
prevPoseIndex = currPoseIndex - 1
odometry = data.odometry[prevPoseIndex]
newFactors.add(
gtsam.BetweenFactorPose3(symbol('x', prevPoseIndex),
symbol('x', currPoseIndex), odometry,
data.noiseModels.odometry))
# Add visual measurement factors and initializations as necessary
for k in range(len(data.Z[currPoseIndex])):
zij = data.Z[currPoseIndex][k]
j = data.J[currPoseIndex][k]
jj = symbol('l', j)
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(zij,
data.noiseModels.measurement,
symbol('x', currPoseIndex),
jj, data.K))
# TODO: initialize with something other than truth
if not result.exists(jj) and not initialEstimates.exists(jj):
lmInit = truth.points[j]
initialEstimates.insert(jj, lmInit)
# Initial estimates for the new pose.
prevPose = result.atPose3(symbol('x', prevPoseIndex))
initialEstimates.insert(symbol('x', currPoseIndex),
prevPose.compose(odometry))
# Update ISAM
# figure(1)tic
isam.update(newFactors, initialEstimates)
# t=toc plot(frame_i,t,'r.') tic
newResult = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')
return isam, newResult
def pose_estimate(self, observations, estimated_pose):
""" Estimate current pose with matched features through GTSAM and update the trajectory
Parameters:
observations - [(Point2(), Point3())]
pose - gtsam.pose3 Object. The pose at the last state from the trajectory.
Returns:
current_pose - gtsam.pose3 Object. The current estimate pose.
"""
if len(observations) == 0:
print("No observation")
# Initialize factor graph
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
measurement_noise_sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
2, measurement_noise_sigma)
# Because the map is known, we use the landmarks from the visible map with nearly zero error as priors.
point_prior_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01)
for i, observation in enumerate(observations):
print(i)
# Add projection factors with matched feature points
graph.add(
gtsam.GenericProjectionFactorCal3_S2(observation[0],
measurement_noise, X(0),
P(i), self.calibration))
# Create priors for all observations
graph.add(
gtsam.PriorFactorPoint3(P(i), observation[1],
point_prior_noise))
initial_estimate.insert(P(i), observation[1])
# Create initial estimate for the pose
# Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose.
initial_estimate.insert(X(0), estimated_pose)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
result = optimizer.optimize()
# Add current pose to the trajectory
current_pose = result.atPose3(X(0))
print(current_pose)
return current_pose
def bundle_adjustment(self):
"""
Bundle Adjustment.
Input:
self._image_features
self._image_points
Output:
result - gtsam optimzation result
"""
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
pose_indices, point_indices = self.create_index_sets()
initial_estimate = self.create_initial_estimate(pose_indices)
# Create Projection Factor
sigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
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)
if self._landmark_estimates[
landmark_id].seen >= self._min_landmark_seen:
key_point = Point2(keypoint[0], keypoint[1])
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
key_point, measurementNoise, X(img_idx),
P(landmark_id), self._calibration))
# Create priors for first two poses
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 1, 1, 1])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
for idx in (0, 1):
pose_i = initial_estimate.atPose3(X(idx))
graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
sfm_result = optimizer.optimize()
return sfm_result, pose_indices, point_indices
def bundle_adjustment(self, data, y_distance, rotation_error,
translation_error):
""" Use GTSAM to solve Structure from Motion bundle adjustment.
Parameters:
data -- a Data Object, input feature point data from the sfm_data.py file
y_distance -- distances between two continuous poses along the y axis
rotation_error - pose prior rotation error
translation_error - pose prior translation error
Returns:
result -- GTSAM optimization result
"""
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add factors for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_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][j], measurementNoise, X(i), P(j),
self.calibration))
# Create priors and initial estimate
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 10, 10, 10])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
for i, y in enumerate([0, y_distance, 2 * y_distance]):
# Do not consider camera rotation when creating Rot3
wRc = gtsam.Rot3(np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]]).T)
# The approximate height measurement is 1.5
wTi = gtsam.Pose3(wRc, gtsam.Point3(0, y, 1.5))
initialEstimate.insert(X(i), wTi)
# Add priors for two poses
# Add the first pose prior with error to test similarity transform.
graph.add(
gtsam.PriorFactorPose3(
X(0),
gtsam.Pose3(
gtsam.Rot3(
np.array([[
0 + rotation_error, 1 + rotation_error,
0 + rotation_error
],
[
0 + rotation_error, 0 + rotation_error,
-1 + rotation_error
],
[
1 + rotation_error, 0 + rotation_error,
0 + rotation_error
]]).T),
gtsam.Point3(0 + translation_error, 0 + translation_error,
1.5 + translation_error)), posePriorNoise))
graph.add(
gtsam.PriorFactorPose3(
X(1), gtsam.Pose3(wRc, gtsam.Point3(0, y_distance, 1.5)),
posePriorNoise))
# Add initial estimates for Points.
for j in range(self.nrPoints):
# Generate initial estimates by back projecting key points extracted at the initial pose. The scale is set based on experience.
point_j = self.back_project(data.Z[0][j], self.calibration, 15)
initialEstimate.insert(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
# Marginalization
marginals = gtsam.Marginals(graph, result)
return result
def full_bundle_adjustment_update(self, sfm_map: SfmMap):
# Variable symbols for camera poses.
X = gtsam.symbol_shorthand.X
# Variable symbols for observed 3D point "landmarks".
L = gtsam.symbol_shorthand.L
# Create a factor graph.
graph = gtsam.NonlinearFactorGraph()
# Extract the first two keyframes (which we will assume has ids 0 and 1).
kf_0 = sfm_map.get_keyframe(0)
kf_1 = sfm_map.get_keyframe(1)
# We will in this function assume that all keyframes have a common, given (constant) camera model.
common_model = kf_0.camera_model()
calibration = gtsam.Cal3_S2(common_model.fx(), common_model.fy(), 0,
common_model.cx(), common_model.cy())
# Unfortunately, the dataset does not contain any information on uncertainty in the observations,
# so we will assume a common observation uncertainty of 3 pixels in u and v directions.
obs_uncertainty = gtsam.noiseModel.Isotropic.Sigma(2, 3.0)
# Add measurements.
for keyframe in sfm_map.get_keyframes():
for keypoint_id, map_point in keyframe.get_observations().items():
obs_point = keyframe.get_keypoint(keypoint_id).point()
factor = gtsam.GenericProjectionFactorCal3_S2(
obs_point, obs_uncertainty, X(keyframe.id()),
L(map_point.id()), calibration)
graph.push_back(factor)
# Set prior on the first camera (which we will assume defines the reference frame).
no_uncertainty_in_pose = gtsam.noiseModel.Constrained.All(6)
factor = gtsam.PriorFactorPose3(X(kf_0.id()), gtsam.Pose3(),
no_uncertainty_in_pose)
graph.push_back(factor)
# Set prior on distance to next camera.
no_uncertainty_in_distance = gtsam.noiseModel.Constrained.All(1)
prior_distance = 1.0
factor = gtsam.RangeFactorPose3(X(kf_0.id()), X(kf_1.id()),
prior_distance,
no_uncertainty_in_distance)
graph.push_back(factor)
# Set initial estimates from map.
initial_estimate = gtsam.Values()
for keyframe in sfm_map.get_keyframes():
pose_w_c = gtsam.Pose3(keyframe.pose_w_c().to_matrix())
initial_estimate.insert(X(keyframe.id()), pose_w_c)
for map_point in sfm_map.get_map_points():
point_w = gtsam.Point3(map_point.point_w().squeeze())
initial_estimate.insert(L(map_point.id()), point_w)
# Optimize the graph.
params = gtsam.GaussNewtonParams()
params.setVerbosity('TERMINATION')
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
# Update map with results.
for keyframe in sfm_map.get_keyframes():
updated_pose_w_c = result.atPose3(X(keyframe.id()))
keyframe.update_pose_w_c(SE3.from_matrix(
updated_pose_w_c.matrix()))
for map_point in sfm_map.get_map_points():
updated_point_w = result.atPoint3(L(map_point.id())).reshape(3, 1)
map_point.update_point_w(updated_point_w)
sfm_map.has_been_updated()
def bundle_adjustment(self):
MIN_LANDMARK_SEEN = 3
depth = 15
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add factors for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(
2, measurementNoiseSigma)
landmark_id_list = {}
img_pose_id_list = {}
for i in range(self.row*self.col*self.angles):
ir,i_mod = divmod(i,self.angles*self.col)
ic, ia = divmod(i_mod, self.angles)
img_pose = self.img_pose[ir][ic][ia]
for k in range(len(img_pose.kp)):
if img_pose.kp_3d_exist(k):
landmark_id = img_pose.kp_3d(k)
if(self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN):
key_point = Point2(img_pose.kp[k][0],img_pose.kp[k][1])
if landmark_id_list.get(landmark_id) == None:
pose = self.pose_initial[ir][ic][ia]
landmark_point = self.back_projection(key_point,pose,depth)
landmark_id_list[landmark_id] = landmark_point
# print("careful",ir,ic,ia,pose,landmark_point)
if img_pose_id_list.get(i) == None:
img_pose_id_list[i] = True
graph.add(gtsam.GenericProjectionFactorCal3_S2(
key_point, measurementNoise,
X(i), P(landmark_id), self.cal))
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 10, 10, 10])
# poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
print(img_pose_id_list)
for i,idx in enumerate(img_pose_id_list):
ir,i_mod = divmod(idx,self.angles*self.col)
ic, ia = divmod(i_mod, self.angles)
# Initialize estimate for poses
pose_i = self.pose_initial[ir][ic][ia]
initialEstimate.insert(X(idx), pose_i)
# Create priors for poses
# counter = 0
# if(counter == 0 and ia == 4):
# temp_a = ia
# temp_r = ir
# temp_c = ic
# graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# counter+=1
# if(counter == 1 and temp_a == ia and (temp_r-ir)**2 + (temp_c-ic)**2>=4):
# graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# counter +=1
# if i <2:
# graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# Initialize estimate for landmarks
print(landmark_id_list)
for idx in landmark_id_list:
point_j = landmark_id_list.get(idx)
initialEstimate.insert(P(idx), point_j)
self.landmark_descriptor.append(self.landmark[idx].desc)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
sfm_result = optimizer.optimize()
# Marginalization
marginals = gtsam.Marginals(graph, sfm_result)
for idx in img_pose_id_list:
marginals.marginalCovariance(X(idx))
for idx in landmark_id_list:
marginals.marginalCovariance(P(idx))
return sfm_result,img_pose_id_list,landmark_id_list
def bundle_adjustment(self):
MIN_LANDMARK_SEEN = 3
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add factors for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(
2, measurementNoiseSigma)
landmark_id_list = {}
img_pose_id_list = {}
for i, img_pose in enumerate(self.img_pose):
for k in range(len(img_pose.kp)):
if img_pose.kp_3d_exist(k):
landmark_id = img_pose.kp_3d(k)
if (self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN):
landmark_id_list[landmark_id] = True
img_pose_id_list[i] = True
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
Point2(img_pose.kp[k][0],
img_pose.kp[k][1]), measurementNoise,
X(i), P(landmark_id), self.cal))
# Initialize estimate for poses
# Create priors
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 10, 10, 10])
# poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
# theta = np.radians(30)
# wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0 , math.cos(theta)],
# [-math.cos(theta), 0 , -math.sin(theta)],
# [0, 1 , 0]]))
wRc = gtsam.Rot3(np.identity(3))
for i, idx in enumerate(img_pose_id_list):
initialEstimate.insert(X(idx), self.img_pose[idx].T)
if i < 2:
# graph.add(gtsam.PriorFactorPose3(X(idx), gtsam.Pose3(
# wRc, gtsam.Point3(-math.sin(theta)*1.58*idx, -math.cos(theta)*1.58*idx, 1.2)), posePriorNoise))
graph.add(
gtsam.PriorFactorPose3(
X(idx),
gtsam.Pose3(wRc,
gtsam.Point3(1.38 * idx, 0, -0.29 * idx)),
posePriorNoise))
# i+=1
# Initialize estimate for landmarks
for idx in landmark_id_list:
p = self.landmark[idx].point
initialEstimate.insert(P(idx), Point3(p[0], p[1], p[2]))
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
# sfm_result = optimizer.optimize()
for i in range(1000):
optimizer.iterate()
sfm_result = optimizer.values()
# print(sfm_result)
# Marginalization
marginals = gtsam.Marginals(graph, sfm_result)
for idx in img_pose_id_list:
marginals.marginalCovariance(X(idx))
for idx in landmark_id_list:
marginals.marginalCovariance(P(idx))
nrCamera = len(img_pose_id_list)
nrPoint = len(landmark_id_list)
return sfm_result, img_pose_id_list, landmark_id_list
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 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 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 bundle_adjustment(self):
MIN_LANDMARK_SEEN = 3
theta = np.radians(30)
wRc = Rot3(
np.array([[-math.sin(theta), 0,
math.cos(theta)],
[-math.cos(theta), 0, -math.sin(theta)], [0, -1, 0]]))
depth = 15
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add factors for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(
2, measurementNoiseSigma)
landmark_id_list = {}
img_pose_id_list = {}
for i, img_pose in enumerate(self.img_pose):
for k in range(len(img_pose.kp)):
if img_pose.kp_3d_exist(k):
landmark_id = img_pose.kp_3d(k)
if (self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN):
key_point = Point2(img_pose.kp[k][0],
img_pose.kp[k][1])
if landmark_id_list.get(landmark_id) == None:
pose = Pose3(
wRc,
Point3(-math.sin(theta) * 1.58 * i,
-math.cos(theta) * 1.58 * i, 1.2))
landmark_point = self.back_projection(
key_point, pose, depth)
landmark_id_list[landmark_id] = landmark_point
if img_pose_id_list.get(i) == None:
img_pose_id_list[i] = True
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
key_point, measurementNoise, X(i),
P(landmark_id), self.cal))
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 10, 10, 10])
# poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
for i, idx in enumerate(img_pose_id_list):
# Initialize estimate for poses
pose_i = Pose3(
wRc,
Point3(-math.sin(theta) * 1.58 * idx,
-math.cos(theta) * 1.58 * idx, 1.2))
initialEstimate.insert(X(idx), pose_i)
# Create priors for poses
if (idx == 0 or idx == 1):
graph.add(
gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# Initialize estimate for landmarks
for idx in landmark_id_list:
point_j = landmark_id_list.get(idx)
initialEstimate.insert(P(idx), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
sfm_result = optimizer.optimize()
# Marginalization
marginals = gtsam.Marginals(graph, sfm_result)
for idx in img_pose_id_list:
marginals.marginalCovariance(X(idx))
for idx in landmark_id_list:
marginals.marginalCovariance(P(idx))
return sfm_result, img_pose_id_list, landmark_id_list
def bundle_adjustment(cal, kp1, kp2, undist=0):
""" Use GTSAM to solve Structure from Motion.
Parameters:
cal - camera calibration matrix,gtsam.Cal3_S2/ gtsam.Cal3DS2
kp1 - keypoints extracted at camera pose 1,gtsam.Point2
kp2 - keypoints extracted at camera pose 2,gtsam.Point2
Output:
landmarks - a list includes landmark points, gtsam.Point3
poses - a list includes camera poses, gtsam.Pose3
"""
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Add factors for all measurements
measurement_noise_sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
2, measurement_noise_sigma)
for i in range(6):
if undist == 0:
graph.add(gtsam.GenericProjectionFactorCal3DS2(
kp1[i], measurement_noise,
X(0), P(i), cal))
graph.add(gtsam.GenericProjectionFactorCal3DS2(
kp2[i], measurement_noise,
X(1), P(i), cal))
if undist == 1:
graph.add(gtsam.GenericProjectionFactorCal3_S2(
kp1[i], measurement_noise,
X(0), P(i), cal))
graph.add(gtsam.GenericProjectionFactorCal3_S2(
kp2[i], measurement_noise,
X(1), P(i), cal))
# Create priors and initial estimate
s = np.radians(60) # pylint: disable=invalid-name
pose_noise_sigmas = np.array([s, s, s, 1, 1, 1])
# pose_noise_sigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
# Create Rot3
wRc = Rot3(np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]]
).T) # pylint: disable=invalid-name
for i, y in enumerate([0, 5*1.58]):
# The approximate height measurement is 1.2
initial_estimate.insert(X(i), Pose3(wRc, Point3(0, y, 1.2)))
graph.add(gtsam.PriorFactorPose3(X(i), Pose3(
wRc, Point3(0, y, 1.2)), pose_prior_noise))
# Add initial estimates for Points.
for j in range(6):
# Generate initial estimates by back projecting feature points collected at the initial pose
point_j = back_projection(cal, kp1[j], Pose3(
wRc, Point3(0, 0, 1.2)), 30, undist)
initial_estimate.insert(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
result = optimizer.optimize()
# Why this is not working?
# tol_error = gtsam.reprojectionErrors(graph, result)
# print("tol_error: ", tol_error)
error = graph.error(result)
print("Graph error:", error)
# print(result)
plot_with_result(result)
landmarks = [result.atPoint3(P(i))for i in range(6)]
poses = [result.atPose3(X(0)), result.atPose3(X(1))]
return landmarks, poses
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
