python - solvePnP() から取得したポーズ (回転行列と並進ベクトル) を介してポーズの速度と加速度を取得するにはどうすればよいですか?

Question

私は現在、solvePnP() を介して apriltag オブジェクトのポーズを取得し、projectPoints() を使用してポイントを投影しています。

これは videoStream で呼び出されるため、solvePnP() を最適化するために、前のポーズ (前のフレームのオブジェクトのポーズ) を取得し、そのポーズを現在のフレームの solvePnP() に渡します。

コードは次のとおりです。

# This function takes the image frame, and previous rotation and translation vectors as params: img, pvecs, tvecs

# If 1 or more apriltags are detected
if num_detections > 0:
    # If the camera was calibrated and the matrix is supplied
    if mtx is not None:
        # Image points are the corners of the apriltag
        imagePoints = detection_results[0].corners.reshape(1,4,2) 
        
        # objectPoints are obtained within another function

        # If pose is None, call solvePnP() without Guessing extrinsics
        if [x for x in (prvecs, ptvecs) if x is None]:
            success, prvecs, ptvecs = cv2.solvePnP(objectPoints, imagePoints, mtx, dist, flags=cv2.SOLVEPNP_ITERATIVE)
        else:
        # Else call solvePnP() with predefined rvecs and tvecs
            print("Got prvecs and tvecs")
            success, prvecs, ptvecs = cv2.solvePnP(objectPoints, imagePoints, mtx, dist, prvecs, ptvecs, True, flags=cv2.SOLVEPNP_ITERATIVE)

        # If the pose is obtained successfully, the project the 3D points 
        if success:
            imgpts, jac = cv2.projectPoints(opointsArr, prvecs, ptvecs, mtx, dist)
      
            # Draw the 3D points onto image plane
            draw_contours(img, dimg, imgpts)

動画ストリーミング機能内:

# Create a cv2 window to show images
window = 'Camera'
cv2.namedWindow(window)

# Open the first camera to get the video stream and the first frame
cap = cv2.VideoCapture(0)
success, frame = cap.read()

if dist is not None:
    frame = undistort_frame(frame)

prvecs = None
ptvecs = None
# Obtain previous translation and rotation vectors (pose)
img, dimg, prvecs, ptvecs = apriltag_real_time_pose_estimation(frame, prvecs, ptvecs)

while True:

    success, frame = cap.read()
    if dist is not None:
        frame = undistort_frame(frame)

    if not success:
        break
    
    # Keep on passing the pose obtained from the previous frame
    img, dimg, prvecs, ptvecs = apriltag_real_time_pose_estimation(frame, prvecs, ptvecs)

ポーズの速度と加速度を取得し、それを solvePnP() に渡したいと思います。

ポーズの速度については、現在の並進ベクトルから前の並進ベクトルを差し引くだけでよいことはわかっていますが、回転行列 (Rodrigues() を介して取得し、加速度を取得する) についてはどうすればよいかわかりません。 . 2 つの回転行列の間の角度を取得する必要があるかもしれないと考えています, そしてその差は変化を与え、したがって角速度を与えるでしょうか? または、solvePnP() から回転ベクトル間の差を見つけるのと同じくらい簡単です. ?

私の質問は次のとおりです。

1. 回転行列の観点からポーズ間の速度を取得するにはどうすればよいですか?現在の移動ベクトルから前の移動ベクトルを差し引くだけの方法は、移動速度を取得するという点で正しいですか?
2. 並進ベクトルと回転行列の加速度を取得するにはどうすればよいですか?
3. 前のポーズを取得するために使用している方法は最善の方法ですか?
4. 加速度については、速度の変化だけなので、フレーム間で取得した速度を追跡し、差を取得して加速度を取得するのが賢明でしょうか?

どんな助けでも大歓迎です！