利用ArUco码实现相机位姿估计（Python）

量大福大

环境：python3.7opencv-contrib-python4.5.1.48一、相机标定相机标定就是求相机的内参矩阵mtx和畸变系数dist首先需要利用相机（你需要标定的）拍摄标定板照片，20张左右。拍摄照片的时候各个角度都拍一些，可以变换标定板的位置，但不要大幅度变换相机（不要翻转镜头，上下颠倒相机等等）。拍摄的时候看一下照片的属性，这个分辨率需要和后边识别marker时的视频流分辨率保持一致，否侧会测距不准。（博主踩过的大坑这是）我标定的时候把重投影误差大于0.3个像素的删除了，这样标定的比较准一些。importnumpyasnpimportcv2importglobcap=cv2.VideoCapture(1)#设置视频流的分辨率为1280x720cap.set(cv2.CAP_PROP_FRAME_WIDTH,1280)cap.set(cv2.CAP_PROP_FRAME_HEIGHT,720)#设置用于相机标定的迭代算法的终止条件criteria=(cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001)#设置棋盘格的规格chessboard_size=(11,8)#棋盘格内角点数目#初始化对象点和图像点列表objpoints=[]#3Dpointsinrealworldspaceimgpoints=[]#2Dpointsinimageplane.#准备棋盘格对象点objp=np.zeros((chessboard_size[0]*chessboard_size[1],3),np.float32)objp[:,:2]=np.mgrid[0:chessboard_size[0],0:chessboard_size[1]].T.reshape(-1,2)#遍历所有的图像文件路径images=glob.glob('checkerboard1/*.jpg')#遍历所有的图像文件路径forfnameinimages:img=cv2.imread(fname)#读取当前图像文件gray=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)#将当前图像转换为灰度图#查找棋盘格角点ret,corners=cv2.findChessboardCorners(gray,chessboard_size,None)#如果找到棋盘格角点ifret:#将棋盘格角点添加到imgpoints列表中imgpoints.append(corners)#添加相应的3D对象点objpoints.append(objp)#使用对象点和图像点进行相机标定，得到相机的内参矩阵、畸变系数以及旋转矩阵和平移矩阵ret,mtx,dist,rvecs,tvecs=cv2.calibrateCamera(objpoints,imgpoints,gray.shape[::-1],None,None)#剔除重投影误差大于0.3的图像new_objpoints=[]new_imgpoints=[]foriinrange(len(objpoints)):imgpoints2,_=cv2.projectPoints(objpoints[i],rvecs[i],tvecs[i],mtx,dist)error=cv2.norm(imgpoints[i],imgpoints2,cv2.NORM_L2)/len(imgpoints2)iferror0:foriinrange(len(ids)):#遍历每个检测到的ArUco标记#估计ArUco标记的姿态（旋转向量和平移向量）rvec,tvec,_=aruco.estimatePoseSingleMarkers(corners[i],0.10,mtx,dist)R,_=cv2.Rodrigues(rvec)#将旋转向量（rvec）转换为旋转矩阵（R）3*3R_inv=np.transpose(R)#旋转矩阵是正交矩阵，所以它的逆矩阵等于它的转置R_inv表示从相机坐标系到标记坐标系的旋转。iftvec.shape!=(3,1):tvec_re=tvec.reshape(3,1)#print('-------tvec_re------')#print(tvec_re)tvec_inv=-R_inv@tvec_retvec_inv1=np.transpose(tvec_inv)pos=tvec_inv1[0]print(pos)distance=math.sqrt(pos[0]**2+pos[1]**2+pos[2]**2)rad=pos[0]/pos[2]angle_in_radians=math.atan(rad)angle_in_degrees=math.degrees(angle_in_radians)#绘制ArUco标记的坐标轴cv2.drawFrameAxes(frame,mtx,dist,rvec,tvec,0.05)tvec_inv_str="".join([f"{num:.2f}m"fornumintvec_inv1.flatten()])cv2.putText(frame,"tvec_inv:"+tvec_inv_str,(0,80),font,1,(0,255,0),2,cv2.LINE_AA)cv2.putText(frame,'distance:'+str(round(distance,4))+str('m'),(0,110),cv2.FONT_HERSHEY_SIMPLEX,1,(0,255,0),2,cv2.LINE_AA)cv2.putText(frame,"degree:"+str(angle_in_degrees),(0,150),font,1,(0,0,255),2,cv2.LINE_AA)#获取ArUco标记相对于相机坐标系的位置#pos_str_cam=f"X:{tvec_inv1[0][0][0]:.2f}m,Y:{tvec_inv1[0][0][1]:.2f}m,Z:{tvec[0][0][2]:.2f}m"#在图像上标注ArUco标记的相对于相机坐标系的位置信息#cv2.putText(frame,pos_str_cam,(int(corners[i][0][0][0]),int(corners[i][0][0][1])-10),#font,0.5,(0,255,0),1,cv2.LINE_AA)#在图像上绘制检测到的ArUco标记aruco.drawDetectedMarkers(frame,corners)#如果没有检测到ArUco标记，则在图像上显示"NoIds"else:cv2.putText(frame,"NoIds",(0,64),font,1,(0,255,0),2,cv2.LINE_AA)returnframe#返回处理后的图像12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061注意：aruco.estimatePoseSingleMarkers（）函数输出的rvec和tvec是marker的中点相对于相机坐标系的旋转向量和平移向量。而我们要求的是相机相对于marker坐标系的位姿。所以需要将rvec和tvec进行一下坐标系变换。如下： R,_=cv2.Rodrigues(rvec)#将旋转向量（rvec）转换为旋转矩阵（R）3*3R_inv=np.transpose(R)#旋转矩阵是正交矩阵，所以它的逆矩阵等于它的转置R_inv表示从相机坐标系到标记坐标系的旋转。iftvec.shape!=(3,1):tvec_re=tvec.reshape(3,1)#print('-------tvec_re------')#print(tvec_re)tvec_inv=-R_inv@tvec_retvec_inv1=np.transpose(tvec_inv)pos=tvec_inv1[0]print(pos)1234567891011whileTrue:ret,frame=cap.read()ifnotret:breakmtx=np.array([[711.77689507,0,672.53236606],[0,711.78573804,313.37884074],[0,0,1],])dist=np.array([1.26638295e-01,-1.16132908e-01,-2.24690373e-05,-2.25867957e-03,-6.76164003e-02])#调用estimate_pose函数对当前帧进行姿态估计和标记检测frame=estimate_pose(frame,mtx,dist)new_width=1280new_height=720frame=cv2.resize(frame,(new_width,new_height))cv2.imshow('frame',frame)#等待按键输入，如果按下键盘上的'q'键则退出循环ifcv2.waitKey(1)&0xFF==ord('q'):break#释放VideoCapture对象cap.release()cv2.destroyAllWindows()123456789101112131415161718192021222324三、效果