原文地址：https://tunmx.github.io/posts/CH-Camera-internal-and-external-parameters/

學(xué)習(xí)人臉3D重建的第一天，在首次接觸3D相關(guān)的內(nèi)容，必須要搞清楚相機(jī)的成像原理，如何將真實(shí)三維空間中的三維點(diǎn)與顯示器、屏幕和圖像等二維成像的平面映射，以及了解該過(guò)程的推導(dǎo)方式和相關(guān)坐標(biāo)系的換算，如像素坐標(biāo)，圖像坐標(biāo)，相機(jī)坐標(biāo)以及世界坐標(biāo)這四種關(guān)系的變換。

主要內(nèi)容從以下博主的文章整理，并結(jié)合自己的實(shí)驗(yàn)代碼進(jìn)行測(cè)試，推薦直接看原帖，無(wú)中間商賺差價(jià)：

https://www.cnblogs.com/wangguchangqing/p/8126333.html#autoid-0-5-0

一文帶你搞懂相機(jī)內(nèi)參外參(Intrinsics & Extrinsics) - Yanjie Ze的文章 - 知乎?

針孔模型

從圖中所示，我們可以清楚的看到兩種坐標(biāo)系：

• 相機(jī)坐標(biāo)系（3D）：以光心中心點(diǎn)為原點(diǎn)，建立三維坐標(biāo)系；

• 圖像坐標(biāo)系（2D）：以被投射的平面中為原點(diǎn)，建立二維坐標(biāo)系。

從圖所示，取真實(shí)世界中的任意一點(diǎn)通過(guò)相機(jī)的光心點(diǎn)映射到成像平面上的點(diǎn)，其中我們令點(diǎn)，則對(duì)應(yīng)到點(diǎn)，這邊比較特殊，將成像的平面與光點(diǎn)的距離記為f，即為像距，所以可以用以下圖表示坐標(biāo)系和映射點(diǎn)之間的關(guān)系：

通過(guò)上圖相似三角形關(guān)系可以得出以下關(guān)系式：

其中出現(xiàn)負(fù)號(hào)的原因是因?yàn)樽鴺?biāo)軸在映射過(guò)程中的成像為倒立所導(dǎo)致，為了表達(dá)方便，看到一個(gè)博主是這樣解釋和處理的：

為了表示起來(lái)更方便，我們把成像平面從相機(jī)的后面對(duì)稱到前面去，這樣，負(fù)號(hào)就沒(méi)有了。

一文帶你搞懂相機(jī)內(nèi)參外參(Intrinsics & Extrinsics) - Yanjie Ze的文章 - 知乎?

經(jīng)過(guò)轉(zhuǎn)換后的關(guān)系：

通現(xiàn)在我們把上面的關(guān)系式以解出點(diǎn)為目的進(jìn)行變形，可得：

上面便是整理好的小孔模型基本公式，通過(guò)這些公式我們可以進(jìn)一步的去推算利用該模型下求解相機(jī)的內(nèi)參和外參。

簡(jiǎn)單的描述一下相機(jī)內(nèi)參：相機(jī)內(nèi)參描述了相機(jī)本身自帶的一些屬性，如焦距、像素間距等；通常是用一個(gè)內(nèi)參矩陣K來(lái)表示，這個(gè)矩陣K用于描述從三維場(chǎng)景到二維場(chǎng)景的映射形狀和大小。

上一步我們求解出了小孔模型的基本公式，需要進(jìn)一步將所述的坐標(biāo)點(diǎn)映射到像素坐標(biāo)系中，像素坐標(biāo)定義通常如下：

• 像素坐標(biāo)系（2D）：通常以圖像的左上角為坐標(biāo)的原點(diǎn)，u軸從左到右與x軸平行，v軸從上到下與y軸平行；

我們?cè)O(shè)像素坐標(biāo)軸同時(shí)在u軸和v軸縮放了S的倍數(shù)，倍數(shù)定義為，即u軸縮放了α倍，v軸縮放了β倍；同時(shí)，原點(diǎn)坐標(biāo)也平移了C個(gè)像素點(diǎn)，即，在與上一步求解的點(diǎn)的坐標(biāo)關(guān)系如下：

