-
Notifications
You must be signed in to change notification settings - Fork 3
/
test_bundle_adjustment.py
127 lines (106 loc) · 6.2 KB
/
test_bundle_adjustment.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
from .context import multiple_view_geometry
import numpy as np
import g2o
from .scene_fixture import SceneWithNoiseFixture, add_gaussian_noise_to_homogeneous_matrix
from multiple_view_geometry.algorithm import (
solve_essential_matrix,
reconstruct_translation_and_rotation_from_svd_of_essential_matrix,
structure_from_motion_options,
)
from multiple_view_geometry.homogeneous_matrix import HomogeneousMatrix
from multiple_view_geometry.bundle_ajustment import BundleAjustment
class TestBundleAdjustment(SceneWithNoiseFixture):
def setUp(self):
SceneWithNoiseFixture.setUp(self)
self._bundle_adjustment = BundleAjustment()
self._points_3d_initial_index = 2
def _compute_transform_cam0_wrt_cam1_by_bundle_adjustment(
self, transform_cam0_wrt_cam1, points_3d_in_camera_frame0, iterations=10, verbose=False
):
self._bundle_adjustment.add_camera_parameters(self._camera0.focal_length_in_pixels, self._camera0.pixel_center)
# First camera frame0 is regarded as the origin of the world
self._bundle_adjustment.add_pose(0, g2o.SE3Quat(R=np.identity(3), t=np.zeros(3)), fixed=True)
# IMPORTANT NOTE: rotation and translation of pose is described as world wrt to pose
self._bundle_adjustment.add_pose(
1, g2o.SE3Quat(R=transform_cam0_wrt_cam1.rotation, t=transform_cam0_wrt_cam1.translation)
)
points_id = np.arange(len(points_3d_in_camera_frame0.T)) + self._points_3d_initial_index
for point_id, point_3d in zip(points_id, points_3d_in_camera_frame0.T):
self._bundle_adjustment.add_point(point_id, point_3d)
pose_id_to_points_map = {
0: (points_id, self._points_in_image_frame0.T),
1: (points_id, self._points_in_image_frame1.T),
}
for pose_id, bundle in pose_id_to_points_map.items():
for point_id, measured_point_2d in zip(*bundle):
self._bundle_adjustment.add_edge(point_id, pose_id, measured_point_2d, information=np.identity(2))
self._bundle_adjustment.optimize(iterations, verbose)
return HomogeneousMatrix(self._bundle_adjustment.vertex_estimate(1).matrix()[:3, :4])
def _mse_key_points_cube(self, noisy_key_points_cube):
return ((self._key_points_cube - noisy_key_points_cube) ** 2).mean()
def test_bundle_adjustment_when_initial_condition_given_by_eight_point_algorithm(self):
self.add_noise_in_pixel(0, 0.0)
self.assertGreaterEqual(self._key_points_cube.shape[1], 8)
# we know the points are not all on the same surface
# and we use more than 8 points here to guarentee that rank of A is larger than 8
essential_matrix, u, s, vh = solve_essential_matrix(
self._points_in_image_frame0, self._camera0, self._points_in_image_frame1, self._camera1
)
# there are two possible solutions, after we implement reconstuct stuction function, we can check
# the plausible solution should have positive depth
T1, R1, T2, R2 = reconstruct_translation_and_rotation_from_svd_of_essential_matrix(u, s, vh)
homogeneous_points_in_camera_frame0 = self._camera0.points_2d_to_homogeneous_coordinates(
self._points_in_image_frame0
)
homegeneous_points_in_camera_frame1 = self._camera1.points_2d_to_homogeneous_coordinates(
self._points_in_image_frame1
)
transform_cam1_wrt_cam0, points_depth_scale_in_cam1 = structure_from_motion_options(
homogeneous_points_in_camera_frame0,
homegeneous_points_in_camera_frame1,
HomogeneousMatrix.create(T1, R1),
HomogeneousMatrix.create(T2, R2),
)
points_3d_in_camera_frame1 = points_depth_scale_in_cam1 * homegeneous_points_in_camera_frame1
points_3d_in_camera_frame0 = transform_cam1_wrt_cam0 * points_3d_in_camera_frame1
transform_cam0_wrt_cam1 = HomogeneousMatrix(transform_cam1_wrt_cam0.inv())
transform_cam0_wrt_cam1_bundle_adjustment = self._compute_transform_cam0_wrt_cam1_by_bundle_adjustment(
transform_cam0_wrt_cam1, points_3d_in_camera_frame0
)
# test the recovered translation and rotation is the same as the ground truth
scale = 3.0 / 5
ground_truth_transform = self._camera0.get_transform_wrt(self._camera1)
# rotation and translation estimated by bundle adjustment
np.testing.assert_array_almost_equal(
transform_cam0_wrt_cam1_bundle_adjustment.translation * scale, ground_truth_transform.translation
)
np.testing.assert_array_almost_equal(
transform_cam0_wrt_cam1_bundle_adjustment.rotation, ground_truth_transform.rotation
)
def test_bundle_adjustment_when_initial_condition_given_by_groundtruth_with_noise(self):
self.add_noise_in_pixel(0, 2)
noisy_key_points_cube = self.add_noise_in_3d_points(0, 0.2)
gt_transform_cam0_wrt_cam1 = self._camera0.get_transform_wrt(self._camera1)
noisy_transform_cam0_wrt_cam1 = add_gaussian_noise_to_homogeneous_matrix(
gt_transform_cam0_wrt_cam1, 0, 0.05, 0, 0.2
)
noisy_3d_points_in_camera0 = self._camera0.world_frame_to_camera_frame(noisy_key_points_cube)
transform_cam0_wrt_cam1_bundle_adjustment = self._compute_transform_cam0_wrt_cam1_by_bundle_adjustment(
noisy_transform_cam0_wrt_cam1, noisy_3d_points_in_camera0
)
np.testing.assert_array_almost_equal(
transform_cam0_wrt_cam1_bundle_adjustment.translation, gt_transform_cam0_wrt_cam1.translation, decimal=0.1
)
np.testing.assert_array_almost_equal(
transform_cam0_wrt_cam1_bundle_adjustment.rotation, gt_transform_cam0_wrt_cam1.rotation, decimal=0.1
)
mse_before_ba = self._mse_key_points_cube(noisy_key_points_cube)
ba_points_wrt_cam0 = np.asarray(
[
self._bundle_adjustment.vertex_estimate(i)
for i in np.arange(len(self._key_points_cube.T)) + self._points_3d_initial_index
]
)
ba_points_wrt_world = self._camera0.extrinsic * ba_points_wrt_cam0.T
mse_after_ba = self._mse_key_points_cube(ba_points_wrt_world)
self.assertLess(mse_after_ba, mse_before_ba)