def triangulate_points(cameras,
filtered_applied,
stereo_triangulation,
min_likelihood=0.7):
if len(cameras) < 2:
raise Exception('Triangulation process needs at least two cameras')
number_of_frames = len(cameras[0].frames)
number_of_markers = len(cameras[0].frames[0].markers)
triangulated_frames = []
stereo_pair = None
if stereo_triangulation:
stereo_pair = get_best_pair(cameras)
# set up filter values
dt = 1.0 / 24
transition_matrix = np.eye(9, dtype=np.float32)
transition_matrix[0][3] = dt
transition_matrix[0][6] = 0.5 * dt * dt
transition_matrix[1][4] = dt
transition_matrix[1][7] = 0.5 * dt * dt
transition_matrix[2][5] = dt
transition_matrix[2][8] = 0.5 * dt * dt
measurement_matrix = np.array([(1, 0, 0, 0, 0, 0, 0, 0, 0),
(0, 1, 0, 0, 0, 0, 0, 0, 0),
(0, 0, 1, 0, 0, 0, 0, 0, 0)],
dtype=np.float32)
# init filters for all markers tracked
filters = []
for i in range(number_of_markers):
kalman_filter = cv2.KalmanFilter(9, 3, 0)
kalman_filter.transitionMatrix = transition_matrix
kalman_filter.measurementMatrix = measurement_matrix
filters.append(Filter(kalman_filter))
for i in range(number_of_frames):
triangulated_markers = []
for j in range(number_of_markers):
visible_counter = 0
for camera in cameras:
if camera.frames[i].markers[j].likelihood > 0 and \
((stereo_triangulation and (camera in stereo_pair[0] or camera in stereo_pair[1]))
or not stereo_triangulation):
visible_counter += 1
if visible_counter < 2:
continue
# check if old stereo triangulation method is used
if stereo_triangulation:
best_cameras = get_front_back_cameras_for_marker(
stereo_pair, i, j, 0.5)
triangulated = triangulate_point(best_cameras, i, j,
best_cameras[0].image_size)
else:
# use n view triangulation method
best_cameras = get_best_cameras(cameras, i, j, len(cameras),
min_likelihood)
system = MultiCameraSystem([cam.model for cam in best_cameras])
points = [
(cam.model.name,
[cam.frames[i].markers[j].x, cam.frames[i].markers[j].y])
for cam in best_cameras
]
triangulated = system.find3d(points)
average_likelihood = np.mean(
[cam.frames[i].markers[j].likelihood for cam in best_cameras])
point_triangulated = triangulated is not None and average_likelihood > min_likelihood
marker_key = best_cameras[0].frames[i].markers[j].marker_key
if point_triangulated:
# check if kalman filter is necessary
if filtered_applied:
triangulated_ec_world_frame_formatted = np.array(
([triangulated]), np.float32).T
# compensate for the initial state set to 0,0,0 in opencv kalman filter
if filters[j].first:
for l in range(100):
filters[j].filter.predict()
filters[j].filter.correct(
triangulated_ec_world_frame_formatted)
filters[j].first = False
filters[j].filter.predict()
estimated = filters[j].filter.correct(
triangulated_ec_world_frame_formatted)
# append triangulated point
triangulated_markers.append({
'point':
np.array([
estimated[0][0], estimated[1][0], estimated[2][0]
]),
'marker':
marker_key,
'cameras':
"".join([str(cam.number) for cam in best_cameras]),
'likelihood':
str(average_likelihood)
})
else:
# append triangulated point
triangulated_markers.append({
'point':
triangulated,
'marker':
marker_key,
'cameras':
"".join([str(cam.number) for cam in best_cameras]),
'likelihood':
str(average_likelihood)
})
triangulated_frames.append(triangulated_markers)
return triangulated_frames
