根據(jù)相機(jī)位姿求指定點(diǎn)的世界坐標(biāo)及其python實(shí)現(xiàn)

Author	shaniadolphin
e-mail	349948204@qq.com

求解目的

??本文將展示位姿估計(jì)的一種應(yīng)用量瓜，即通過(guò)單目相機(jī)對(duì)環(huán)境進(jìn)行測(cè)量。簡(jiǎn)單來(lái)說(shuō)侥猩，本文的工作就是利用下面的兩幅圖榔至，在已知P1、P2欺劳、P3唧取、P4四點(diǎn)世界坐標(biāo)的情況下，計(jì)算出其它點(diǎn)的世界坐標(biāo)划提。

??如圖所示枫弟，一個(gè)標(biāo)準(zhǔn)的標(biāo)定板，標(biāo)定板每個(gè)格子的尺寸是30mm鹏往，通過(guò)標(biāo)定四周的4個(gè)點(diǎn)P1淡诗、P2、P3、P4韩容，旁邊有茶罐款违，有待求點(diǎn)為P5、P6群凶。

??這種應(yīng)用可以利用一個(gè)已經(jīng)尺寸物體插爹，通過(guò)兩張不同視角的照片求未知物體的尺寸。比如上圖中的通過(guò)已知的標(biāo)定板求茶罐的尺寸请梢。而在現(xiàn)實(shí)應(yīng)用中可以用這種方式來(lái)求車(chē)的尺寸赠尾，建筑的高度，貨物的體積等等毅弧。

求解原理

??如下圖气嫁，根據(jù)P1、P2够坐、P3寸宵、P4四點(diǎn)的空間坐標(biāo)，通過(guò)openCV的PNP函數(shù)元咙，可以計(jì)算出兩次拍照的相機(jī)位姿邓馒，從而進(jìn)一步計(jì)算出相機(jī)的坐標(biāo) $Oc_1$ 與 $Oc_2$ 。那么將 $Oc_1$ 與 $P$ 蛾坯， $Oc_2$ 與 $P$ 連成直線，獲得兩條直線方程 $A$ 和 $B$ 疏遏，組成方程組求解得到它們的交點(diǎn)脉课，即為待求目標(biāo)點(diǎn)的坐標(biāo)。

1. 求出 $P$ 點(diǎn)的相機(jī)坐標(biāo)系坐標(biāo) $P_c$

??關(guān)于P點(diǎn)如何從二維映射到三維财异，參考上圖倘零， $O_c$ 的坐標(biāo)通過(guò)解 $PNP$ 已經(jīng)求出，待求點(diǎn)P在圖像中的像素坐標(biāo)為 $(u,v)$ 戳寸。

??求出 $P$ 在相機(jī)坐標(biāo)系中的坐標(biāo) $P_c$ （也就是上圖中的 $P_c$ 點(diǎn)）呈驶。具體的轉(zhuǎn)換公式如下，式中 $f$ 為相機(jī)鏡頭的焦距 $(mm)$ 疫鹊，在本次實(shí)驗(yàn)中使用的是中一光學(xué)的35mm手動(dòng)鏡頭袖瞻。 $(u,v)$ 為點(diǎn)的像素坐標(biāo)，其余為相機(jī)內(nèi)參數(shù)拆吆。
$\begin{cases}x_c=(u-u_0)*f/f_x \\y_c=(v-v_0)*f/f_y \\z_c=f \end{cases}$
??輸入拍到的圖片的點(diǎn)聋迎，包括待求點(diǎn)的像素坐標(biāo)，示例如下：

    P11 = np.array([0, 0, 0])
    P12 = np.array([0, 300, 0])
    P13 = np.array([210, 0, 0])
    P14 = np.array([210, 300, 0])    
    p11 = np.array([1765, 725])
    p12 = np.array([3068, 1254])
    p13 = np.array([1249, 1430])
    p14 = np.array([2648, 2072]) 
    p4psolver1.Points3D[0] = np.array([P11,P12,P13,P14])
    p4psolver1.Points2D[0] = np.array([p11,p12,p13,p14])
    #p4psolver1.point2find = np.array([4149, 671])
    #p4psolver1.point2find = np.array([675, 835])
    p4psolver1.point2find = np.array([691, 336])

??讀取標(biāo)定文件中的相機(jī)內(nèi)參枣耀，代碼如下霉晕，在代碼里預(yù)設(shè)了相機(jī)的傳感器大小，筆者所用的D7000單反是DX畫(huà)幅的，根據(jù)可查到的資料牺堰，傳感器的規(guī)格為23.6mm*15.6mm拄轻。

