如何通过从 solvePnP() 获得的姿势旋转矩阵和平移向量获得姿势速度和加速度？

问题描述

我目前正在通过 solvePnP() 获取 apriltag 对象的姿势并使用 projectPoints() 投影点

这是在视频流上调用的，因此为了尝试优化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,ptvecs,True,flags=cv2.soLVEPNP_IteraTIVE)

        # If the pose is obtained successfully,the project the 3D points 
        if success:
            imgpts,jac = cv2.projectPoints(opointsArr,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,ptvecs = apriltag_real_time_pose_estimation(frame,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,ptvecs)

我现在想获得姿势的速度和加速度，并将其传递给solvePnP()。

对于姿势速度，我知道我只需要从当前平移向量中减去先前的平移向量，但我不确定如何处理旋转矩阵（通过 Rodrigues() 获得，以及获得加速度。我在想，也许我必须得到两个旋转矩阵之间的角度，而差异会产生变化，从而导致角速度？或者就像从以下位置找到旋转向量之间的差异一样简单solvePnP()?

我的问题是：

1. 如何根据旋转矩阵获得姿势之间的速度，从当前的平移向量中减去前一个平移向量的方法在获得平移速度方面是否正确？
2. 如何获得平移向量和旋转矩阵的加速度？
3. 我用来获得先前姿势的方法是最好的方法吗？
4. 对于加速度，既然它只是速度的变化，那么只跟踪帧之间获得的速度并获取差异以获得加速度是否明智？

任何帮助将不胜感激！

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）