相机姿态估计

Aruco Marker是一种特殊的二维码，来源于美国中央俄克拉荷马大学(UCO)，并且为他配套开发了适用openCV的库，现在已经广泛用于机器视觉中的姿态估计。下图展示了arucomarker的样子。

ArucoMarker有不同的规格，称之为字典，例如DICT_6X6_250字典集中的marker维度是6*6, 能表示250个marker.

Maker 编码的时候考虑到了方向性，因此无论如何摆放，都可以区分四个不同的角。左上角的序号记为0，顺时针依次排列。网站http://chev.me/arucogen/可以在线生成marker。我打印了4x4，编号分别是0,1,2,3的marker放在地板上定位世界坐标系；打印了4x4, 编号为10, 11的marker 贴在遥控车上作为目标识别。

拍到的图像是这样的。

地面上的四个marker作为世界坐标系中的基准，测量他们之间的相对位置之后，用字典变量指定它们的坐标

refMarkerArray={ \

0: [4.0, 6.0, 0.0], \

1: [4.0, 2.0, 0.0], \

2: [2.0, 2.0, 0.0], \

3: [2.0, 6.0, 0.0], \

}

使用aruco.detectMarkers()函数可以检测到marker，返回ID和标志板的4个角点坐标。

    img_gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)parameters = aruco.DetectorParameters_create()corners, ids, rejectedImgPoints = aruco.detectMarkers(img_gray, aruco_dict, parameters=parameters)

这样，有了一组物理世界中的3D点坐标和图像中的2D点坐标，又由于相机内参已知，可以求解下列公式中的R和T，也就是从世界坐标系到相机坐标系的变换关系。

确定一个平面至少需要3个点，所以本文在地板上放置了4 个基准点。

Opencv中的cv2.solvePnP()函数可以根据一组3D-2D坐标，求解相机的姿态R和T.

cv2.solvePnP()没有直接返回旋转矩阵R，而是旋转向量rvec, 这两者直接是等价的，只是表示方法不同，cv2.Rodrigues()负责在两种格式之间转换。

        retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMtx, dist)rMatrix,jacobian = cv2.Rodrigues(rvec)

至此就得到了相机的外部参数，也就是从世界坐标系到相机坐标系的转换关系R和T.

目标测量

目标顶面也用aruco marker标记，这里使用的是4*4，ID=10、11的marker.
同样使用aruco.detectMarkers()检测小车目标marker的4个角点在图像中的坐标，求4个点的均值算得marker中心点的坐标(u,v)
接下来的任务是通过图像上的2D点计算相机坐标系中的3D点。实际上图像上的2维点对应了空间中一系列的三维点，因为深度z轴的信息是不知道的。
根据成像模型，相机坐标系中的3维点可以这样算得，记住，除了使用到成像模型外，还需要考虑镜头的畸变。
// (u,v) is the input point, (u', v') is the output point
// camera_matrix=[fx 0 cx; 0 fy cy; 0 0 1]
// P=[fx' 0 cx' tx; 0 fy' cy' ty; 0 0 1 tz]
x" = (u - cx)/fx
y" = (v - cy)/fy
(x',y') = undistort(x",y",dist_coeffs)

然而我们无需自己动手计算，因为Opencv中的undistortPoints()函数实现了上述过程，给小车目标在图像上的像素坐标markerCenter, 如下方式调用可以得到修正了畸变之后，在相机坐标系中，z轴归一化为1的坐标点(x’,y’).

markerCenterIdeal=cv2.undistortPoints(markerCenter.reshape([1,-1,2]),cameraMatrix,dist)

补上z=1，得到相机坐标系中的点P1=(x’, y’, 1).
P1与相机成像点的中心点 (也就是相机坐标系原点)P0=(0,0,0)确定了一条射线，而小车目标就位于这条射线上。计算这条射线与地板平面的交点，就能确定目标的坐标。
首先将P1，P0转换到世界坐标系。根据两个坐标系之间的转换关系，

，即

将P1、P0转换到世界坐标系.
这两个点确定的直线表示为：

在地面平面中，z=0，带入上式即可计算得到目标在运动平面上的位置（x,y,z=0）
下图展示了平面目标定位的原理。

后记