??筆者用在本機(jī)的鏡頭是中一的35mm手動(dòng)定焦鏡頭。

    def getudistmap(self, filename):
        with open(filename, 'r',newline='') as csvfile:
            spamreader = csv.reader(csvfile, delimiter=',', quotechar='"')
            rows = [row for row in spamreader]
            self.cameraMatrix = np.zeros((3, 3))
            #Dt = np.zeros((4, 1))
            size_w = 23.6
            size_h = 15.6
            imageWidth = int(rows[0][1])
            imageHeight = int(rows[0][2])
            self.cameraMatrix[0][0] = rows[1][1]
            self.cameraMatrix[1][1] = rows[1][2]
            self.cameraMatrix[0][2] = rows[1][3]
            self.cameraMatrix[1][2] = rows[1][4]
            self.cameraMatrix[2][2] = 1
            print(len(rows[2]))
            if len(rows[2]) == 5:
                print('fisheye')
                self.distCoefs = np.zeros((4, 1))
                self.distCoefs[0][0] = rows[2][1]
                self.distCoefs[1][0] = rows[2][2]
                self.distCoefs[2][0] = rows[2][3]
                self.distCoefs[3][0] = rows[2][4]
                scaled_K = self.cameraMatrix * 0.8 # The values of K is to scale with image dimension.
                scaled_K[2][2] = 1.0 
                #newcameramtx = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.cameraMatrix, self.distCoefs, (imageWidth, imageHeight), np.eye(3), balance=0)
                #map1, map2 = cv2.fisheye.initUndistortRectifyMap(self.cameraMatrix, self.distCoefs, np.eye(3), newcameramtx, (imageWidth, imageHeight), cv2.CV_32FC1)                     
            else:
                print('normal')
                self.distCoefs = np.zeros((1, 5))
                self.distCoefs[0][0] = rows[2][1]
                self.distCoefs[0][1] = rows[2][2]
                self.distCoefs[0][2] = rows[2][3]
                self.distCoefs[0][3] = rows[2][4]
                self.distCoefs[0][4] = rows[2][5]
                #newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.cameraMatrix, self.distCoefs, (imageWidth, imageHeight), 1, (imageWidth, imageHeight))
                #map1, map2 = cv2.initUndistortRectifyMap(self.cameraMatrix, self.distCoefs, None, newcameramtx, (imageWidth, imageHeight), cv2.CV_32FC1)        
            print('dim = %d*%d'%(imageWidth, imageHeight))
            print('Kt = \n', self.cameraMatrix)
            #print('newcameramtx = \n', newcameramtx)
            print('Dt = \n', self.distCoefs)
            self.f = [self.cameraMatrix[0][0]*(size_w/imageWidth), self.cameraMatrix[1][1]*(size_h/imageHeight)]
            #self.f = [350, 350]
            print('f = \n', self.f)
            #print(map1,'\n',map2.T)
            return

??然后就可以將像素坐標(biāo)轉(zhuǎn)換到世界坐標(biāo)了：

    def WordFrame2ImageFrame(self, WorldPoints):
        pro_points, jacobian = cv2.projectPoints(WorldPoints, self.rvecs, self.tvecs, self.cameraMatrix, self.distCoefs)
        return pro_points

    def ImageFrame2CameraFrame(self, pixPoints):
        fx = self.cameraMatrix[0][0]
        u0 = self.cameraMatrix[0][2]
        fy = self.cameraMatrix[1][1]
        v0 = self.cameraMatrix[1][2]
        zc = (self.f[0]+self.f[1])/2
        xc = (pixPoints[0] - u0) * self.f[0] / fx  #f=fx*傳感器尺寸/分辨率
        yc = (pixPoints[1] - v0) * self.f[1] / fy
        point = np.array([xc,yc,zc])
        return point

2.求出P點(diǎn)在世界坐標(biāo)系中的方向向量

??通過(guò)以上運(yùn)算得到了 $P_c(x_c,y_c,z_c)$ 伟葫，但這個(gè)點(diǎn)坐標(biāo)是在相機(jī)坐標(biāo)系中的恨搓，需要進(jìn)一步求解 $P$ 點(diǎn)在世界坐標(biāo)系中對(duì)應(yīng)的坐標(biāo) $P_w(x_w,y_w,c_w)$ 。

