Как реализовать мультипликативный расширенный фильтр Калмана в приложении OpenCV ARUCO? ⇐ Python

[Anonymous]

Я искал способ удаления дрожания из проекта, включающего отслеживание двух маркерных досок ARUCO. Камера статична, и у меня есть две маркерные доски, движущиеся по отношению друг к другу. Я смотрел на расширенные фильтры Калмана, и кажется, что мультипликативное может быть решением для меня. Ссылка ниже суммирует подход для подхода, основанного на кватернионе, который работает на ориентации с использованием imu.
https://matthewhampsey.github.io/blog/2020/07/18/mekf написал/> Функция. />https://drive.google.com/drive/folders/ ... ingобразно Макет двух объектов, перемещающихся с очень видимым джиттером (RVEC и TVEC генерируются из камеры калиброванной калиброванной машины - 4K -поток - входное изображение для OpenCV настолько хорошо /> < /p>
Ниже приведен некоторый код шаблона, чтобы экспериментировать с набором данных и увидеть джиттер. < /p>

Код: Выделить всё

import numpy as np
import open3d as o3d
import cv2
import time
def convert_to_open3d_affine(r,t):
#takes rvec and tvec and converts to 4x4 affine transform with both rotation and translation
R, _ = cv2.Rodrigues(r)
bob=np.eye(4,4)
bob[:3,:3]=R
bob[:,3][:3]=t.reshape(1,3)
#convert from opencv format of affine to open3d
bob[1:3]=-bob[1:3]
return bob

#load saved opencv tracking data
import pickle
with open('p1_rvec_store.pickle', 'rb') as handle:
probe_1_rvec_store = pickle.load(handle)
with open('p1_tvec_store.pickle', 'rb') as handle:
probe_1_tvec_store = pickle.load(handle)
with open('p2_rvec_store.pickle', 'rb') as handle:
probe_2_rvec_store = pickle.load(handle)
with open('p2_tvec_store.pickle', 'rb') as handle:
probe_2_tvec_store = pickle.load(handle)
with open('probe_1_vertices.pickle', 'rb') as handle:
probe_1_vertices = pickle.load(handle)
with open('probe_2_vertices.pickle', 'rb') as handle:
probe_2_vertices = pickle.load(handle)

#load point clouds of tracked objects
pcd=o3d.geometry.PointCloud()
pcd.points=o3d.utility.Vector3dVector(probe_1_vertices)
pcd2=o3d.geometry.PointCloud()
pcd2.points=o3d.utility.Vector3dVector(probe_2_vertices)

#setup visualiser
vis = o3d.visualization.Visualizer()
vis.create_window(height=480, width=640)
vis.add_geometry(pcd)
vis.add_geometry(pcd2)
while True:
for pr,pt,sr,st in zip(probe_1_rvec_store,probe_1_tvec_store,probe_2_rvec_store,probe_2_tvec_store):
time.sleep(0.05)
pcd.points=o3d.utility.Vector3dVector(probe_1_vertices)
pcd2.points=o3d.utility.Vector3dVector(probe_2_vertices)

#need to insert kalman filter here, taking pr,pt,sr and st as input

pcd.points=pcd.transform(convert_to_open3d_affine(pr,pt)).points #open3d takes 4x4 affine as input for transforms hence conversion
pcd2.points=pcd2.transform(convert_to_open3d_affine(sr,st)).points #open3d takes 4x4 affine as input for transforms hence conversion

vis.update_geometry(pcd)
vis.update_geometry(pcd2)
vis.poll_events()
vis.update_renderer()

Любая помощь, возможно, с кодом Python, чтобы уточнить>

Подробнее здесь: https://stackoverflow.com/questions/796 ... ruco-appli