为什么不用aruco直接测量目标姿态？
阅读aruco库的文档，发现cv::aruco::estimatePoseSingleMarkers()函数提供了目标姿态测量的功能，可以直接返回marker到相机的姿态，用旋转向量rvec和平移向量tvec来表示，因此在marker标志板中的中心点Pm=(0,0,0)可以换算到相机坐标系中的坐标Pc=PmR+t=t，然后再将该点从相机坐标系换算到世界坐标系，最终也得到了Pw. 为何不这么干呢？

原因归根结底在于测量精度。视觉测量中，目标从三维空间投影到图像中的二维像素空间，z轴的深度信息是丢失的。要从二维还原到三维，只能依靠目标在x、y轴上的先验知识恢复z轴信息。在标志板中，这个先验知识就是marker4个角点的相对位置关系。
但是，当marker距离相机较远时，图像上的成像面积越小，物理世界中的x、y信息在图像中变成可怜的几个像素：有可能是5个像素，也有可能是6个像素，这一个像素之差，就造成了z轴位置20%的误差。Aruco marker距离相机越近，成像面积越大，精度就越高。在本文这样的大空间中，依靠小车上那块小小的标志板，误差相当大。
但是，本文中用做世界坐标系基准点的4块标记板的间隔距离放得很大，然后再用solvePnP()解算相机姿态，相当于用了一块覆盖整个房间的Aruco Marker来做相机姿态估计，解决了成像面积小造成的误差。实际上aruco库中的cv::aruco::estimatePoseSingleMarkers()函数底层也是用solvePnP()来估计相机姿态的。
另外，目标的测量没用取用它的Z轴信息，而是计算得到了一条射线，这条射线表示的方向信息是绝对准确的。再通过射线与平面的交点确定目标位置，尽可能提高了测量精度。

附代码：