??為了將 $P_c$ 轉(zhuǎn)為 $P_w$ 扒俯，即求出原點(diǎn) $O_w$ 在相機(jī)坐標(biāo)系下的坐標(biāo)奶卓，需要使用到解 $PNP$ 求位姿時(shí)得到的三個(gè)歐拉角 $\theta_x,\theta_y,\theta_z$ 。相機(jī)坐標(biāo)系 $C$ 按照 $z$ 軸撼玄、 $y$ 軸夺姑、 $x$ 軸的順序旋轉(zhuǎn)以上角度后將與世界坐標(biāo)系 $W$ 完全平行，在這三次旋轉(zhuǎn)中 $P_c$ 也會(huì)與坐標(biāo)系一起旋轉(zhuǎn)的掌猛，其在世界系 $W$ 中的位置會(huì)發(fā)生改變盏浙。為了保證 $C$ 系旋轉(zhuǎn)后 $P$ 點(diǎn)依然保持在世界坐標(biāo)系W原本的位置，需要對(duì) $P_c$ 進(jìn)行三次反向旋轉(zhuǎn)，旋轉(zhuǎn)后得到點(diǎn) $P_c$ 在相機(jī)坐標(biāo)系 $C$ 中新的坐標(biāo)值記為 $P_c^{'}$ ， $P_c^{'}$ 的值等于世界坐標(biāo)系中向量 $OP$ 的值音五。最終匆篓， $P$ 點(diǎn)的世界坐標(biāo) $P_w$ = $P_c^{'}$ 的值+ $Oc$ 的世界坐標(biāo)值，具體操作如下：

第一次旋轉(zhuǎn)：

??原始相機(jī)坐標(biāo)系 $C$ 繞 $z$ 軸旋轉(zhuǎn)了 $\theta_z$ 變?yōu)?img class="math-inline" src="https://math.jianshu.com/math?formula=C_1" alt="C_1" mathimg="1">系澎迎， $P_0=(x_0,y_0,z_0)$ ，將 $P$ 點(diǎn)繞 $z$ 軸旋轉(zhuǎn) $-\theta_z$ ，得到 $P_1=(x_1,y_1,z_1)$ 灌闺，其為 $C_1$ 系中 $O_w$ 的坐標(biāo)。

    def CodeRotateByZ(self, x,  y,  thetaz):#將空間點(diǎn)繞Z軸旋轉(zhuǎn)
        x1=x #將變量拷貝一次坏瞄，保證&x == &outx這種情況下也能計(jì)算正確
        y1=y
        rz = thetaz*3.141592653589793/180
        outx = math.cos(rz)*x1 - math.sin(rz)*y1
        outy = math.sin(rz)*x1 + math.cos(rz)*y1
        return outx,outy

第二次旋轉(zhuǎn)：

?? $C_1$ 繞 $y$ 軸旋轉(zhuǎn)了 $\theta_y$ 變?yōu)?img class="math-inline" src="https://math.jianshu.com/math?formula=C_2" alt="C_2" mathimg="1">系桂对， $P_1=(x_1,y_1,z_1)$ ，將 $P_1$ 點(diǎn)繞 $y$ 軸旋轉(zhuǎn) $-\theta_y$ 鸠匀，得到 $P_2=(x_2,y_2,z_2)$ 蕉斜，其為 $C_2$ 系中 $O_w$ 的坐標(biāo)。

    def CodeRotateByY(self, x, z, thetay):
        x1=x
        z1=z
        ry = thetay * 3.141592653589793 / 180
        outx = math.cos(ry) * x1 + math.sin(ry) * z1
        outz = math.cos(ry) * z1 - math.sin(ry) * x1
        return outx,outz

第三次旋轉(zhuǎn)：

?? $C_2$ 繞 $x$ 軸旋轉(zhuǎn)了 $\theta_x$ 變?yōu)?img class="math-inline" src="https://math.jianshu.com/math?formula=C_3" alt="C_3" mathimg="1">系缀棍， $P_2=(x_2,y_2,z_2)$ 宅此，將 $P_2$ 點(diǎn)繞 $x$ 軸旋轉(zhuǎn) $-\theta_x$ ，得到 $P_3=(x_3,y_3,z_3)$ 爬范，其為 $C_3$ 系中 $O_w$ 的坐標(biāo)诽凌。

    def CodeRotateByX(self, y, z, thetax):
        y1=y
        z1=z
        rx = (thetax * 3.141592653589793) / 180
        outy = math.cos(rx) * y1 - math.sin(rx) * z1
        outz = math.cos(rx) * z1 + math.sin(rx) * y1
        return outy,outz

??此時(shí)，世界坐標(biāo)系中坦敌，相機(jī)的位置坐標(biāo)為 $(-x_3,-y_3,-z_3)$ 侣诵。結(jié)合上面的旋轉(zhuǎn)函數(shù)痢法，完整的求解代碼如下所示：

    def solver(self):
        retval, self.rvec, self.tvec = cv2.solvePnP(self.Points3D, self.Points2D, self.cameraMatrix, self.distCoefs)
        #print('self.rvec:',self.rvec)
        #print('self.tvec:',self.tvec)
        thetax,thetay,thetaz = self.rotationVectorToEulerAngles(self.rvec, 0)
        x = self.tvec[0][0]
        y = self.tvec[1][0]
        z = self.tvec[2][0]
        self.Position_OwInCx = x
        self.Position_OwInCy = y
        self.Position_OwInCz = z
        self.Position_theta = [thetax, thetay, thetaz]
        #print('Position_theta:',self.Position_theta)
        x, y = self.CodeRotateByZ(x, y, -1 * thetaz)
        x, z = self.CodeRotateByY(x, z, -1 * thetay)
        y, z = self.CodeRotateByX(y, z, -1 * thetax)
        self.Theta_W2C = (-1*thetax, -1*thetay,-1*thetaz)
        self.Position_OcInWx = x*(-1)
        self.Position_OcInWy = y*(-1)
        self.Position_OcInWz = z*(-1)
        self.Position_OcInW = np.array([self.Position_OcInWx, self.Position_OcInWy, self.Position_OcInWz])
        print('Position_OcInW:', self.Position_OcInW)