將上一步以與關(guān)系得出的小孔模型公式代入可得：

我們令，，可得：

我們將方程組寫(xiě)成齊次坐標(biāo)的形式：

我們可以把z挪到左邊得到：

并且，令，，

可得簡(jiǎn)化的表達(dá)式：

通過(guò)上面的推導(dǎo)，我們已經(jīng)計(jì)算出了相機(jī)內(nèi)參(Camera Intrinsics)的內(nèi)參矩陣K，通常在實(shí)際項(xiàng)目中該內(nèi)參的得出方式通常需要標(biāo)定，標(biāo)定的方式后面等到具體實(shí)踐再進(jìn)行整理。

相機(jī)外參

簡(jiǎn)單的描述一下相機(jī)的外參：相機(jī)的外參主要描述的是相機(jī)在三維場(chǎng)景下的位置以及鏡頭朝向，通常以一個(gè)旋轉(zhuǎn)矩陣R和平移向量t進(jìn)行表示，描述了相機(jī)的位置、方向和觀察角度，決定了相機(jī)從哪個(gè)角度觀察場(chǎng)景。

在上面的推導(dǎo)過(guò)程中，我們已經(jīng)求出了坐標(biāo)系之間的關(guān)系以及內(nèi)參矩陣，其中內(nèi)參矩陣是通過(guò)相機(jī)坐標(biāo)系與像素坐標(biāo)之間的關(guān)系得出，所以我們這步需要通過(guò)世界坐標(biāo)系與相機(jī)坐標(biāo)系之間的關(guān)系來(lái)推導(dǎo)相機(jī)外參，并記錄過(guò)程。

根據(jù)上面描述的內(nèi)容，我們繼續(xù)以上述為例，設(shè)是在相機(jī)坐標(biāo)系的點(diǎn)，是在世界坐標(biāo)系下的點(diǎn)，我們可以使用一個(gè)旋轉(zhuǎn)矩陣和一個(gè)平移向量，把變換到，即：

其中，是一個(gè)3x3的旋轉(zhuǎn)矩陣，是一個(gè)3x1的平移向量，我們將其使用其次坐標(biāo)表達(dá)：

可以改變式子，把加號(hào)也干掉：

所以將旋轉(zhuǎn)矩陣和平移向量帶入到上述公式可得：

所以我們可以使用該矩陣來(lái)表示相機(jī)的外參：

內(nèi)參與外參組合使用

由于上述內(nèi)容可得知：

相機(jī)外參公式：

相機(jī)內(nèi)參公式：

則將外參公式帶入內(nèi)參可得如下結(jié)果：

代碼示例

完成上述公式的推導(dǎo)后，使用Python環(huán)境下進(jìn)行實(shí)際操作一下，僅使用numpy的ndarray作為數(shù)據(jù)結(jié)構(gòu)和opencv的繪圖工具以及solvePnP來(lái)驗(yàn)證相機(jī)外參解法。

首先我們需要先定義一個(gè)被觀測(cè)的目標(biāo)，并將其定義在世界坐標(biāo)中；這里我們就選擇使用一個(gè)立方體作為觀測(cè)目標(biāo)：

import cv2
import numpy as np

# 定義方形畫(huà)布像素坐標(biāo)長(zhǎng)度
canvas_square_size = 320
# 定義立方體的邊長(zhǎng)
length = 1

# 定義立方體的8個(gè)頂點(diǎn)坐標(biāo) 使用世界坐標(biāo)作為表達(dá)
vertices_w = np.array([
    [-length / 2, -length / 2, -length / 2],
    [-length / 2, -length / 2, length / 2],
    [-length / 2, length / 2, -length / 2],
    [-length / 2, length / 2, length / 2],
    [length / 2, -length / 2, -length / 2],
    [length / 2, -length / 2, length / 2],
    [length / 2, length / 2, -length / 2],
    [length / 2, length / 2, length / 2]])
print("像素坐標(biāo)系頂點(diǎn)集合: ", vertices_uv.shape)
# 打印結(jié)果：
# 世界坐標(biāo)系頂點(diǎn)集合:  (8, 3)

