根據(jù)相機位姿求指定點的世界坐標及其python實現(xiàn)

Author shaniadolphin
e-mail 349948204@qq.com

求解目的

??本文將展示位姿估計的一種應用，即通過單目相機對環(huán)境進行測量。簡單來說，本文的工作就是利用下面的兩幅圖，在已知P1、P2、P3、P4四點世界坐標的情況下，計算出其它點的世界坐標。

??如圖所示，一個標準的標定板，標定板每個格子的尺寸是30mm，通過標定四周的4個點P1、P2、P3、P4，旁邊有茶罐，有待求點為P5、P6。

??這種應用可以利用一個已經(jīng)尺寸物體，通過兩張不同視角的照片求未知物體的尺寸。比如上圖中的通過已知的標定板求茶罐的尺寸。而在現(xiàn)實應用中可以用這種方式來求車的尺寸，建筑的高度，貨物的體積等等。

求解原理

??如下圖，根據(jù)P1、P2、P3、P4四點的空間坐標，通過openCV的PNP函數(shù)，可以計算出兩次拍照的相機位姿，從而進一步計算出相機的坐標Oc_1與Oc_2。那么將Oc_1與P，Oc_2與P連成直線，獲得兩條直線方程A和B，組成方程組求解得到它們的交點，即為待求目標點的坐標。

1. 求出P點的相機坐標系坐標P_c

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

??求出P在相機坐標系中的坐標P_c（也就是上圖中的P_c點）。具體的轉(zhuǎn)換公式如下，式中f為相機鏡頭的焦距(mm)，在本次實驗中使用的是中一光學的35mm手動鏡頭。(u,v)為點的像素坐標，其余為相機內(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}
??輸入拍到的圖片的點，包括待求點的像素坐標，示例如下：

    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])

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

??筆者用在本機的鏡頭是中一的35mm手動定焦鏡頭。

    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

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

    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點在世界坐標系中的方向向量

??通過以上運算得到了P_c(x_c,y_c,z_c)，但這個點坐標是在相機坐標系中的，需要進一步求解P點在世界坐標系中對應的坐標P_w(x_w,y_w,c_w)。

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

第一次旋轉(zhuǎn)：

??原始相機坐標系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點繞z軸旋轉(zhuǎn)-\theta_z，得到P_1=(x_1,y_1,z_1)，其為C_1系中O_w的坐標。

    def CodeRotateByZ(self, x,  y,  thetaz):#將空間點繞Z軸旋轉(zhuǎn)
        x1=x #將變量拷貝一次，保證&x == &outx這種情況下也能計算正確
        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點繞y軸旋轉(zhuǎn)-\theta_y，得到P_2=(x_2,y_2,z_2)，其為C_2系中O_w的坐標。

    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點繞x軸旋轉(zhuǎn)-\theta_x，得到P_3=(x_3,y_3,z_3)，其為C_3系中O_w的坐標。

    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

??此時，世界坐標系中，相機的位置坐標為(-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)

??通過世界坐標系的相機坐標a1，P點坐標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ù)兩幅圖得到的兩條直線，計算出P點的世界坐標

??對另外一幅圖也進行如上操作，獲得兩條直線A、B，因此求出兩條直線A與B的交點即可求出目標點的世界坐標。然而在現(xiàn)實中，由于誤差的存在，A與B基本不會相交，因此在計算時需要求他們之間的最近點。

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é)與驗證

??通過以上的講解，說明了大致的原理和過程。完整的求解代碼及結(jié)果如下，其中代碼中打開的“calibration.csv”是一個標定生成的文件，存取了筆者D7000標定后得到的內(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組世界坐標位置
        self.Points2D = np.zeros((1, 4, 2), np.float32)   #存放4組像素坐標位置
        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):#將空間點繞Z軸旋轉(zhuǎn)
        x1=x #將變量拷貝一次，保證&x == &outx這種情況下也能計算正確
        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點的世界坐標點是：

distance= 0.13766177937900279
[241.64926392 -78.7377477  166.08307887]

??B點的世界坐標點是：

distance= 0.7392672771306183
[ 141.010851   -146.64449841  167.28164652]

??計算AB點的距離：

    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(玻璃標定板的高度為4mm)，對角的長度約為121mm。

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

    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])

??即可直接得到對應的結(jié)果：

[ 141.010851   -146.64449841  171.28164652]
121.41191667813395

參考文檔

# 鏈接地址 文檔名稱
1 https://www.cnblogs.com/singlex/p/pose_estimation_3.html 根據(jù)兩幅圖像的位姿估計結(jié)果求某點的世界坐標
2 https://www.cnblogs.com/singlex/p/6037020.html 子坐標系C在父坐標系W中的旋轉(zhuǎn)問題
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成，瀏覽時請結(jié)合常識與多方信息審慎甄別。
平臺聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點，簡書系信息發(fā)布平臺，僅提供信息存儲服務(wù)。

相關(guān)閱讀更多精彩內(nèi)容

  • 2035年,我三十七歲了。那時的我已經(jīng)是一名優(yōu)秀的宇航員。 我駕駛著火星三號漫游太空…… “宇航員登艙!”我穿著那...
    Acy_笑笑笑閱讀 360評論 0贊 0
  • 來到一個城市，只為一個理想，以為通過努力就能后改變，從而過上自己想要的生活，再不濟可以選擇的工作還是很多。真正...
    礪劍必鋒閱讀 263評論 0贊 0
  • 引言:人生沒有白走的路，每一步都算數(shù)。 所謂歷練出來的成熟，就是原本可能該哭該鬧的你，卻選擇了不言不語地微笑。So...
    Elsa_Fu閱讀 557評論 0贊 1
  • 今天是周日，本來早應該完成作業(yè)日更的?？墒乾F(xiàn)在已經(jīng)是晚上10點多了，我還在為日更的主題，而犯愁，到底該寫些什么呢？...
    藍藍的天空77閱讀 223評論 0贊 1

友情鏈接更多精彩內(nèi)容