??通過(guò)世界坐標(biāo)系的相機(jī)坐標(biāo)a1，P點(diǎn)坐標(biāo)a2杜顺，構(gòu)成第一條直線A财搁。

    def SetLineA(self, A1x, A1y, A1z, A2x, A2y, A2z):
        self.a1 = np.array([A1x, A1y, A1z]) 
        self.a2 = np.array([A2x, A2y, A2z])

3. 最后，根據(jù)兩幅圖得到的兩條直線躬络，計(jì)算出P點(diǎn)的世界坐標(biāo)

??對(duì)另外一幅圖也進(jìn)行如上操作尖奔，獲得兩條直線A、B穷当，因此求出兩條直線A與B的交點(diǎn)即可求出目標(biāo)點(diǎn)的世界坐標(biāo)提茁。然而在現(xiàn)實(shí)中，由于誤差的存在馁菜，A與B基本不會(huì)相交茴扁，因此在計(jì)算時(shí)需要求他們之間的最近點(diǎn)。

class GetDistanceOf2linesIn3D():
    def __init__(self):
        print('GetDistanceOf2linesIn3D class')

    def dot(self, ax, ay, az, bx, by, bz):
        result = ax*bx + ay*by + az*bz
        return result

    def cross(self, ax, ay, az, bx, by, bz):
        x = ay*bz - az*by
        y = az*bx - ax*bz
        z = ax*by - ay*bx
        return x,y,z

    def crossarray(self, a, b):
        x = a[1]*b[2] - a[2]*b[1]
        y = a[2]*b[0] - a[0]*b[2]
        z = a[0]*b[1] - a[1]*b[0]
        return np.array([x,y,z])

    def norm(self, ax, ay, az):
        return math.sqrt(self.dot(ax, ay, az, ax, ay, az))

    def norm2(self, one):
        return math.sqrt(np.dot(one, one))


    def SetLineA(self, A1x, A1y, A1z, A2x, A2y, A2z):
        self.a1 = np.array([A1x, A1y, A1z]) 
        self.a2 = np.array([A2x, A2y, A2z])

    def SetLineB(self, B1x, B1y, B1z, B2x, B2y, B2z):
        self.b1 = np.array([B1x, B1y, B1z])    
        self.b2 = np.array([B2x, B2y, B2z])

    def GetDistance(self):
        d1 = self.a2 - self.a1
        d2 = self.b2 - self.b1
        e = self.b1 - self.a1

        cross_e_d2 = self.crossarray(e,d2)
        cross_e_d1 = self.crossarray(e,d1)
        cross_d1_d2 = self.crossarray(d1,d2)

        dd = self.norm2(cross_d1_d2)
        t1 = np.dot(cross_e_d2, cross_d1_d2)
        t2 = np.dot(cross_e_d1, cross_d1_d2)

        t1 = t1/(dd*dd)
        t2 = t2/(dd*dd)

        self.PonA = self.a1 + (self.a2 - self.a1) * t1
        self.PonB = self.b1 + (self.b2 - self.b1) * t2

        self.distance = self.norm2(self.PonB - self.PonA)
        print('distance=', self.distance)
        return self.distance

總結(jié)與驗(yàn)證

??通過(guò)以上的講解汪疮，說(shuō)明了大致的原理和過(guò)程峭火。完整的求解代碼及結(jié)果如下，其中代碼中打開(kāi)的“calibration.csv”是一個(gè)標(biāo)定生成的文件智嚷，存取了筆者D7000標(biāo)定后得到的內(nèi)參卖丸，如表格清單所示。

??表格清單：

dim	3696	2448
cameraMatrix	5546.18009098042	5572.883	1821.049	1347.461
distCoefs	-0.12735	0.200792	-0.00209	0.000943	-0.79439

??代碼清單：

#!/usr/bin/env python3
# coding:utf-8
import cv2
import numpy as np
import time
from PIL import Image,ImageTk
import threading
import os
import re    
import subprocess
import random
import math
import csv
import argparse