定義好世界坐標(biāo)下的立方體后，我們來(lái)手動(dòng)定義一組相機(jī)外參，上述提到過(guò)，相機(jī)外參是由一個(gè)旋轉(zhuǎn)矩陣R和一個(gè)平移向量t組成，這里我們利用旋轉(zhuǎn)矩陣的特性，定義一個(gè)攜帶讓其沿著roll軸旋轉(zhuǎn)一定角度的RulerAngle屬性的旋轉(zhuǎn)矩陣R_roll，并手動(dòng)設(shè)置一個(gè)t向量；使用世界坐標(biāo)系對(duì)其進(jìn)行變換得出相機(jī)坐標(biāo)系頂點(diǎn)集，即使用公式：，求解步驟代碼如下：

# 定義一個(gè)角度
a = 45
# 轉(zhuǎn)換為弧度制
a = np.deg2rad(a)

# 手動(dòng)定一個(gè)相機(jī)外參R旋轉(zhuǎn)矩陣，并設(shè)置讓其繞roll軸旋轉(zhuǎn)a度
R_roll = np.array([
    [1, 0, 0],
    [0, np.cos(a), -np.sin(a)],
    [0, np.sin(a), np.cos(a)]
])
# 手動(dòng)定一個(gè)相機(jī)外參的偏移向量t 即在x, y, z的位置看面朝單位
t1 = 0
t2 = 0
t3 = 5  # 數(shù)值越大則距離觀測(cè)目標(biāo)距離則越長(zhǎng)
T = np.array([t1, t2, t3])

# 求基于相機(jī)坐標(biāo)系的頂點(diǎn)集
vertices_c = np.matmul(R_roll, vertices_w.T).T + T

再求出新的點(diǎn)集后，我們手動(dòng)定義一組相機(jī)內(nèi)參的K矩陣，并將中心點(diǎn)設(shè)置在像素坐標(biāo)系中畫(huà)布的中心點(diǎn)，以便我們可視化時(shí)可能更清晰的觀測(cè)到目標(biāo)；定義內(nèi)參K矩陣后，我們定義一個(gè)透視投影函數(shù)，用于將三維的坐標(biāo)系投影到像素坐標(biāo)系中，即公式：，函數(shù)定義如下：

def perspective_projection(vertices, K):
    """use perspective projection"""
    vertices_2d = np.matmul(K, vertices.T).T
    vertices_2d[:, 0] /= vertices_2d[:, 2]
    vertices_2d[:, 1] /= vertices_2d[:, 2]
    vertices_2d = vertices_2d[:, :2].astype(np.int32)

    return vertices_2d

定義好函數(shù)perspective_projection后，我們?cè)谥鞒绦蛑欣^續(xù)定義我們的內(nèi)參矩陣K，并使用函數(shù)進(jìn)行坐標(biāo)轉(zhuǎn)換：

# 手動(dòng)定一組相機(jī)內(nèi)參K
fx = 800
fy = 800
cx = canvas_square_size // 2
cy = canvas_square_size // 2
K = np.array([
    [fx, 0, cx],
    [0, fy, cy],
    [0, 0, 1]
])

# 使用透視投影解出像素坐標(biāo)的頂點(diǎn)集
vertices_uv = perspective_projection(vertices_c, K)
print("像素坐標(biāo)系頂點(diǎn)集合: ", vertices_uv.shape)
# 打印結(jié)果：
# 世界坐標(biāo)系頂點(diǎn)集合:  (8, 3)

完成像素坐標(biāo)系的轉(zhuǎn)換后，我們準(zhǔn)備一個(gè)繪制函數(shù)，用來(lái)顯示我們的處理結(jié)果，將投影后的像素坐標(biāo)系顯示出來(lái)：

