def test_bundle_adjustment(self):
"""Test Structure from Motion bundle adjustment solution with accurate prior value."""
# Create the set of ground-truth landmarks
expected_points = sfm_data.create_points()
# Create the set of ground-truth poses
expected_poses = sfm_data.create_poses()
# Create the nrCameras*nrPoints feature data input for atrium_sfm()
feature_data = sfm_data.Data(self.nrCameras, self.nrPoints)
# Project points back to the camera to generate synthetic key points
for i, pose in enumerate(expected_poses):
for j, point in enumerate(expected_points):
feature_data.J[i][j] = j
camera = gtsam.PinholeCameraCal3_S2(pose, self.calibration)
feature_data.Z[i][j] = camera.project(point)
result = self.sfm.bundle_adjustment(feature_data,2.5, 0, 0)
# Compare output poses with ground truth poses
for i in range(len(expected_poses)):
pose_i = result.atPose3(symbol(ord('x'), i))
self.assert_gtsam_equals(pose_i, expected_poses[i])
# Compare output points with ground truth points
for j in range(len(expected_points)):
point_j = result.atPoint3(symbol(ord('p'), j))
self.assert_gtsam_equals(point_j, expected_points[j])
def landmark_projection(self, pose):
""" Project landmark points in the map to the given camera pose.
And filter landmark points outside the view of the current camera pose.
Parameters:
pose - gtsam.Point3, the pose of a camera
self.map - A Landmarks Object
Returns:
observed_landmarks - A Landmarks Object
"""
# Check if the atrium map is empty
assert self.map.get_length(), "the map is empty"
observed_landmarks = Landmarks([], [])
for i, landmark_point in enumerate(self.map.landmarks):
camera = gtsam.PinholeCameraCal3_S2(pose, self.calibration)
# feature is gtsam.Point2 object
landmark_point = Point3(landmark_point[0], landmark_point[1],
landmark_point[2])
feature_point = camera.project(landmark_point)
# Check if the projected feature is within the field of view.
if (feature_point.x() >= 0 and feature_point.x() < self.width
and feature_point.y() >= 0
and feature_point.y() < self.height):
observed_landmarks.append(
self.map.landmarks[i], self.map.descriptors[i],
[feature_point.x(), feature_point.y()])
return observed_landmarks
def landmarks_projection(self, pose):
""" Project landmark points in the map to the given camera pose.
And filter landmark points outside the view of the current camera pose.
Parameters:
pose: gtsam.Point3, the pose of a camera
Returns:
Features: A Feature object. Contains all the projected features that can be viewed at the current camera pose.
map_indices: A list of indices. Records the corresponding indices in the map of projected feature.
The length is the number of feature points in Features.
"""
# Check if the atrium map is empty
assert self.atrium_map.get_length(), "the atrium map is empty"
points = []
descriptors = []
map_indices = []
for i, landmark_point in enumerate(self.atrium_map.landmark_list):
camera = gtsam.PinholeCameraCal3_S2(pose, self.calibration)
# feature is gtsam.Point2 object
feature_point = camera.project(landmark_point)
# Check if the projected feature is within the field of view.
if (feature_point.x() > 0 and feature_point.x() < self.image_width
and feature_point.y() > 0 and feature_point.y() < self.image_height):
points.append(feature_point)
descriptors.append(self.atrium_map.get_descriptor_from_list(i))
map_indices.append(i)
return Features(np.array(points), np.array(descriptors)), map_indices
def test_insertBackprojections(self):
"""Test insertBackprojections."""
values = gtsam.Values()
cam = gtsam.PinholeCameraCal3_S2()
gtsam.utilities.insertBackprojections(
values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
10)
np.testing.assert_allclose(values.atPoint3(0),
gtsam.Point3(200, 200, 10))
def test_bundle_adjustment_with_error(self):
"""Test Structure from Motion solution with ill estimated prior value."""
# Create the set of actual points and poses
actual_points = []
actual_poses = []
# Create the set of ground-truth landmarks
points = sfm_data.create_points()
# Create the set of ground-truth poses
poses = sfm_data.create_poses()
# Create the nrCameras*nrPoints feature point data input for atrium_sfm()
feature_data = sfm_data.Data(self.nrCameras, self.nrPoints)
# Project points back to the camera to generate feature points
for i, pose in enumerate(poses):
for j, point in enumerate(points):
feature_data.J[i][j] = j
camera = gtsam.PinholeCameraCal3_S2(pose, self.calibration)
feature_data.Z[i][j] = camera.project(point)
result = self.sfm.bundle_adjustment(
feature_data, 2.5, self.rotation_error, self.translation_error)
# Similarity transform
s_poses = []
s_points = []
_sim3 = sim3.Similarity3()
for i in range(len(poses)):
pose_i = result.atPose3(symbol(ord('x'), i))
s_poses.append(pose_i)
for j in range(len(points)):
point_j = result.atPoint3(symbol(ord('p'), j))
s_points.append(point_j)
pose_pairs = [[s_poses[2], poses[2]], [s_poses[1], poses[1]], [s_poses[0], poses[0]]]
_sim3.sim3_pose(pose_pairs)
s_map = (s_poses, s_points)
actual_poses, actual_points = _sim3.map_transform(s_map)
# Compare output poses with ground truth poses
for i, pose in enumerate(actual_poses):
self.assert_gtsam_equals(pose, poses[i],1e-2)
# Compare output points with ground truth points
for i, point in enumerate(actual_points):
self.assert_gtsam_equals(point, points[i], 1e-2)
def test_reprojectionErrors(self):
"""Test reprojectionErrors."""
pixels = np.asarray([[20, 30], [20, 30]])
I = [1, 2]
K = gtsam.Cal3_S2()
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K)
values = gtsam.Values()
values.insert(0, gtsam.Pose3())
cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K)
gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10)
errors = gtsam.utilities.reprojectionErrors(graph, values)
np.testing.assert_allclose(errors, np.zeros((2, 2)))
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()
K = np.array([[50.0, 0, 50.0], [0.0, 50.0, 50.0], [0.0, 0.0, 1.0]])
D = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
Ksim = gtsam.Cal3_S2(iSAM2Wrapper.CameraMatrix_to_Cal3_S2(K))
# Create a sample world point, this case it is 10,10,10
points = SFMdata.createPoints()
point_3d = points[0]
# Create 2 camera poses to measure the 3d point from
poses = SFMdata.createPoses(Ksim)
pose1 = poses[0]
pose2 = poses[1]
# Create GTSAM camera objects with poses
camera1 = gtsam.PinholeCameraCal3_S2(pose1, Ksim)
camera2 = gtsam.PinholeCameraCal3_S2(pose2, Ksim)
# Project the 3D point into the 2 Camera poses
kp1 = camera1.project(point_3d)
kp2 = camera2.project(point_3d)
# Create Transforms of world frame with respect to camera frames
Pose_1Tw = T_inv(poses[0].matrix())
Pose_2Tw = T_inv(poses[1].matrix())
# Undistort and nomalize camera measurements
kp1_ud = cv2.undistortPoints(
np.expand_dims(np.expand_dims(kp1.vector(), 0), 1), K, D)[:, 0, :]
kp2_ud = cv2.undistortPoints(
np.expand_dims(np.expand_dims(kp2.vector(), 0), 1), K, D)[:, 0, :]
factor_graph = iSAM2Wrapper(poses[0].matrix(),
pose0_to_pose1_range=25,
relinearizeThreshold=0.1,
relinearizeSkip=1,
K=np.eye(3),
proj_noise_val=1.0 / 100)
#factor_graph.set_Camera_matrix(np.eye(3))
#factor_graph.set_Projection_noise(1.0/100)
# Create a Factor Graph and Values to hold the new data
# 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, Ksim)
measurement = camera.project(point)
undist_m = cv2.undistortPoints(
np.expand_dims(
np.expand_dims(iSAM2Wrapper.Point2arr(measurement), 0), 1),
K, D)[:, 0, :]
#undist_pt = iSAM2Wrapper.arr2Point(undist_m)
#m_pt = np.array([measurement.x(), measurement.y()])
factor_graph.add_GenericProjectionFactorCal3_S2_factor(
undist_m, i, np.array([j]))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
pose_est = pose.compose(
gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25),