#!/usr/bin/ python
# -*- coding: utf-8 -*- # 使用視覺方法測量目標在世界坐標系中的坐標
# 首先估計相機姿態,然後測算目標marker中心點在世界坐標系中的位置.
# 使用方法:
# 1. 相機校準,
# 2. 在空間中放置4個以上的基準坐標點,在程序中給定這些點的信息,包括ID和世界坐標
# 3. 被測目標使用marker標記,在程序中給定這些點的markerID
# 4. 拍攝錄像,確保4個標志點在視野內.
# 5. 運行程序處理視頻幀
# CR@ Guofeng, mailto:gf@gfshen.cn
#
# ------版本歷史---
# ---V1.0
# ---2019年7月19日
#    初次編寫import numpy as np
import cv2
import cv2.aruco as arucodef estimateCameraPose(cameraMtx, dist, refMarkerArray,corners,markerIDs):'''根据基准点的marker，解算相机的旋转向量rvecs和平移向量tvecs，(solvePnP(）实现)并将rvecs转换为旋转矩阵输出(通过Rodrigues())输入：cameraMtx内参矩阵，dist畸变系数。当前处理的图像帧frame，用于定位世界坐标系的参考点refMarkerArray.  py字典类型,需要len(refMarkerArray)>=3, 格式：{ID:[X, Y, Z], ID:[X,Y,Z]..}corners, detectMarkers()函數的輸出markerIDs, detectMarkers()函數的輸出输出：旋转矩阵rMatrix, 平移向量tVecs'''marker_count = len(refMarkerArray)if marker_count<4: #标志板少于四个raise RuntimeError('at least 3 pair of points required when invoking solvePnP')corners=corners; ids=markerIDsprint('ids:\n')print(ids)print('corners:\n')print(corners)objectPoints=[]imagePoints=[]#检查是否探测到了所有预期的基准markerif len(ids) !=0: #檢測到了marker,存儲marker的世界坐標到objectPoints，構建對應的圖像平面坐標列表 imagePointsprint('------detected ref markers----')for i in range(len(ids)): #遍歷探測到的marker ID,if ids[i][0] in refMarkerArray: #如果是參考點的標志，提取基准点的图像坐标，用于构建solvePnP()的输入print('id:\n ' + str(ids[i][0]))print('cornors: \n '+ str(corners[i][0]))objectPoints.append(refMarkerArray[ ids[i][0] ])imagePoints.append(corners[i][0][0].tolist()) #提取marker的左上點objectPoints=np.array(objectPoints)imagePoints=np.array(imagePoints)print('------------------------------\n')print('objectPoints:\n'+str(objectPoints))print('imagePoints:\n'+str(imagePoints))passelse:return False, None, None#如果檢測到的基準參考點大於3個，可以解算相機的姿態啦if len(objectPoints)>=4:#至少需要4個點retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMtx, dist)rMatrix,jacobian = cv2.Rodrigues(rvec)return True, rMatrix, tvecelse:return False, None, None#返回值#return rMatrix=[], tVecs=[]def detectTarget(cameraMatrix, dist, rMatrix, tvec, targetMarker, corners, markerIDs,zWorld = 0.0):'''測算目標marker中心在世界坐標系中的位置輸入:輸出:與markerIDs長度相等的列表,包含位置確定的目標坐標,未檢測到填None,例如[None,[x2,y2,z2]]'''if rMatrix==[]:returntargets_count=len(targetMarker)if targets_count == 0:raise Exception('targets empty, areyou dou?')#創建與targetMarker相同尺寸的列表,用於存儲解算所得到目標的世界坐標targetsWorldPoint=[None] * targets_countfor i in range(len(markerIDs)): #遍歷探測到的marker ID,markerIDThisIterate = markerIDs[i][0]if markerIDThisIterate in targetMarker: #如果是目標marker的ID#獲得當前處理的marker在targetMarker中的下標,用於填充targetsWorldPointtargetIndex = targetMarker.index(markerIDThisIterate)else:continue#計算marker中心的圖像坐標markerCenter = corners[i][0].sum(0)/4.0#畸變較正,轉換到相機坐標系,得到(u,v,1)#https://stackoverflow.com/questions/39394785/opencv-get-3d-coordinates-from-2dmarkerCenterIdeal=cv2.undistortPoints(markerCenter.reshape([1,-1,2]),cameraMatrix,dist)markerCameraCoodinate=np.append(markerCenterIdeal[0][0],[1])print('++++++++markerCameraCoodinate')print(markerCameraCoodinate)#marker的坐標從相機轉換到世界坐標markerWorldCoodinate = np.linalg.inv(rMatrix).dot((markerCameraCoodinate-tvec.reshape(3)) )print('++++++++markerworldCoodinate')print(markerWorldCoodinate)#將相機的坐標原點轉換到世界坐標系originWorldCoodinate = np.linalg.inv(rMatrix).dot((np.array([0, 0, 0.0])-tvec.reshape(3)) )#兩點確定了一條直線 (x-x0)/(x0-x1) = (y-y0)/(y0-y1) = (z-z0)/(z0-z1) #當z=0時,算得x,ydelta = originWorldCoodinate-markerWorldCoodinate#zWorld = 0.0xWorld = (zWorld-originWorldCoodinate[2])/delta[2] * delta[0] + originWorldCoodinate[0]yWorld = (zWorld-originWorldCoodinate[2])/delta[2] * delta[1] + originWorldCoodinate[1]targetsWorldPoint[targetIndex]=[xWorld,yWorld,zWorld]print('-=-=-=\n Target Position '+ str(targetsWorldPoint[targetIndex]) )passreturn targetsWorldPointif __name__ == '__main__':frame = cv2.imread('./inputImage2.bmp')try:npzfile = np.load('./calibrateDataMi5.npz')mtx = npzfile['mtx']dist = npzfile['dist']except IOError:raise Exception('cant find calibration data, do that first')#保存基準點的信息,檢測到之後會更新.rMatrix=[]tvec=[]########處理視頻畫面 cv2.namedWindow('image',cv2.WINDOW_NORMAL)cv2.resizeWindow('image', 1280,720)cv2.imshow("image",frame)##process and measure target position#0.1. 指定基準點的marker ID和世界坐標# [[marker ID, X, Y, Z]..]refMarkerArray={   \0: [4.0, 6.0, 0.0], \1: [4.0, 2.0, 0.0], \2: [2.0, 2.0, 0.0], \3: [2.0, 6.0, 0.0], \}#0.2 指定目標的markr IDtargetMarker =[10,11]#1. 估計camera pose #1.1 detect aruco markersimg_gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)parameters = aruco.DetectorParameters_create()corners, ids, rejectedImgPoints = aruco.detectMarkers(img_gray, aruco_dict, parameters=parameters)aruco.drawDetectedMarkers(img_gray, corners) #Draw A square around the markerscv2.namedWindow('detect',cv2.WINDOW_NORMAL)cv2.resizeWindow('detect', 1280,720)cv2.imshow("detect",img_gray)#1.2 estimate camera posegotCameraPose, rMatrixTemp, tvecTemp = estimateCameraPose(mtx, dist, refMarkerArray,corners,ids)#1.3 updata R, T to static value if gotCameraPose: rMatrix = rMatrixTemptvec = tvecTempprint('rMatrix\n'+str(rMatrixTemp))print('tvec\n'+str(tvecTemp))#2. 根據目標的marker來計算世界坐標系坐標detectTarget(mtx, dist, rMatrix, tvec, targetMarker, corners, ids)'''if ( cv2.waitKey(10) & 0xFF ) == ord('q'):cap.release()cv2.destroyAllWindows()'''#cap.release()cv2.destroyAllWindows()