def display_vertices_uv(vertices_2d, win_name='vertices', wait_key=0, canvas_size=(320, 320)):
    """Show the vertices on uv-coordinates"""
    img = np.zeros((canvas_size[0], canvas_size[1], 3), dtype=np.uint8)
    edges = np.array([
        [0, 1], [0, 2], [0, 4],
        [1, 3], [1, 5],
        [2, 3], [2, 6],
        [4, 5], [4, 6],
        [7, 5], [7, 6], [7, 3]])

    for edge in edges:
        pt1 = tuple(vertices_2d[edge[0]])
        pt2 = tuple(vertices_2d[edge[1]])
        cv2.line(img, pt1, pt2, (0, 255, 255), 2)

    cv2.imshow(win_name, img)
    cv2.waitKey(wait_key)

定義好函數(shù)后，我們回到主程序運(yùn)行代碼顯示結(jié)果：

# 顯示求解后的uv頂點(diǎn)集
display_vertices_uv(vertices_uv, canvas_size=(canvas_square_size, canvas_square_size))

結(jié)果如下：

可以看到我們已經(jīng)成功的使用自己定義的模擬相機(jī)內(nèi)外參去對(duì)該立方體進(jìn)行世界坐標(biāo)轉(zhuǎn)換到像素坐標(biāo)，并成功可視化觀測(cè)到這個(gè)立方體。

這里我們將相機(jī)通過(guò)外參矩陣R的形式設(shè)置的是把相機(jī)擺放在一個(gè)以roll轉(zhuǎn)角45度的觀測(cè)角度，為了以更加豐富的角度觀測(cè)到這個(gè)立方體，我們可以在roll轉(zhuǎn)角45度的情況下，再加入另外一種歐拉角屬性pitch，使其可以添加一種新的角度去觀測(cè)該對(duì)象，代碼如下：

# 再定義一組旋轉(zhuǎn)矩陣，以pitch軸進(jìn)行b度旋轉(zhuǎn)
b = 25
b = np.deg2rad(b)
R_pitch = np.array([
    [0, np.cos(b), -np.sin(b)],
    [1, 0, 0],
    [0, np.sin(b), np.cos(b)]
])
# 重新調(diào)整一下外參的旋轉(zhuǎn)矩陣R
R = np.matmul(R_roll, R_pitch)
# 重新求基于相機(jī)坐標(biāo)系的頂點(diǎn)集 加入yaw旋轉(zhuǎn)角
vertices_c_pitch = np.matmul(R, vertices_w.T).T + T
# 繼續(xù)使用內(nèi)參K透視投影解出像素坐標(biāo)的頂點(diǎn)集
vertices_uv_pitch = perspective_projection(vertices_c_pitch, K)
# 顯示求解后的uv頂點(diǎn)集
display_vertices_uv(vertices_uv_pitch)

通過(guò)代碼可得知我們?cè)谥匦露x了一個(gè)外參矩陣R，是將之前定義的外參矩陣R_roll與當(dāng)前新定義的R_pitch進(jìn)行一個(gè)線性變換處理所得出的結(jié)果，其目的是讓機(jī)位角在roll軸旋轉(zhuǎn)45度的情況下再對(duì)其pitch軸旋轉(zhuǎn)25度，得出該外參矩陣并進(jìn)行坐標(biāo)轉(zhuǎn)換，結(jié)果如下：

可以看到我們能觀測(cè)到的立方體視角更加豐富了

示例2：使用解PnP求出外參矩陣

通過(guò)上述的案例，我們手動(dòng)定義了相機(jī)的內(nèi)外參矩陣，這里補(bǔ)充一點(diǎn)，在知道觀測(cè)目標(biāo)的世界坐標(biāo)與相機(jī)內(nèi)參的情況下，我們是可以使用一些數(shù)學(xué)手段去解出相機(jī)的外參矩陣的，這里采用的方法是使用opencv提供的solvePnP解法，去解出外參矩陣，接續(xù)上一個(gè)案例下進(jìn)行代碼的補(bǔ)充即可，代碼如下：

# 使用solvePnP嘗試解出相機(jī)外參
rvec = np.zeros((3, 1))
tvec = np.zeros((3, 1))

retval, rvec, tvec = cv2.solvePnP(vertices_w.astype(np.float32), vertices_uv_pitch.astype(np.float32),
                                  K.astype(np.float32),
                                  None, rvec, tvec, False, cv2.SOLVEPNP_ITERATIVE)
