如何计算相机与物体的夹角

Question

我正在做一个应用程序来检测、解码、测量贴在墙上一定高度的二维码的距离和角度。在

为此，我使用了树莓相机模块附加到一个raspberry pib+、UV4L框架、python2.7、opencv2.3和Zbar。利用Zbar和OpenCV，我成功地检测、解码并测量了摄像机到检测到的QR码的距离。在

此外，UV4L还用于Opencv和Raspicam之间的兼容性。我使用的是UV4L框架包含的HTTP流服务器。我使用cv2.VideoCapture（）打开流，然后处理帧以查找二维码

在我成功地用Zbar检测到QR码后，我提取了QR码的四个角，并用简单的数学运算计算了从摄像机到QR码的距离

拉斯皮卡已经校准了，我得到了本质和变形矩阵。 以下是我掌握的数据：

1. 固有参数和畸变参数
2. 关于二维码的4个角的4个X，Y像素点
3. 从摄像机到二维码的距离（米）
4. 罗盘视野：54x41度
5. 相机分辨率：640x480

问题是我还没有找到一个正确的方法来计算相机和二维码之间的角度。在

任何帮助都将不胜感激，感谢并为我糟糕的英语道歉。在

这是我一直在使用的python代码。在

import cv2
import numpy as np
import zbar
import math
import time


#width of the object
qrwcoord = 0
qrPL = 0
qrNorm = 0
box = 0
focalLenght = 608 #Calibration data
intrinsic = np.array([[608.14578, 0, 292.31368], [0, 608.82584, 261.47420], [0, 0, 1]])  #3x3 Calibration intrinsic matrix
distortion = np.array([ [0.02232355, -0.11265588, 0.00247792, -0.00201052, 0.22786218]]) #1x5 Distortion vector

def distance_to_camera(knownWidth, focalLength, perWidth):
    return (knownWidth * focalLength) / perWidth

def decode_QR(image):
    global box

    gray = cv2.cvtColor(image,cv2.COLOR_RGB2GRAY)
    (height, width)=gray.shape
    raw = str(gray.data)
    scanner = zbar.ImageScanner()
    scanner.parse_config('enable')
    image1 = zbar.Image(width, height, 'Y800', raw)
    scanner.scan(image1)
    for symbol in image1:
        TLC, BLC, BRC, TRC = [item for item in symbol.location]
        box = np.array([[TLC],[TRC],[BRC],[BLC]])
        qrData=symbol.data
        qrStrip = []
        qrStrip = qrData.split(';')
        qrNorm = qrStrip[0] #Qr Code norm vector
        qrPL = qrStrip[1]
        qrwcoord = qrStrip[2] #QR Code World Coordinates
        qrPL=float(qrPL) #Qr Code size in meters

        print 'decoded', symbol.type, 'symbol', '"%s"' % symbol.data
        qrpixelwidth = math.sqrt((TLC[0]-TRC[0])**2+(TLC[1]-TRC[1])**2)
        qrpixelheight = math.sqrt((BLC[0]-BRC[0])**2+(BLC[1]-BRC[1])**2)

        pixel_size = float(qrPL/qrpixelwidth) #Pixel Size in meter
        plane_x_size = float(pixel_size*640)
        plane_y_size = float(pixel_size*480)

        dist = distance_to_camera(qrPL, focalLenght, qrpixelwidth)
        print qrNorm
        print qrPL
        print qrwcoord
        print box
        print qrpixelwidth
        print qrpixelheight
        print "Distance from QR to Camera is:", dist
        print qrStrip
        return box

cv2.namedWindow("Streaming...")
capture = cv2.VideoCapture(0)
capture.open('http://192.168.5.2:8080/stream/video.mjpeg')
while True:
    (grabbed, img) = capture.read()
    if not grabbed:
        break
    box = decode_QR(img)
    cv2.drawContours(img, [box], -1, (0, 255, 0), 2)
    cv2.imshow("Streaming...", img)
    #time.sleep(0.05)
    key = cv2.waitKey(1)& 0xFF
    if key == ord("q"):
        break
cv2.destroyWindow("Streaming...")