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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269 | def calibrate(
self,
correspondences: Iterable[Correspondence],
camera_1: Optional[Camera] = None,
camera_2: Optional[Camera] = None,
) -> Tuple[StereoCamera, StereoCalibrationStats]:
"""
Calibrate from the provided correspondences.
Args:
correspondences: correspondences to calibrate the stereo camera with.
Returns:
Stereo camera
Stereo calibration stats
"""
# Decompose correspondences for each camera
camera_1_correpsondences = []
camera_2_correspondences = []
for correspondence in correspondences:
decomposed_correspondences = list(correspondence.decompose())
assert len(decomposed_correspondences) == 2, "Expected two views per correspondence."
camera_1_correpsondences.append(decomposed_correspondences[0])
camera_2_correspondences.append(decomposed_correspondences[1])
# If USE_EXTRINSIC_GUESS is set, use the provided extrinsics from camera 2
prior_rotation_matrix = None
prior_translation_vector = None
if self._flags & cv2.CALIB_USE_EXTRINSIC_GUESS:
if camera_2 is None:
raise ValueError("CALIB_USE_EXTRINSIC_GUESS is set, but camera 2 is not provided.")
prior_rotation_matrix = camera_2.extrinsics.rotation.to_matrix()
prior_translation_vector = camera_2.extrinsics.translation.to_vector().flatten()
# If either camera is not provided, calibrate it
mono_calibrator = OpenCVMonocularCalibrator(
self._image_width, self._image_height, self._flags
)
if camera_1 is None:
self.logger.info("Camera 1 not provided, calibrating")
camera_1, _ = mono_calibrator.calibrate(camera_1_correpsondences)
if camera_2 is None:
self.logger.info("Camera 2 not provided, calibrating")
camera_2, _ = mono_calibrator.calibrate(camera_2_correspondences)
# Get camera intrinsics and distortion coefficients as numpy arrays
prior_intrinsic_matrix_1 = camera_1.intrinsics.to_matrix()
prior_distortion_coefficients_vector_1 = camera_1.distortion.to_vector().ravel()
prior_intrinsic_matrix_2 = camera_2.intrinsics.to_matrix()
prior_distortion_coefficients_vector_2 = camera_2.distortion.to_vector().ravel()
for _ in range(1):
# Compute the relative transforms between the two cameras
relative_translations = []
for correspondence in correspondences:
_, rvec_1, tvec_1 = cv2.solvePnP(
correspondence.object_points,
correspondence.image_points[0],
prior_intrinsic_matrix_1,
prior_distortion_coefficients_vector_1,
)
_, rvec_2, tvec_2 = cv2.solvePnP(
correspondence.object_points,
correspondence.image_points[1],
prior_intrinsic_matrix_2,
prior_distortion_coefficients_vector_2,
)
pose_1 = Pose3D(Rotation3D_Rodrigues(rvec_1), Translation3D.from_vector(tvec_1))
pose_2 = Pose3D(Rotation3D_Rodrigues(rvec_2), Translation3D.from_vector(tvec_2))
relative_pose = pose_1 @ pose_2.invert()
relative_translations.append(relative_pose.translation.to_vector().ravel())
# Filter outlier correspondences
relative_translations = np.array(relative_translations)
mean_translation = np.mean(relative_translations, axis=0)
self.logger.debug(f"Mean translation: {mean_translation}")
std_translation = np.std(relative_translations, axis=0)
translation_threshold = 2.0 * std_translation
self.logger.debug(f"Translation threshold: {translation_threshold}")
correspondences = [
correspondence
for correspondence, relative_translation in zip(
correspondences, relative_translations
)
if np.all(np.abs(relative_translation - mean_translation) < translation_threshold)
]
# Get object points and image points for each camera
object_points = [correspondence.object_points for correspondence in correspondences]
image_points_1 = [correspondence.image_points[0] for correspondence in correspondences]
image_points_2 = [correspondence.image_points[1] for correspondence in correspondences]
# Run the calibration
self.logger.info(f"Calibrating stereo camera with {len(correspondences)} correspondences")
(
_,
intrinsic_matrix_1,
distortion_coefficients_1,
intrinsic_matrix_2,
distortion_coefficients_2,
rotation_matrix,
translation_vector,
essential_matrix,
fundamental_matrix,
rvecs_1,
tvecs_1,
_,
) = cv2.stereoCalibrateExtended(
object_points,
image_points_1,
image_points_2,
prior_intrinsic_matrix_1,
prior_distortion_coefficients_vector_1,
prior_intrinsic_matrix_2,
prior_distortion_coefficients_vector_2,
self.size,
R=prior_rotation_matrix,
T=prior_translation_vector,
flags=self._flags,
)
# Compute relative pose
rotation_1_2 = Rotation3D_Matrix(rotation_matrix)
translation_1_2 = Translation3D.from_vector(translation_vector)
pose_1_2 = Pose3D(rotation_1_2, translation_1_2)
# Compute rvecs, tvecs in camera 2 frame, and poses in both
rvecs_2 = []
tvecs_2 = []
poses_1 = []
poses_2 = []
for rvec_1, tvec_1 in zip(rvecs_1, tvecs_1):
rotation_1 = Rotation3D_Rodrigues(rvec_1)
translation_1 = Translation3D.from_vector(tvec_1)
pose_1 = Pose3D(rotation_1, translation_1)
pose_2 = pose_1_2 @ pose_1
poses_1.append(pose_1)
poses_2.append(pose_2)
rvec_2, _ = cv2.Rodrigues(pose_2.rotation.to_matrix())
tvec_2 = pose_2.translation.to_vector()
rvecs_2.append(rvec_2)
tvecs_2.append(tvec_2)
# Compute reprojection errors
reprojection_errors_1 = compute_reprojection_errors_opencv(
intrinsic_matrix_1,
distortion_coefficients_1,
rvecs_1,
tvecs_1,
object_points,
image_points_1,
)
reprojection_errors_2 = compute_reprojection_errors_opencv(
intrinsic_matrix_2,
distortion_coefficients_2,
rvecs_2,
tvecs_2,
object_points,
image_points_2,
)
camera_1_stats = MonocularCalibrationStats(reprojection_errors_1, poses_1)
camera_2_stats = MonocularCalibrationStats(reprojection_errors_2, poses_2)
self.logger.debug(f"Intrinsic matrix 1: \n{intrinsic_matrix_1}")
self.logger.debug(f"Distortion coefficients 1: \n{distortion_coefficients_1}")
self.logger.debug(f"Intrinsic matrix 2: \n{intrinsic_matrix_2}")
self.logger.debug(f"Distortion coefficients 2: \n{distortion_coefficients_2}")
self.logger.debug(f"Rotation matrix: \n{rotation_matrix}")
self.logger.debug(f"Translation vector: \n{translation_vector}")
self.logger.debug(f"Essential matrix: \n{essential_matrix}")
self.logger.debug(f"Fundamental matrix: \n{fundamental_matrix}")
# Collect stereo stats
stereo_stats = StereoCalibrationStats(
camera_1_stats=camera_1_stats, camera_2_stats=camera_2_stats
)
# Collect cameras
camera_1 = Camera(
intrinsics=CameraIntrinsics.from_matrix(intrinsic_matrix_1),
distortion=DistortionCoefficients.from_vector(distortion_coefficients_1),
)
camera_2 = Camera(
intrinsics=CameraIntrinsics.from_matrix(intrinsic_matrix_2),
distortion=DistortionCoefficients.from_vector(distortion_coefficients_2),
extrinsics=Pose3D(
rotation=Rotation3D_Matrix(rotation_matrix),
translation=Translation3D.from_vector(translation_vector),
).invert(), # Invert since OpenCV provides the extrinsics of the first camera relative to the second camera
)
# Collect stereo camera
stereo_camera = StereoCamera(camera_1=camera_1, camera_2=camera_2)
return stereo_camera, stereo_stats
|