R_solved, _ = cv2.Rodrigues(rvec)
print("解PnP得出的R Matrix: ", -R_solved)    # 解出的坐標(biāo)系是反著需要自行調(diào)整
print("自己定的R Matrix: ", R)

# 打印結(jié)果：
# 解PnP得出的R Matrix:  [[ 1.27441147e-04  9.06220345e-01 -4.22805713e-01]
#  [ 7.06805406e-01 -2.99177784e-01 -6.41029463e-01]
#  [ 7.07408017e-01  2.98759670e-01  6.40559566e-01]]
# 自己定的R Matrix:  [[ 0.          0.90630779 -0.42261826]
#  [ 0.70710678 -0.29883624 -0.64085638]
#  [ 0.70710678  0.29883624  0.64085638]]

可以看到，使用solvePnP解出的外參矩陣R與我們自行定義的矩陣R基本相等，該結(jié)果的精度與觀測(cè)目標(biāo)的頂點(diǎn)數(shù)量有關(guān)，頂點(diǎn)越多精度會(huì)越高。

示例3：旋轉(zhuǎn)的立方體

通過(guò)上面的案例，我們可以寫(xiě)一個(gè)小玩意，通過(guò)不斷的改變相機(jī)外參來(lái)實(shí)時(shí)更新并顯示出被觀測(cè)對(duì)象的畫(huà)面，可讓立方體仿佛動(dòng)起來(lái)一樣，代碼如下：

# 定義方形畫(huà)布像素坐標(biāo)長(zhǎng)度
canvas_square_size = 320
# 定義立方體的邊長(zhǎng)
length = 1

# 定義立方體的8個(gè)頂點(diǎn)坐標(biāo)
vertices_w = np.array([
    [-length / 2, -length / 2, -length / 2],
    [-length / 2, -length / 2, length / 2],
    [-length / 2, length / 2, -length / 2],
    [-length / 2, length / 2, length / 2],
    [length / 2, -length / 2, -length / 2],
    [length / 2, -length / 2, length / 2],
    [length / 2, length / 2, -length / 2],
    [length / 2, length / 2, length / 2]])

# 手動(dòng)定一組相機(jī)內(nèi)參K
fx = 800
fy = 800
cx = canvas_square_size // 2
cy = canvas_square_size // 2
K = np.array([
    [fx, 0, cx],
    [0, fy, cy],
    [0, 0, 1]
])

# 初始化角度
a = 0
while True:
    # 手動(dòng)定一個(gè)相機(jī)外參R旋轉(zhuǎn)矩陣，并設(shè)置讓三個(gè)軸旋轉(zhuǎn)a度
    R = construct_extrinsic_matrix_R(a, a, a)

    # 手動(dòng)定一個(gè)相機(jī)外參的偏移向量t 即在x, y, z的位置看面朝單位
    t1 = 0
    t2 = 0
    t3 = 5  # 數(shù)值越大則距離觀測(cè)目標(biāo)距離則越長(zhǎng)
    T = np.array([t1, t2, t3])
    # 求基于相機(jī)坐標(biāo)系的頂點(diǎn)集
    vertices_c = np.matmul(R, vertices_w.T).T + T
    # 使用透視投影解出像素坐標(biāo)的頂點(diǎn)集
    vertices_uv = perspective_projection(vertices_c, K)
    # 顯示求解后的uv頂點(diǎn)集
    display_vertices_uv(vertices_uv, wait_key=30, canvas_size=(canvas_square_size, canvas_square_size))
    a += 1

結(jié)果如下：

完整代碼：

所有示例的完整代碼如下：

import cv2
import numpy as np


def display_vertices_uv(vertices_2d, win_name='vertices', wait_key=0, canvas_size=(320, 320)):
    """Show the vertices on uv-coordinates"""
    img = np.zeros((canvas_size[0], canvas_size[1], 3), dtype=np.uint8)
    edges = np.array([
        [0, 1], [0, 2], [0, 4],
        [1, 3], [1, 5],
        [2, 3], [2, 6],
        [4, 5], [4, 6],
        [7, 5], [7, 6], [7, 3]])

    for edge in edges:
        pt1 = tuple(vertices_2d[edge[0]])
        pt2 = tuple(vertices_2d[edge[1]])
        cv2.line(img, pt1, pt2, (0, 255, 255), 2)

    cv2.imshow(win_name, img)
    cv2.waitKey(wait_key)


