def run():
"""Execution."""
# Undistortion
if run_undistortion:
feature_matcher = FeatureMatcher(basedir, image_extension,
source_image_size, number_images)
feature_matcher.undistortion(distort_calibration_matrix,
distortion_coefficients, resize_output)
# Feature Extraction
feature_matcher = FeatureMatcher(basedir, image_extension,
source_image_size, number_images)
if run_feature_extraction:
feature_matcher.feature_extraction(undistort_img_size, feature_type)
# Feature Matching
if run_feature_matching:
feature_matcher.feature_matching(undistort_img_size, feature_type,
matching_type, calibration_matrix)
if run_bundle_adjustment:
# Create pose estimates
pose_estimates = pose_estimate_generator_rectangle(
theta, delta_x, delta_y, delta_z, prior1_delta, prior2_delta, rows,
cols, angles)
# Create measurement noise for bundle adjustment
sigma = 1.0
# measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
measurement_noise = gtsam.noiseModel_Robust(
gtsam.noiseModel_mEstimator_Huber(1.345),
gtsam.noiseModel_Isotropic.Sigma(2, sigma))
# Create 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
])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
# Create MappingBackEnd instance
back_end = MappingBackEnd(basedir, number_images, calibration_matrix,
pose_estimates, measurement_noise,
pose_prior_noise,
filter_bad_landmarks_enable,
min_obersvation_number, prob, threshold,
backprojection_depth)
# Bundle Adjustment
tic_ba = time.time()
sfm_result = back_end.bundle_adjustment()
toc_ba = time.time()
print('BA spents ', toc_ba - tic_ba, 's')
# print(sfm_result)
# Plot Result
plot_with_result(sfm_result, 30, 30, 30, 0.5)
# Save map data
if save_result:
back_end.save_map_to_file(sfm_result)
back_end.save_poses_to_file(sfm_result)
def run():
"""Execution."""
# Input images(undistorted) calibration
calibration = Cal3_S2(fx=232.0542,
fy=252.8620,
s=0,
u0=325.3452,
v0=240.2912)
# Create pose estimates
theta = 45
delta_x = 1
delta_y = -0.5
delta_z = 1.2
rows = 2
cols = 2
angles = 8
prior1_delta = [0, -1, 1.2, 0]
prior2_delta = [1, -1, 1.2, 0]
pose_estimates = pose_estimate_generator(theta, delta_x, delta_y, delta_z,
prior1_delta, prior2_delta, rows,
cols, angles)
# Create measurement noise for bundle adjustment
sigma = 1.0
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# Create 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
])
pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas)
# Create MappingBackEnd instance
data_directory = 'mapping/datasets/library_data/library_4X8/undistort_images/features/'
num_images = 34
filter_bad_landmarks_enable = True
min_obersvation_number = 3
prob = 0.9
threshold = 3
backprojection_depth = 2
back_end = MappingBackEnd(data_directory, num_images, calibration,
pose_estimates, measurement_noise,
pose_prior_noise, filter_bad_landmarks_enable,
min_obersvation_number, prob, threshold,
backprojection_depth)
# Bundle Adjustment
tic_ba = time.time()
sfm_result = back_end.bundle_adjustment()
toc_ba = time.time()
print('BA spents ', toc_ba - tic_ba, 's')
# Plot Result
plot_with_result(sfm_result, 5, 5, 5, 0.5)
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