class PNPSolver(): 
    def __init__(self):
        self.COLOR_WHITE = (255,255,255)
        self.COLOR_BLUE = (255,0,0)
        self.COLOR_LBLUE = (255, 200, 100)
        self.COLOR_GREEN = (0,240,0)
        self.COLOR_RED = (0,0,255)
        self.COLOR_DRED = (0,0,139)
        self.COLOR_YELLOW = (29,227,245)
        self.COLOR_PURPLE = (224,27,217)
        self.COLOR_GRAY = (127,127,127)  
        self.Points3D = np.zeros((1, 4, 3), np.float32)  #存放4組世界坐標(biāo)位置
        self.Points2D = np.zeros((1, 4, 2), np.float32)   #存放4組像素坐標(biāo)位置
        self.point2find = np.zeros((1, 2), np.float32)
        self.cameraMatrix = None
        self.distCoefs = None
        self.f = 0

    def rotationVectorToEulerAngles(self, rvecs, anglestype):
        R = np.zeros((3, 3), dtype=np.float64)
        cv2.Rodrigues(rvecs, R)
        sy = math.sqrt(R[2,1] * R[2,1] +  R[2,2] * R[2,2])
        singular = sy < 1e-6
        if  not singular:
            x = math.atan2(R[2,1] , R[2,2])
            y = math.atan2(-R[2,0], sy)
            z = math.atan2(R[1,0], R[0,0])
        else :
            x = math.atan2(-R[1,2], R[1,1])
            y = math.atan2(-R[2,0], sy)
            z = 0
        if anglestype == 0:
            x = x*180.0/3.141592653589793
            y = y*180.0/3.141592653589793
            z = z*180.0/3.141592653589793
        elif anglestype == 1:
            x = x
            y = y
            z = z
        print(x)
        return x,y,z

    def CodeRotateByZ(self, x,  y,  thetaz):#將空間點(diǎn)繞Z軸旋轉(zhuǎn)
        x1=x #將變量拷貝一次盏道，保證&x == &outx這種情況下也能計(jì)算正確
        y1=y
        rz = thetaz*3.141592653589793/180
        outx = math.cos(rz)*x1 - math.sin(rz)*y1
        outy = math.sin(rz)*x1 + math.cos(rz)*y1
        return outx,outy

    def CodeRotateByY(self, x, z, thetay):
        x1=x
        z1=z
        ry = thetay * 3.141592653589793 / 180
        outx = math.cos(ry) * x1 + math.sin(ry) * z1
        outz = math.cos(ry) * z1 - math.sin(ry) * x1
        return outx,outz

    def CodeRotateByX(self, y, z, thetax):
        y1=y
        z1=z
        rx = (thetax * 3.141592653589793) / 180
        outy = math.cos(rx) * y1 - math.sin(rx) * z1
        outz = math.cos(rx) * z1 + math.sin(rx) * y1
        return outy,outz

    def solver(self):
        retval, self.rvec, self.tvec = cv2.solvePnP(self.Points3D, self.Points2D, self.cameraMatrix, self.distCoefs)
        thetax,thetay,thetaz = self.rotationVectorToEulerAngles(self.rvec, 0)
        x = self.tvec[0][0]
        y = self.tvec[1][0]
        z = self.tvec[2][0]
        self.Position_OwInCx = x
        self.Position_OwInCy = y
        self.Position_OwInCz = z
        self.Position_theta = [thetax, thetay, thetaz]
        #print('Position_theta:',self.Position_theta)
        x, y = self.CodeRotateByZ(x, y, -1 * thetaz)
        x, z = self.CodeRotateByY(x, z, -1 * thetay)
        y, z = self.CodeRotateByX(y, z, -1 * thetax)
        self.Theta_W2C = ([-1*thetax, -1*thetay,-1*thetaz])
        self.Position_OcInWx = x*(-1)
        self.Position_OcInWy = y*(-1)
        self.Position_OcInWz = z*(-1)
        self.Position_OcInW = np.array([self.Position_OcInWx, self.Position_OcInWy, self.Position_OcInWz])
        print('Position_OcInW:', self.Position_OcInW)

    def WordFrame2ImageFrame(self, WorldPoints):
        pro_points, jacobian = cv2.projectPoints(WorldPoints, self.rvecs, self.tvecs, self.cameraMatrix, self.distCoefs)
        return pro_points

    def ImageFrame2CameraFrame(self, pixPoints):
        fx = self.cameraMatrix[0][0]
        u0 = self.cameraMatrix[0][2]
        fy = self.cameraMatrix[1][1]
        v0 = self.cameraMatrix[1][2]
        zc = (self.f[0]+self.f[1])/2
        xc = (pixPoints[0] - u0) * self.f[0] / fx  #f=fx*傳感器尺寸/分辨率
        yc = (pixPoints[1] - v0) * self.f[1] / fy
        point = np.array([xc,yc,zc])
        return point

    def getudistmap(self, filename):
        with open(filename, 'r',newline='') as csvfile:
            spamreader = csv.reader(csvfile, delimiter=',', quotechar='"')
            rows = [row for row in spamreader]
            self.cameraMatrix = np.zeros((3, 3))
            #Dt = np.zeros((4, 1))
            size_w = 23.6
            size_h = 15.6
            imageWidth = int(rows[0][1])
            imageHeight = int(rows[0][2])
            self.cameraMatrix[0][0] = rows[1][1]
            self.cameraMatrix[1][1] = rows[1][2]
            self.cameraMatrix[0][2] = rows[1][3]
            self.cameraMatrix[1][2] = rows[1][4]
            self.cameraMatrix[2][2] = 1
            print(len(rows[2]))
            if len(rows[2]) == 5:
                print('fisheye')
                self.distCoefs = np.zeros((4, 1))
                self.distCoefs[0][0] = rows[2][1]
                self.distCoefs[1][0] = rows[2][2]
                self.distCoefs[2][0] = rows[2][3]
                self.distCoefs[3][0] = rows[2][4]
                scaled_K = self.cameraMatrix * 0.8 # The values of K is to scale with image dimension.
                scaled_K[2][2] = 1.0                   
            else:
                print('normal')
                self.distCoefs = np.zeros((1, 5))
                self.distCoefs[0][0] = rows[2][1]
                self.distCoefs[0][1] = rows[2][2]
                self.distCoefs[0][2] = rows[2][3]
                self.distCoefs[0][3] = rows[2][4]
                self.distCoefs[0][4] = rows[2][5]     
            print('dim = %d*%d'%(imageWidth, imageHeight))
            print('Kt = \n', self.cameraMatrix)
            print('Dt = \n', self.distCoefs)
            self.f = [self.cameraMatrix[0][0]*(size_w/imageWidth), self.cameraMatrix[1][1]*(size_h/imageHeight)]
            print('f = \n', self.f)
            return