def perspective_projection(vertices, K):
    """use perspective projection"""
    vertices_2d = np.matmul(K, vertices.T).T
    vertices_2d[:, 0] /= vertices_2d[:, 2]
    vertices_2d[:, 1] /= vertices_2d[:, 2]
    vertices_2d = vertices_2d[:, :2].astype(np.int32)

    return vertices_2d


def construct_extrinsic_matrix_R(yaw_angle, roll_angle, pitch_angle):
    """Construct the camera external parameter rotation matrix R"""
    yaw = np.deg2rad(yaw_angle)
    roll = np.deg2rad(roll_angle)
    pitch = np.deg2rad(pitch_angle)
    R_yaw = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    R_roll = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])
    R_pitch = np.array([
        [0, np.cos(pitch), -np.sin(pitch)],
        [1, 0, 0],
        [0, np.sin(pitch), np.cos(pitch)]
    ])
    R = np.matmul(R_pitch, np.matmul(R_yaw, R_roll))

    return R


def sample_a():
    # 定義方形畫(huà)布像素坐標(biāo)長(zhǎng)度
    canvas_square_size = 320
    # 定義立方體的邊長(zhǎng)
    length = 1

    # 定義立方體的8個(gè)頂點(diǎn)坐標(biāo) 使用世界坐標(biāo)作為表達(dá)
    vertices_w = np.array([
        [-length / 2, -length / 2, -length / 2],
        [-length / 2, -length / 2, length / 2],
        [-length / 2, length / 2, -length / 2],
        [-length / 2, length / 2, length / 2],
        [length / 2, -length / 2, -length / 2],
        [length / 2, -length / 2, length / 2],
        [length / 2, length / 2, -length / 2],
        [length / 2, length / 2, length / 2]])
    print("世界坐標(biāo)系頂點(diǎn)集合: ", vertices_w.shape)

    # 定義一個(gè)角度
    a = 45
    # 轉(zhuǎn)換為弧度制
    a = np.deg2rad(a)

    # 手動(dòng)定一個(gè)相機(jī)外參R旋轉(zhuǎn)矩陣，并設(shè)置讓其繞roll軸旋轉(zhuǎn)a度
    R_roll = np.array([
        [1, 0, 0],
        [0, np.cos(a), -np.sin(a)],
        [0, np.sin(a), np.cos(a)]
    ])
    # 手動(dòng)定一個(gè)相機(jī)外參的偏移向量t 即在x, y, z的位置看面朝單位
    t1 = 0
    t2 = 0
    t3 = 5  # 數(shù)值越大則距離觀測(cè)目標(biāo)距離則越長(zhǎng)
    T = np.array([t1, t2, t3])

    # 求基于相機(jī)坐標(biāo)系的頂點(diǎn)集
    vertices_c = np.matmul(R_roll, vertices_w.T).T + T

    # 手動(dòng)定一組相機(jī)內(nèi)參K
    fx = 800
    fy = 800
    cx = canvas_square_size // 2
    cy = canvas_square_size // 2
    K = np.array([
        [fx, 0, cx],
        [0, fy, cy],
        [0, 0, 1]
    ])

    # 使用透視投影解出像素坐標(biāo)的頂點(diǎn)集
    vertices_uv = perspective_projection(vertices_c, K)
    print("像素坐標(biāo)系頂點(diǎn)集合: ", vertices_uv.shape)

    # 顯示求解后的uv頂點(diǎn)集
    display_vertices_uv(vertices_uv, canvas_size=(canvas_square_size, canvas_square_size))

    # 再定義一組旋轉(zhuǎn)矩陣，以pitch軸進(jìn)行b度旋轉(zhuǎn)
    b = 25
    b = np.deg2rad(b)
    R_pitch = np.array([
        [0, np.cos(b), -np.sin(b)],
        [1, 0, 0],
        [0, np.sin(b), np.cos(b)]
    ])
    # 重新調(diào)整一下外參的旋轉(zhuǎn)矩陣R
    R = np.matmul(R_roll, R_pitch)
    # 重新求基于相機(jī)坐標(biāo)系的頂點(diǎn)集 加入yaw旋轉(zhuǎn)角
    vertices_c_pitch = np.matmul(R, vertices_w.T).T + T
    # 繼續(xù)使用內(nèi)參K透視投影解出像素坐標(biāo)的頂點(diǎn)集
    vertices_uv_pitch = perspective_projection(vertices_c_pitch, K)
    # 顯示求解后的uv頂點(diǎn)集
    display_vertices_uv(vertices_uv_pitch)

    # 使用solvePnP嘗試解出相機(jī)外參
    rvec = np.zeros((3, 1))
    tvec = np.zeros((3, 1))

    retval, rvec, tvec = cv2.solvePnP(vertices_w.astype(np.float32), vertices_uv_pitch.astype(np.float32),
                                      K.astype(np.float32),
                                      None, rvec, tvec, False, cv2.SOLVEPNP_ITERATIVE)
    R_solved, _ = cv2.Rodrigues(rvec)
    print("解PnP得出的R Matrix: ", -R_solved)   # 解出的坐標(biāo)系是反著需要自行調(diào)整
    print("自己定的R Matrix: ", R)