基于单目视觉的平面目标定位和坐标测量 (下) - 相机姿态估计和目标测量相关推荐

融合人体姿态估计和目标检测的学生课堂行为识别
融合人体姿态估计和目标检测的学生课堂行为识别_参考网摘要: 在課堂教学中 , 人工智能技术可以帮助实现学生行为分析自动化 , 让教师能够高效且直观地掌握学生学习行为投入的情况 , 为后续优化教学设计 ...
AAAI 2023 | 腾讯优图实验室16篇论文入选，含多标签分类、姿态估计、目标检测、HOI、小样本学习等研究方向...
关注并星标从此不迷路计算机视觉研究院转自腾讯优图近日,AAAI 2023(Association for the Advancement of Artificial Intelligence) ...
论文速递：AAAI 2023 | 优图16篇论文速览，含多标签分类、姿态估计、目标检测、HOI、小样本学习等研究方向
近日,AAAI 2023(Association for the Advancement of Artificial Intelligence)国际先进人工智能协会公布了录用结果,本届会议共有8777 ...
相机计算坐标公式_相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标...
关键词:相机位姿估计,单目尺寸测量,环境探知用途:基于相机的环境测量,SLAM,单目尺寸测量文章类型:原理说明.Demo展示 @Author:VShawn @Date:2016-11-28 @La ...
格物钛数据平台国内外经典开源数据汇总（自动驾驶、目标检测、人脸识别、人体姿态估计、文本检测、NLP、医疗）
本文整理了国内外经典的开源数据,包含了目标检测.自动驾驶.人脸识别.自然语言处理.文本检测.医疗等方向,具体如下. 一.自动驾驶领域数据集 KITTI数据集 KITTI数据集由德国卡尔斯鲁厄理工学院和 ...
基于直线的最小非线性SLAM相机姿态估计方法
Minimal Non-linear Camera Pose Estimation Method Using Lines for SLAM Applications 1. 介绍 2. 非线性优化方法 ...
Realsense与英伟达Jetson的结合体？这款专注3D目标定位识别的AI深度相机凭什么在国外大火？
编辑:OAK中国首发:oakchina.cn 喜欢的话,请多多
mpu9250姿态融合算法_基于投票方式的机器人装配姿态估计
作者:仲夏夜之星来源:公众号 @3D视觉工坊链接:基于投票方式的机器人装配姿态估计论文题目:<Voting-Based Pose Estimation for Robotic Assemb ...
姿态估计目标检测论文整理（1）
一.姿态估计 1. 现阶段人体姿态识别主流的通常有2个思路: Top-Down(自上而下)方法:将人体检测和关键点检测分离,在图像上首先进行人体检测,找到所有的人体框,对每个人体框图再使用关键点检测, ...

基于单目视觉的平面目标定位和坐标测量 (下) - 相机姿态估计和目标测量

相机姿态估计

目标测量

后记

基于单目视觉的平面目标定位和坐标测量 (下) - 相机姿态估计和目标测量相关推荐

最新文章

热门文章