Я использую Python с OpenCV 3.4.
У меня есть система, состоящая из 2 камер, которые я хочу использовать для отслеживания объекта и получения его траектории, а затем его скорости.
В настоящее время я могу выполнять внутреннюю и внешнюю калибровку каждой из моих камер. Я могу отслеживать свой объект через видео и получать 2d координаты в моем видео плане.
Моя проблема сейчас в том, что я хотел бы проецировать свои точки из моего 2D-плана в 3D-точки.
Я пробовал функции как triangulatePoints, но, похоже, он не работает должным образом.
Вот моя настоящая функция для получения 3d координат. Возвращает некоторые координаты, которые кажутся немного смещенными по сравнению с фактическими координатами
def get_3d_coord(left_two_d_coords, right_two_d_coords):
pt1 = left_two_d_coords.reshape((len(left_two_d_coords), 1, 2))
pt2 = right_two_d_coords.reshape((len(right_two_d_coords), 1, 2))
extrinsic_left_camera_matrix, left_distortion_coeffs, extrinsic_left_rotation_vector, \
extrinsic_left_translation_vector = trajectory_utils.get_extrinsic_parameters(
1)
extrinsic_right_camera_matrix, right_distortion_coeffs, extrinsic_right_rotation_vector, \
extrinsic_right_translation_vector = trajectory_utils.get_extrinsic_parameters(
2)
#returns arrays of the same size
(pt1, pt2) = correspondingPoints(pt1, pt2)
projection1 = computeProjMat(extrinsic_left_camera_matrix,
extrinsic_left_rotation_vector, extrinsic_left_translation_vector)
projection2 = computeProjMat(extrinsic_right_camera_matrix,
extrinsic_right_rotation_vector, extrinsic_right_translation_vector)
out = cv2.triangulatePoints(projection1, projection2, pt1, pt2)
oc = []
for idx, elem in enumerate(out[0]):
oc.append((out[0][idx], out[1][idx], out[2][idx], out[3][idx]))
oc = np.array(oc, dtype=np.float32)
point3D = []
for idx, elem in enumerate(oc):
W = out[3][idx]
obj = [None] * 4
obj[0] = out[0][idx] / W
obj[1] = out[1][idx] / W
obj[2] = out[2][idx] / W
obj[3] = 1
pt3d = [obj[0], obj[1], obj[2]]
point3D.append(pt3d)
return point3D
Вот несколько скриншотов 2d траектории, которую я получаю для обеих моих камер:
Вот несколько скриншотов 3d-траектории, которую мы получаем для той же камеры.
Как вы можете видеть, 2-я траектория не выглядит как 3-я, и я не могу получить точное расстояние между двумя точками.
Я просто хотел бы получить реальные координаты, это значит знать (почти) точное реальное расстояние, пройденное человеком даже по кривой дороге.
ИЗМЕНИТЬ, чтобы добавить справочные данные и примеры
Вот некоторые примеры и входные данные для воспроизведения проблемы.
Во-первых, вот некоторые данные.
2D очки для камеры1
546,357
646,351
767,357
879,353
986,360
1079,365
1152,364
соответствующий 2D для камеры2
236,305
313,302
414,308
532,308
647,314
752,320
851,323
3D очки, которые мы получаем от triangulatePoints
"[0.15245444, 0.30141047, 0.5444277]"
"[0.33479974, 0.6477136, 0.25396818]"
"[0.6559921, 1.0416716, -0.2717265]"
"[1.1381898, 1.5703914, -0.87318224]"
"[1.7568599, 1.9649554, -1.5008119]"
"[2.406788, 2.302272, -2.0778883]"
"[3.078426, 2.6655817, -2.6113863]"
На этих следующих изображениях мы видим двухмерную траекторию (верхняя линия) и трехмерную проекцию, перепроектированную в 2d (нижняя линия). Цвета чередуются, чтобы показать, какие 3d точки соответствуют 2d точке.
И, наконец, вот некоторые данные для воспроизведения.
камера 1: матрица камеры
5.462001610064596662e+02 0.000000000000000000e+00 6.382260289544193483e+02
0.000000000000000000e+00 5.195528638702176067e+02 3.722480290221320161e+02
0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00
камера 2: матрица камеры
4.302353276501239066e+02 0.000000000000000000e+00 6.442674231451971991e+02
0.000000000000000000e+00 4.064124751062329324e+02 3.730721752718034736e+02
0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00
камера 1: вектор искажения
-1.039009381799949928e-02 -6.875769941694849507e-02 5.573643708806085006e-02 -7.298826373638074051e-04 2.195279856716004369e-02
камера 2: вектор искажения
-8.089289768586239993e-02 6.376634681503455396e-04 2.803641672679824115e-02 7.852965318823987989e-03 1.390248981867302919e-03
камера 1: вектор вращения
1.643658457134109296e+00
-9.626823326237364531e-02
1.019865700311696488e-01
камера 2: вектор вращения
1.698451227150894471e+00
-4.734769748661146055e-02
5.868343803315514279e-02
камера 1: вектор перевода
-5.004031689969588026e-01
9.358682517577661120e-01
2.317689087311113116e+00
камера 2: вектор перевода
-4.225788801112133619e+00
9.519952012307866251e-01
2.419197507326224184e+00
камера 1: точки объекта
0 0 0
0 3 0
0.5 0 0
0.5 3 0
1 0 0
1 3 0
1.5 0 0
1.5 3 0
2 0 0
2 3 0
камера 2: точки объекта
4 0 0
4 3 0
4.5 0 0
4.5 3 0
5 0 0
5 3 0
5.5 0 0
5.5 3 0
6 0 0
6 3 0
камера 1: точки изображения
5.180000000000000000e+02 5.920000000000000000e+02
5.480000000000000000e+02 4.410000000000000000e+02
6.360000000000000000e+02 5.910000000000000000e+02
6.020000000000000000e+02 4.420000000000000000e+02
7.520000000000000000e+02 5.860000000000000000e+02
6.500000000000000000e+02 4.430000000000000000e+02
8.620000000000000000e+02 5.770000000000000000e+02
7.000000000000000000e+02 4.430000000000000000e+02
9.600000000000000000e+02 5.670000000000000000e+02
7.460000000000000000e+02 4.430000000000000000e+02
камера 2: точки изображения
6.080000000000000000e+02 5.210000000000000000e+02
6.080000000000000000e+02 4.130000000000000000e+02
7.020000000000000000e+02 5.250000000000000000e+02
6.560000000000000000e+02 4.140000000000000000e+02
7.650000000000000000e+02 5.210000000000000000e+02
6.840000000000000000e+02 4.150000000000000000e+02
8.400000000000000000e+02 5.190000000000000000e+02
7.260000000000000000e+02 4.160000000000000000e+02
9.120000000000000000e+02 5.140000000000000000e+02
7.600000000000000000e+02 4.170000000000000000e+02