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

上回说到，我们回顾了机器视觉的基础知识，然后一顿操作之后得到了相机内参A和畸变系数dist. 现在我们开始定位目标的坐标。

这个过程包括几个子部分：
1.    标记检测
2.    指定世界坐标系，求相机外参
3.    确定目标的坐标

相机姿态估计

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 aruco





def 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=markerIDs
    print('ids:\n')
    print(ids)
    print('corners:\n')
    print(corners)

    objectPoints=[]
    imagePoints=[]
    #检查是否探测到了所有预期的基准marker
    if len(ids) !=0: #檢測到了marker,存儲marker的世界坐標到objectPoints，構建對應的圖像平面坐標列表 imagePoints
        print('------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))
        pass
    else:
        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, tvec
    else:
        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==[]:
        return
    targets_count=len(targetMarker)
    if targets_count == 0:
        raise Exception('targets empty, areyou dou?')

    #創建與targetMarker相同尺寸的列表,用於存儲解算所得到目標的世界坐標
    targetsWorldPoint=[None] * targets_count

    for i in range(len(markerIDs)): #遍歷探測到的marker ID,
        markerIDThisIterate = markerIDs[i][0]
        if markerIDThisIterate in targetMarker: #如果是目標marker的ID
            #獲得當前處理的marker在targetMarker中的下標,用於填充targetsWorldPoint
            targetIndex = 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-2d
        markerCenterIdeal=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,y
        delta = originWorldCoodinate-markerWorldCoodinate
        #zWorld = 0.0
        xWorld = (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]) )
        pass
    return targetsWorldPoint





if __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 ID
    targetMarker =[10,11]

    #1. 估計camera pose 
    #1.1 detect aruco markers
    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)
    aruco.drawDetectedMarkers(img_gray, corners) #Draw A square around the markers
    cv2.namedWindow('detect',cv2.WINDOW_NORMAL)
    cv2.resizeWindow('detect', 1280,720)
    cv2.imshow("detect",img_gray)
    #1.2 estimate camera pose
    gotCameraPose, rMatrixTemp, tvecTemp = estimateCameraPose(mtx, dist, refMarkerArray,corners,ids)

    #1.3 updata R, T to static value 
    if gotCameraPose: 
        rMatrix = rMatrixTemp
        tvec = tvecTemp
        print('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()