def sample_b():
    # 定義方形畫(huà)布像素坐標(biāo)長(zhǎng)度
    canvas_square_size = 320
    # 定義立方體的邊長(zhǎng)
    length = 1

    # 定義立方體的8個(gè)頂點(diǎn)坐標(biāo)
    vertices_w = np.array([
        [-length / 2, -length / 2, -length / 2],
        [-length / 2, -length / 2, length / 2],
        [-length / 2, length / 2, -length / 2],
        [-length / 2, length / 2, length / 2],
        [length / 2, -length / 2, -length / 2],
        [length / 2, -length / 2, length / 2],
        [length / 2, length / 2, -length / 2],
        [length / 2, length / 2, length / 2]])

    # 手動(dòng)定一組相機(jī)內(nèi)參K
    fx = 800
    fy = 800
    cx = canvas_square_size // 2
    cy = canvas_square_size // 2
    K = np.array([
        [fx, 0, cx],
        [0, fy, cy],
        [0, 0, 1]
    ])

    # 初始化角度
    a = 0
    while True:
        # 手動(dòng)定一個(gè)相機(jī)外參R旋轉(zhuǎn)矩陣，并設(shè)置讓三個(gè)軸旋轉(zhuǎn)a度
        R = construct_extrinsic_matrix_R(a, a, a)

        # 手動(dòng)定一個(gè)相機(jī)外參的偏移向量t 即在x, y, z的位置看面朝單位
        t1 = 0
        t2 = 0
        t3 = 5  # 數(shù)值越大則距離觀測(cè)目標(biāo)距離則越長(zhǎng)
        T = np.array([t1, t2, t3])
        # 求基于相機(jī)坐標(biāo)系的頂點(diǎn)集
        vertices_c = np.matmul(R, vertices_w.T).T + T
        # 使用透視投影解出像素坐標(biāo)的頂點(diǎn)集
        vertices_uv = perspective_projection(vertices_c, K)
        # 顯示求解后的uv頂點(diǎn)集
        display_vertices_uv(vertices_uv, wait_key=30, canvas_size=(canvas_square_size, canvas_square_size))
        a += 1


if __name__ == '__main__':
    sample_a()  # 示例1
    sample_b()  # 示例2

Note:

由于忘性較大，倉(cāng)促實(shí)驗(yàn)和匆忙記錄只為記錄自己的理解和實(shí)驗(yàn)經(jīng)過(guò)留后期繼續(xù)學(xué)習(xí)和觀看，可能會(huì)有不少理解錯(cuò)誤，如果給您帶來(lái)困擾煩請(qǐng)指正和海涵。文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-610356.html