三天搞定双摄像头交叉立体动捕，第二天

第二天，我想起来aruco可以得到摄像机的相对姿态，但是查到的资料都是利用单个标记，没有使用整个charuco标定板的，虽然文档里有提到。

https://docs.opencv.org/4.8.0/df/d4a/tutorial_charuco_detection.html

OpenCV: ArUco marker detection (aruco module)

但是我所有的资料也只有这个文档，再没有其他。

这里其实使用单个标记也没问题，但是出于迷信和对精度的追求，我还是尽量用更多的标记。

凭借经验和大量资料拼凑出来的代码如下

import numpy as np
import time
import cv2
import cv2.aruco as aruco

cameraMatrix0=np.array([[1.29707448e+03, 0.00000000e+00, 5.10584630e+02],
 [0.00000000e+00, 1.30621569e+03, 1.04979705e+03],
 [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
distCoeffs0=np.array([[-0.41566624 , 0.20888931, -0.0021686 , -0.00091129, -0.05888535]])

#设置预定义的字典
aruco_dict = aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
parameters =  aruco.DetectorParameters()
board = aruco.CharucoBoard((5, 7), .04, .02, aruco_dict)#0.025单位是米

cap0 = cv2.VideoCapture(0)  # 0表示默认摄像头
cap0.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
cap0.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)


while True:
    #读取图片
    _,frame=cap0.read()
    frame = cv2.transpose(frame)
    frame = cv2.flip(frame, 1)
    #调整图片大小
    #frame=cv2.resize(frame,None,fx=0.2,fy=0.2,interpolation=cv2.INTER_CUBIC)
    #灰度话
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    #使用aruco.detectMarkers()函数可以检测到marker，返回ID和标志板的4个角点坐标
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)
    if len(corners) > 6:
        #画出标志位置
        #aruco.drawDetectedMarkers(frame, corners,ids)
        ret,charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, frame, board)
        #ret, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, frame, board, cameraMatrix, distCoeffs);
        if charuco_corners is not None:
            frame=aruco.drawDetectedCornersCharuco(frame, charuco_corners, charuco_ids);
            rvec=np.array([.0,.0,.0])
            tvec=np.array([.0,.0,.0])
            valid,rvec, tvec=aruco.estimatePoseCharucoBoard(charuco_corners, charuco_ids, board, cameraMatrix0, distCoeffs0,rvec,tvec);
            if valid:
                m=cv2.Rodrigues(rvec)
                cv2.drawFrameAxes(frame, cameraMatrix0, distCoeffs0, rvec, tvec, 0.1);
    cv2.imshow("frame",frame)
    cv2.waitKey(10)
cv2.destroyAllWindows()

得到rvec,tvec就是摄像机相对标定板的旋转和平移，旋转矢量可以转换为旋转矩阵。实际上两个向量可以合成一个4x4的投影矩阵。

R0 = np.zeros((4, 4))
R0[:3, :3] = cv2.Rodrigues(rvec0)[0]
R0[:3, 3] = tvec0
R0[3, 3] = 1

然后理论上

R2=R0*R1.inv()就可以得到两个摄像机的相对位置，而我在这里卡了很久，理论上摄像机的位姿是相对标定板的坐标系为原点，所以标定板移动摄像机的位姿也会移动，但是两个相机的相对姿态是不变的，可是我计算出的结果一直在变。我还查找了上面姿态估计函数的坐标系说明，没找到。而且资料里坐标系也不同，有z轴朝里的，也有朝外的，在图片上画的框也不同。

试了其他的计算组合，各种取逆，取转置，都不对

查了大量资料，最有价值的如下Kinect与Ir相机外参标定与深度对齐 - 知乎

一顿操作猛如虎，最后的结论就是我上面那个简短的公式，至少证实了自己的理论没问题。

最后的发现是在python里*和np.dot是不同的。R3=np.dot(R1, R0.T)就对了

第二天结束