class GetDistanceOf2linesIn3D():
    def __init__(self):
        print('GetDistanceOf2linesIn3D class')

    def dot(self, ax, ay, az, bx, by, bz):
        result = ax*bx + ay*by + az*bz
        return result

    def cross(self, ax, ay, az, bx, by, bz):
        x = ay*bz - az*by
        y = az*bx - ax*bz
        z = ax*by - ay*bx
        return x,y,z

    def crossarray(self, a, b):
        x = a[1]*b[2] - a[2]*b[1]
        y = a[2]*b[0] - a[0]*b[2]
        z = a[0]*b[1] - a[1]*b[0]
        return np.array([x,y,z])

    def norm(self, ax, ay, az):
        return math.sqrt(self.dot(ax, ay, az, ax, ay, az))

    def norm2(self, one):
        return math.sqrt(np.dot(one, one))


    def SetLineA(self, A1x, A1y, A1z, A2x, A2y, A2z):
        self.a1 = np.array([A1x, A1y, A1z]) 
        self.a2 = np.array([A2x, A2y, A2z])

    def SetLineB(self, B1x, B1y, B1z, B2x, B2y, B2z):
        self.b1 = np.array([B1x, B1y, B1z])    
        self.b2 = np.array([B2x, B2y, B2z])

    def GetDistance(self):
        d1 = self.a2 - self.a1
        d2 = self.b2 - self.b1
        e = self.b1 - self.a1

        cross_e_d2 = self.crossarray(e,d2)
        cross_e_d1 = self.crossarray(e,d1)
        cross_d1_d2 = self.crossarray(d1,d2)

        dd = self.norm2(cross_d1_d2)
        t1 = np.dot(cross_e_d2, cross_d1_d2)
        t2 = np.dot(cross_e_d1, cross_d1_d2)

        t1 = t1/(dd*dd)
        t2 = t2/(dd*dd)

        self.PonA = self.a1 + (self.a2 - self.a1) * t1
        self.PonB = self.b1 + (self.b2 - self.b1) * t2

        self.distance = self.norm2(self.PonB - self.PonA)
        print('distance=', self.distance)
        return self.distance


if __name__ == "__main__":
    print("***************************************")
    print("test example")
    print("***************************************")
    parser = argparse.ArgumentParser(description='test')
    parser.add_argument('-file', type=str, default = 'calibration.csv')
    args = parser.parse_args()
    calibrationfile = args.file

    p4psolver1 = PNPSolver()
    '''
    P11 = np.array([0, 0, 0])
    P12 = np.array([0, 200, 0])
    P13 = np.array([150, 0, 0])
    P14 = np.array([150, 200, 0])
    p11 = np.array([2985, 1688])
    p12 = np.array([5081, 1690])
    p13 = np.array([2997, 2797])
    p14 = np.array([5544, 2757])
    '''
    P11 = np.array([0, 0, 0])
    P12 = np.array([0, 300, 0])
    P13 = np.array([210, 0, 0])
    P14 = np.array([210, 300, 0])    
    p11 = np.array([1765, 725])
    p12 = np.array([3068, 1254])
    p13 = np.array([1249, 1430])
    p14 = np.array([2648, 2072]) 

    p4psolver1.Points3D[0] = np.array([P11,P12,P13,P14])
    p4psolver1.Points2D[0] = np.array([p11,p12,p13,p14])
    #p4psolver1.point2find = np.array([4149, 671])
    #p4psolver1.point2find = np.array([675, 835])
    p4psolver1.point2find = np.array([691, 336])
    p4psolver1.getudistmap(calibrationfile)
    p4psolver1.solver()

    p4psolver2 = PNPSolver()
    '''
    P21 = np.array([0, 0, 0])
    P22 = np.array([0, 200, 0])
    P23 = np.array([150, 0, 0])
    P24 = np.array([150, 200, 0])
    p21 = np.array([3062, 3073])
    p22 = np.array([3809, 3089])
    p23 = np.array([3035, 3208])
    p24 = np.array([3838, 3217])
    '''
    P21 = np.array([0, 0, 0])
    P22 = np.array([0, 300, 0])
    P23 = np.array([210, 0, 0])
    P24 = np.array([210, 300, 0])  
    p21 = np.array([1307, 790])
    p22 = np.array([2555, 797])
    p23 = np.array([1226, 1459])
    p24 = np.array([2620, 1470])

    p4psolver2.Points3D[0] = np.array([P21,P22,P23,P24])
    p4psolver2.Points2D[0] = np.array([p21,p22,p23,p24])
    #p4psolver2.point2find = np.array([3439, 2691])
    #p4psolver2.point2find = np.array([712, 1016])
    p4psolver2.point2find = np.array([453, 655])
    p4psolver2.getudistmap(calibrationfile)
    p4psolver2.solver()

    point2find1_CF = p4psolver1.ImageFrame2CameraFrame(p4psolver1.point2find)
    #Oc1P_x1 = point2find1_CF[0]
    #Oc1P_y1 = point2find1_CF[1]
    #Oc1P_z1 = point2find1_CF[2]
    Oc1P_1 = np.array(point2find1_CF)
    print(Oc1P_1)

    Oc1P_1[0], Oc1P_1[1] = p4psolver1.CodeRotateByZ(Oc1P_1[0], Oc1P_1[1], p4psolver1.Theta_W2C[2])
    Oc1P_1[0], Oc1P_1[2] = p4psolver1.CodeRotateByY(Oc1P_1[0], Oc1P_1[2], p4psolver1.Theta_W2C[1])
    Oc1P_1[1], Oc1P_1[2] = p4psolver1.CodeRotateByX(Oc1P_1[1], Oc1P_1[2], p4psolver1.Theta_W2C[0])

    a1 = np.array([p4psolver1.Position_OcInWx, p4psolver1.Position_OcInWy, p4psolver1.Position_OcInWz])
    a2 =  a1 + Oc1P_1
    #a2 = (p4psolver1.Position_OcInWx + Oc1P_1[0], p4psolver1.Position_OcInWy + Oc1P_y1, p4psolver1.Position_OcInWz + Oc1P_z1)


    point2find2_CF = p4psolver2.ImageFrame2CameraFrame(p4psolver2.point2find)
    #Oc2P_x2 = point2find2_CF[0]
    #Oc2P_y2 = point2find2_CF[1]
    #Oc2P_z2 = point2find2_CF[2]
    Oc2P_2 = np.array(point2find2_CF)
    print(Oc2P_2)

    Oc2P_2[0], Oc2P_2[1] = p4psolver2.CodeRotateByZ(Oc2P_2[0], Oc2P_2[1], p4psolver2.Theta_W2C[2])
    Oc2P_2[0], Oc2P_2[2] = p4psolver2.CodeRotateByY(Oc2P_2[0], Oc2P_2[2], p4psolver2.Theta_W2C[1])
    Oc2P_2[1], Oc2P_2[2] = p4psolver2.CodeRotateByX(Oc2P_2[1], Oc2P_2[2], p4psolver2.Theta_W2C[0])

    b1 = ([p4psolver2.Position_OcInWx, p4psolver2.Position_OcInWy, p4psolver2.Position_OcInWz])
    b2 = b1 + Oc2P_2
    #b2 = (p4psolver2.Position_OcInWx + Oc2P_x2, p4psolver2.Position_OcInWy + Oc2P_y2, p4psolver2.Position_OcInWz + Oc2P_z2)
    
    g = GetDistanceOf2linesIn3D()
    g.SetLineA(a1[0], a1[1], a1[2], a2[0], a2[1], a2[2])
    g.SetLineB(b1[0], b1[1], b1[2], b2[0], b2[1], b2[2])

    distance = g.GetDistance()

    pt = (g.PonA + g.PonB)/2

    print(pt)


    A = np.array([241.64926392,-78.7377477,166.08307887])
    B = np.array([141.010851,-146.64449841,167.28164652])
    print(math.sqrt(np.dot(A-B,A-B)))

??A點(diǎn)的世界坐標(biāo)點(diǎn)是：

distance= 0.13766177937900279
[241.64926392 -78.7377477  166.08307887]

??B點(diǎn)的世界坐標(biāo)點(diǎn)是：

distance= 0.7392672771306183
[ 141.010851   -146.64449841  167.28164652]

??計(jì)算AB點(diǎn)的距離：

    A = np.array([241.64926392,-78.7377477,166.08307887])
    B = np.array([141.010851,-146.64449841,167.28164652])
    print(math.sqrt(np.dot(A-B,A-B)))

??結(jié)果為：

121.41191667813395

??從數(shù)據(jù)可以看出茶罐的高度約為171mm(玻璃標(biāo)定板的高度為4mm)稍浆，對(duì)角的長(zhǎng)度約為121mm。

??也可以在生成世界坐標(biāo)數(shù)據(jù)的時(shí)候猜嘱，在Z軸數(shù)據(jù)中填入標(biāo)定板的高度粹湃，如下所示：

    P11 = np.array([0, 0, 4])
    P12 = np.array([0, 300, 4])
    P13 = np.array([210, 0, 4])
    P14 = np.array([210, 300, 4])  
    P21 = np.array([0, 0, 4])
    P22 = np.array([0, 300, 4])
    P23 = np.array([210, 0, 4])
    P24 = np.array([210, 300, 4])

??即可直接得到對(duì)應(yīng)的結(jié)果：

[ 141.010851   -146.64449841  171.28164652]
121.41191667813395

參考文檔

#	鏈接地址	文檔名稱
1	`https://www.cnblogs.com/singlex/p/pose_estimation_3.html`	根據(jù)兩幅圖像的位姿估計(jì)結(jié)果求某點(diǎn)的世界坐標(biāo)
2	`https://www.cnblogs.com/singlex/p/6037020.html`	子坐標(biāo)系C在父坐標(biāo)系W中的旋轉(zhuǎn)問(wèn)題

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市泉坐，隨后出現(xiàn)的幾起案子，更是在濱河造成了極大的恐慌裳仆，老刑警劉巖腕让，帶你破解...
沈念sama閱讀 206,214評(píng)論 6贊 481
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件，死亡現(xiàn)場(chǎng)離奇詭異歧斟，居然都是意外死亡纯丸，警方通過(guò)查閱死者的電腦和手機(jī)，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 88,307評(píng)論 2贊 382
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門(mén)静袖，熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)觉鼻，“玉大人，你說(shuō)我怎么就攤上這事队橙∽钩拢” “怎么了萨惑？”我有些...
開(kāi)封第一講書(shū)人閱讀 152,543評(píng)論 0贊 341
道士緝兇錄：失蹤的賣(mài)姜人
文/不壞的土叔我叫張陵，是天一觀的道長(zhǎng)仇矾。經(jīng)常有香客問(wèn)我庸蔼，道長(zhǎng)，這世上最難降的妖魔是什么贮匕？我笑而不...
開(kāi)封第一講書(shū)人閱讀 55,221評(píng)論 1贊 279
?港島之戀（遺憾婚禮）
正文為了忘掉前任姐仅，我火速辦了婚禮，結(jié)果婚禮上刻盐，老公的妹妹穿的比我還像新娘掏膏。我一直安慰自己，他們只是感情好敦锌，可當(dāng)我...
茶點(diǎn)故事閱讀 64,224評(píng)論 5贊 371
惡毒庶女頂嫁案：這布局不是一般人想出來(lái)的
文/花漫我一把揭開(kāi)白布馒疹。她就那樣靜靜地躺著，像睡著了一般供屉。火紅的嫁衣襯著肌膚如雪行冰。梳的紋絲不亂的頭發(fā)上，一...
開(kāi)封第一講書(shū)人閱讀 49,007評(píng)論 1贊 284
城市分裂傳說(shuō)
那天伶丐，我揣著相機(jī)與錄音悼做，去河邊找鬼。笑死哗魂，一個(gè)胖子當(dāng)著我的面吹牛肛走，可吹牛的內(nèi)容都是我干的。我是一名探鬼主播录别，決...
沈念sama閱讀 38,313評(píng)論 3贊 399
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開(kāi)眼朽色，長(zhǎng)吁一口氣：“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼！你這毒婦竟也來(lái)了组题？” 一聲冷哼從身側(cè)響起葫男，我...
開(kāi)封第一講書(shū)人閱讀 36,956評(píng)論 0贊 259
萬(wàn)榮殺人案實(shí)錄
序言：老撾萬(wàn)榮一對(duì)情侶失蹤，失蹤者是張志新（化名）和其女友劉穎崔列，沒(méi)想到半個(gè)月后梢褐，有當(dāng)?shù)厝嗽跇?shù)林里發(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 43,441評(píng)論 1贊 300
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡赵讯，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 35,925評(píng)論 2贊 323
?白月光啟示錄
正文我和宋清朗相戀三年盈咳，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片边翼。...
茶點(diǎn)故事閱讀 38,018評(píng)論 1贊 333
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡鱼响，死狀恐怖，靈堂內(nèi)的尸體忽然破棺而出组底，到底是詐尸還是另有隱情丈积，我是刑警寧澤筐骇，帶...
沈念sama閱讀 33,685評(píng)論 4贊 322
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布，位于F島的核電站桶癣，受9級(jí)特大地震影響拥褂，放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜牙寞，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 39,234評(píng)論 3贊 307
男人毒藥：我在死后第九天來(lái)索命
文/蒙蒙一饺鹃、第九天我趴在偏房一處隱蔽的房頂上張望。院中可真熱鬧间雀，春花似錦悔详、人聲如沸。這莊子的主人今日做“春日...
開(kāi)封第一講書(shū)人閱讀 30,240評(píng)論 0贊 19
一樁弒父案茄螃，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽(yáng)。三九已至连锯，卻和暖如春归苍，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背运怖。一陣腳步聲響...
開(kāi)封第一講書(shū)人閱讀 31,464評(píng)論 1贊 261
情欲美人皮
我被黑心中介騙來(lái)泰國(guó)打工拼弃，沒(méi)想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留，地道東北人摇展。一個(gè)月前我還...
沈念sama閱讀 45,467評(píng)論 2贊 352
代替公主和親
正文我出身青樓吻氧，卻偏偏與公主長(zhǎng)得像，于是被迫代替她去往敵國(guó)和親咏连。傳聞我的和親對(duì)象是個(gè)殘疾皇子盯孙，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 42,762評(píng)論 2贊 345