張正友相機(jī)標(biāo)定法是張正友教授1998年提出的單平面棋盤格的相機(jī)標(biāo)定方法。傳統(tǒng)標(biāo)定法的標(biāo)定板是需要三維的，需要非常精確，這很難制作，而張正友教授提出的方法介于傳統(tǒng)標(biāo)定法和自標(biāo)定法之間，但克服了傳統(tǒng)標(biāo)定法需要的高精度標(biāo)定物的缺點(diǎn)，而僅需使用一個(gè)打印出來(lái)的棋盤格就可以。同時(shí)也相對(duì)于自標(biāo)定而言，提高了精度，便于操作。因此張氏標(biāo)定法被廣泛應(yīng)用于計(jì)算機(jī)視覺(jué)方面。

傳統(tǒng)標(biāo)定法的的標(biāo)定板

張正友標(biāo)定法的標(biāo)定板

今天，我們就來(lái)講解一下張氏標(biāo)定法的原理和實(shí)現(xiàn)，學(xué)會(huì)之后，我們就可以自己去制作一個(gè)棋盤標(biāo)定板，然后拍照，標(biāo)定自己手機(jī)相機(jī)的參數(shù)啦！

一、相機(jī)標(biāo)定介紹

二、算法原理

1.整體流程

2.模型假設(shè)

3.模型求解

（1）內(nèi)外參數(shù)求解

（2）畸變系數(shù)求解

（3）精度優(yōu)化

三、算法實(shí)現(xiàn)

1.main.py

2.homography.py

4.extrinsics.py

5.distortion.py

6.refine_all.py

7.結(jié)果

一、相機(jī)標(biāo)定介紹

相機(jī)標(biāo)定指建立相機(jī)圖像像素位置與場(chǎng)景點(diǎn)位置之間的關(guān)系，根據(jù)相機(jī)成像模型，由特征點(diǎn)在圖像中坐標(biāo)與世界坐標(biāo)的對(duì)應(yīng)關(guān)系，求解相機(jī)模型的參數(shù)。相機(jī)需要標(biāo)定的模型參數(shù)包括內(nèi)部參數(shù)和外部參數(shù)。

針孔相機(jī)成像原理其實(shí)就是利用投影將真實(shí)的三維世界坐標(biāo)轉(zhuǎn)換到二維的相機(jī)坐標(biāo)上去，其模型示意圖如下圖所示：

從圖中我們可以看出，在世界坐標(biāo)中的一條直線上的點(diǎn)在相機(jī)上只呈現(xiàn)出了一個(gè)點(diǎn)，其中發(fā)生了非常大的變化，同時(shí)也損失和很多重要的信息，這正是我們3D重建、目標(biāo)檢測(cè)與識(shí)別領(lǐng)域的重點(diǎn)和難點(diǎn)。實(shí)際中，鏡頭并非理想的透視成像，帶有不同程度的畸變。理論上鏡頭的畸變包括徑向畸變和切向畸變，切向畸變影響較小，通常只考慮徑向畸變。

徑向畸變：徑向畸變主要由鏡頭徑向曲率產(chǎn)生（光線在遠(yuǎn)離透鏡中心的地方比靠近中心的地方更加彎曲）。導(dǎo)致真實(shí)成像點(diǎn)向內(nèi)或向外偏離理想成像點(diǎn)。其中畸變像點(diǎn)相對(duì)于理想像點(diǎn)沿徑向向外偏移，遠(yuǎn)離中心的，稱為枕形畸變；徑向畸點(diǎn)相對(duì)于理想點(diǎn)沿徑向向中心靠攏，稱為桶狀畸變。

用數(shù)學(xué)公式來(lái)表示：

其中，X為相機(jī)中的坐標(biāo)；X為真實(shí)世界坐標(biāo)；K為內(nèi)參矩陣；RT為外參矩陣 K為內(nèi)參矩陣，是相機(jī)內(nèi)部參數(shù)組成的一個(gè)3*3的矩陣，其中，代表焦距；S為畸變參數(shù)為中心點(diǎn)坐標(biāo)，a為縱橫比例參數(shù)，我們可以默認(rèn)設(shè)為1，所以 RT為外參矩陣，R是描述照相機(jī)方向的旋轉(zhuǎn)矩陣，T是描述照相機(jī)中心位置的三維平移向量。

二、算法原理

1.整體流程

2.模型假設(shè)

3.模型求解

（1）內(nèi)外參數(shù)求解

我們令，則

其中，H為一個(gè)3*3的矩陣，并且有一個(gè)元素作為齊次坐標(biāo)。因此，H有8個(gè)自由度。

現(xiàn)在有8個(gè)自由度需要求解，所以需要四個(gè)對(duì)應(yīng)點(diǎn)。也就是四個(gè)點(diǎn)就可以求出圖像平面到世界平面的單應(yīng)性矩陣H。

我想，張氏標(biāo)定法選用的棋盤格作為標(biāo)定板的原因除了角點(diǎn)方便檢測(cè)的另外一個(gè)原因可能就是這個(gè)吧。

通過(guò)4個(gè)點(diǎn)，我們就可以可以獲得單應(yīng)性矩陣H。但是，H是內(nèi)參陣和外參陣的合體。我們想要最終分別獲得內(nèi)參和外參。所以需要想個(gè)辦法，先把內(nèi)參求出來(lái)。然后外參也就隨之解出了。觀察一下這個(gè)式子：

我們可以知道以下約束條件:

①,R1R2 正交，也就是說(shuō) R1 R2=0。其實(shí)這個(gè)不難理解，因?yàn)?R1 R2 是分別繞x軸和y軸得到的，而x軸和y軸均垂直z軸。

②旋轉(zhuǎn)向量的模為1，也就是說(shuō)R1=R2=1,這是因?yàn)樾D(zhuǎn)不改變尺度。

根據(jù)這兩個(gè)約束條件，經(jīng)過(guò)數(shù)學(xué)變換，我們可以得到：

觀察上面的兩個(gè)式子，我們可以看出，由于H1和H2是通過(guò)單應(yīng)性求解出來(lái)的，所以我們要求解的參數(shù)就變成A矩陣中未知的5個(gè)參數(shù)。我們可以通過(guò)三個(gè)單應(yīng)性矩陣來(lái)求解這5個(gè)參數(shù)，利用三個(gè)單應(yīng)性矩陣在兩個(gè)約束下可以生成6個(gè)方程。其中，三個(gè)單應(yīng)性矩陣可以通過(guò)三張對(duì)同一標(biāo)定板不同角度和高度的照片獲得。

用數(shù)學(xué)公式來(lái)表達(dá)如下：

我們很容易發(fā)現(xiàn)B是一個(gè)對(duì)稱陣，所以B的有效元素就剩下6個(gè)，即

進(jìn)一步化簡(jiǎn)：

通過(guò)計(jì)算，我們可以得到

利用上面提到的兩個(gè)約束條件，我們可以得到下面的方程組：

這個(gè)方程組的本質(zhì)和前面那兩個(gè)用h和A組成的約束條件方程組是一樣的。

通過(guò)至少含一個(gè)棋盤格的三幅圖像，應(yīng)用上述公式我們就可以估算出B了。得到B后，我們通過(guò)cholesky分解，就可以得到攝相機(jī)機(jī)的內(nèi)參陣A的六個(gè)自由度，即：

再根據(jù)化簡(jiǎn)可得外部參數(shù)，即：

（2）畸變系數(shù)求解

在文章的開始，我們就講到真實(shí)的鏡頭并非理想的透視成像，而是帶有不同程度的畸變。理論上鏡頭的畸變包括徑向畸變和切向畸變，切向畸變影響較小，通常只考慮徑向畸變，而且在徑向畸變的求解中，僅考慮了起主導(dǎo)的二元泰勒級(jí)數(shù)展開的前兩個(gè)系數(shù)。

具體推導(dǎo)，參考https://blog.csdn.net/onthewaysuccess/article/details/40736177

（3）精度優(yōu)化

在張正友標(biāo)定法中，使用了兩次極大似然估計(jì)策略，第一次是在不考慮畸變的情況下求解內(nèi)參和外參，第二次是求解實(shí)際的畸變系數(shù)。

極大似然參數(shù)估計(jì)，參考https://blog.csdn.net/onthewaysuccess/article/details/40717213

三、算法實(shí)現(xiàn)

1.main.py

#!usr/bin/env/ python

# _*_ coding:utf-8 _*_

import cv2 as cv

import numpy as np

import os

from step.homography import get_homography

from step.intrinsics import get_intrinsics_param

from step.extrinsics import get_extrinsics_param

from step.distortion import get_distortion

from step.refine_all import refinall_all_param

def calibrate():

#求單應(yīng)矩陣

H = get_homography(pic_points, real_points_x_y)

#求內(nèi)參

intrinsics_param = get_intrinsics_param(H)

#求對(duì)應(yīng)每幅圖外參

extrinsics_param = get_extrinsics_param(H, intrinsics_param)

#畸變矯正

k = get_distortion(intrinsics_param, extrinsics_param, pic_points, real_points_x_y)

#微調(diào)所有參數(shù)

[new_intrinsics_param, new_k, new_extrinsics_param] = refinall_all_param(intrinsics_param,

k, extrinsics_param, real_points, pic_points)

print("intrinsics_parm: ", new_intrinsics_param)

print("distortionk: ", new_k)

print("extrinsics_parm: ", new_extrinsics_param)

if __name__ == "__main__":

file_dir = r'..pic'

# 標(biāo)定所用圖像

pic_name = os.listdir(file_dir)

# 由于棋盤為二維平面，設(shè)定世界坐標(biāo)系在棋盤上，一個(gè)單位代表一個(gè)棋盤寬度，產(chǎn)生世界坐標(biāo)系三維坐標(biāo)

cross_corners = [9, 6] #棋盤方塊交界點(diǎn)排列

real_coor = np.zeros((cross_corners[0] * cross_corners[1], 3), np.float32)

real_coor[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)

real_points = []

real_points_x_y = []

pic_points = []

for pic in pic_name:

pic_path = os.path.join(file_dir, pic)

pic_data = cv.imread(pic_path)

# 尋找到棋盤角點(diǎn)

succ, pic_coor = cv.findChessboardCorners(pic_data, (cross_corners[0], cross_corners[1]), None)

if succ:

# 添加每幅圖的對(duì)應(yīng)3D-2D坐標(biāo)

pic_coor = pic_coor.reshape(-1, 2)

pic_points.append(pic_coor)

real_points.append(real_coor)

real_points_x_y.append(real_coor[:, :2])

calibrate()

2.homography.py

這是用于求解單應(yīng)性矩陣的文件

#!usr/bin/env/ python

# _*_ coding:utf-8 _*_

import numpy as np

from scipy import optimize as opt

#求輸入數(shù)據(jù)的歸一化矩陣

def normalizing_input_data(coor_data):

x_avg = np.mean(coor_data[:, 0])

y_avg = np.mean(coor_data[:, 1])

sx = np.sqrt(2) / np.std(coor_data[:, 0])

sy = np.sqrt(2) / np.std(coor_data[:, 1])

norm_matrix = np.matrix([[sx, 0, -sx * x_avg],

[0, sy, -sy * y_avg],

[0, 0, 1]])

return norm_matrix

#求取初始估計(jì)的單應(yīng)矩陣

def get_initial_H(pic_coor, real_coor):

# 獲得歸一化矩陣

pic_norm_mat = normalizing_input_data(pic_coor)

real_norm_mat = normalizing_input_data(real_coor)

M = []

for i in range(len(pic_coor)):

#轉(zhuǎn)換為齊次坐標(biāo)

single_pic_coor = np.array([pic_coor[i][0], pic_coor[i][1], 1])

single_real_coor = np.array([real_coor[i][0], real_coor[i][1], 1])

#坐標(biāo)歸一化

pic_norm = np.dot(pic_norm_mat, single_pic_coor)

real_norm = np.dot(real_norm_mat, single_real_coor)

#構(gòu)造M矩陣

M.append(np.array([-real_norm.item(0), -real_norm.item(1), -1,

0, 0, 0,

pic_norm.item(0) * real_norm.item(0), pic_norm.item(0) * real_norm.item(1), pic_norm.item(0)]))

M.append(np.array([0, 0, 0,

-real_norm.item(0), -real_norm.item(1), -1,

pic_norm.item(1) * real_norm.item(0), pic_norm.item(1) * real_norm.item(1), pic_norm.item(1)]))

#利用SVD求解M * h = 0中h的解

U, S, VT = np.linalg.svd((np.array(M, dtype='float')).reshape((-1, 9)))

# 最小的奇異值對(duì)應(yīng)的奇異向量,S求出來(lái)按大小排列的，最后的最小

H = VT[-1].reshape((3, 3))

H = np.dot(np.dot(np.linalg.inv(pic_norm_mat), H), real_norm_mat)

H /= H[-1, -1]

return H

#返回估計(jì)坐標(biāo)與真實(shí)坐標(biāo)偏差

def value(H, pic_coor, real_coor):

Y = np.array([])

for i in range(len(real_coor)):

single_real_coor = np.array([real_coor[i, 0], real_coor[i, 1], 1])

U = np.dot(H.reshape(3, 3), single_real_coor)

U /= U[-1]

Y = np.append(Y, U[:2])

Y_NEW = (pic_coor.reshape(-1) - Y)

return Y_NEW

#返回對(duì)應(yīng)jacobian矩陣

def jacobian(H, pic_coor, real_coor):

J = []

for i in range(len(real_coor)):

sx = H[0]*real_coor[i][0] + H[1]*real_coor[i][1] +H[2]

sy = H[3]*real_coor[i][0] + H[4]*real_coor[i][1] +H[5]

w = H[6]*real_coor[i][0] + H[7]*real_coor[i][1] +H[8]

w2 = w * w

J.append(np.array([real_coor[i][0]/w, real_coor[i][1]/w, 1/w,

0, 0, 0,

-sx*real_coor[i][0]/w2, -sx*real_coor[i][1]/w2, -sx/w2]))

J.append(np.array([0, 0, 0,

real_coor[i][0]/w, real_coor[i][1]/w, 1/w,

-sy*real_coor[i][0]/w2, -sy*real_coor[i][1]/w2, -sy/w2]))

return np.array(J)

#利用Levenberg Marquart算法微調(diào)H

def refine_H(pic_coor, real_coor, initial_H):

initial_H = np.array(initial_H)

final_H = opt.leastsq(value,

initial_H,

Dfun=jacobian,

args=(pic_coor, real_coor))[0]

final_H /= np.array(final_H[-1])

return final_H

#返回微調(diào)后的H

def get_homography(pic_coor, real_coor):

refined_homographies =[]

error = []

for i in range(len(pic_coor)):

initial_H = get_initial_H(pic_coor[i], real_coor[i])

final_H = refine_H(pic_coor[i], real_coor[i], initial_H)

refined_homographies.append(final_H)

return np.array(refined_homographies)

3.intrinsics.py

這是用于求解內(nèi)參矩陣的文件

#!usr/bin/env/ python

# _*_ coding:utf-8 _*_

import numpy as np

#返回pq位置對(duì)應(yīng)的v向量

def create_v(p, q, H):

H = H.reshape(3, 3)

return np.array([

H[0, p] * H[0, q],

H[0, p] * H[1, q] + H[1, p] * H[0, q],

H[1, p] * H[1, q],

H[2, p] * H[0, q] + H[0, p] * H[2, q],

H[2, p] * H[1, q] + H[1, p] * H[2, q],

H[2, p] * H[2, q]

])

#返回相機(jī)內(nèi)參矩陣A

def get_intrinsics_param(H):

#構(gòu)建V矩陣

V = np.array([])

for i in range(len(H)):

V = np.append(V, np.array([create_v(0, 1, H[i]), create_v(0, 0 , H[i])- create_v(1, 1 , H[i])]))

#求解V*b = 0中的b

U, S, VT = np.linalg.svd((np.array(V, dtype='float')).reshape((-1, 6)))

#最小的奇異值對(duì)應(yīng)的奇異向量,S求出來(lái)按大小排列的，最后的最小

b = VT[-1]

#求取相機(jī)內(nèi)參

w = b[0] * b[2] * b[5] - b[1] * b[1] * b[5] - b[0] * b[4] * b[4] + 2 * b[1] * b[3] * b[4] - b[2] * b[3] * b[3]

d = b[0] * b[2] - b[1] * b[1]

alpha = np.sqrt(w / (d * b[0]))

beta = np.sqrt(w / d**2 * b[0])

gamma = np.sqrt(w / (d**2 * b[0])) * b[1]

uc = (b[1] * b[4] - b[2] * b[3]) / d

vc = (b[1] * b[3] - b[0] * b[4]) / d

return np.array([

[alpha, gamma, uc],

[0, beta, vc],

[0, 0, 1]

])

4.extrinsics.py

這是用于求解外參矩陣的文件

#!usr/bin/env/ python

# _*_ coding:utf-8 _*_

import numpy as np

#返回每一幅圖的外參矩陣[R|t]

def get_extrinsics_param(H, intrinsics_param):

extrinsics_param = []

inv_intrinsics_param = np.linalg.inv(intrinsics_param)

for i in range(len(H)):

h0 = (H[i].reshape(3, 3))[:, 0]

h1 = (H[i].reshape(3, 3))[:, 1]

h2 = (H[i].reshape(3, 3))[:, 2]

scale_factor = 1 / np.linalg.norm(np.dot(inv_intrinsics_param, h0))

r0 = scale_factor * np.dot(inv_intrinsics_param, h0)

r1 = scale_factor * np.dot(inv_intrinsics_param, h1)

t = scale_factor * np.dot(inv_intrinsics_param, h2)

r2 = np.cross(r0, r1)

R = np.array([r0, r1, r2, t]).transpose()

extrinsics_param.append(R)

return extrinsics_param

5.distortion.py

這是用于求解畸變矯正系數(shù)的文件

#!usr/bin/env/ python

# _*_ coding:utf-8 _*_

import numpy as np

#返回畸變矯正系數(shù)k0,k1

def get_distortion(intrinsic_param, extrinsic_param, pic_coor, real_coor):

D = []

d = []

for i in range(len(pic_coor)):

for j in range(len(pic_coor[i])):

#轉(zhuǎn)換為齊次坐標(biāo)

single_coor = np.array([(real_coor[i])[j, 0], (real_coor[i])[j, 1], 0, 1])

#利用現(xiàn)有內(nèi)參及外參求出估計(jì)圖像坐標(biāo)

u = np.dot(np.dot(intrinsic_param, extrinsic_param[i]), single_coor)

[u_estim, v_estim] = [u[0]/u[2], u[1]/u[2]]

coor_norm = np.dot(extrinsic_param[i], single_coor)

coor_norm /= coor_norm[-1]

#r = np.linalg.norm((real_coor[i])[j])

r = np.linalg.norm(coor_norm)

D.append(np.array([(u_estim - intrinsic_param[0, 2]) * r ** 2, (u_estim - intrinsic_param[0, 2]) * r ** 4]))

D.append(np.array([(v_estim - intrinsic_param[1, 2]) * r ** 2, (v_estim - intrinsic_param[1, 2]) * r ** 4]))

#求出估計(jì)坐標(biāo)與真實(shí)坐標(biāo)的殘差

d.append(pic_coor[i][j, 0] - u_estim)

d.append(pic_coor[i][j, 1] - v_estim)

'''

D.append(np.array([(pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 2, (pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 4]))

D.append(np.array([(pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 2, (pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 4]))

#求出估計(jì)坐標(biāo)與真實(shí)坐標(biāo)的殘差

d.append(u_estim - pic_coor[i][j, 0])

d.append(v_estim - pic_coor[i][j, 1])

'''

D = np.array(D)

temp = np.dot(np.linalg.inv(np.dot(D.T, D)), D.T)

k = np.dot(temp, d)

'''

#也可利用SVD求解D * k = d中的k

U, S, Vh=np.linalg.svd(D, full_matrices=False)

temp_S = np.array([[S[0], 0],

[0, S[1]]])

temp_res = np.dot(Vh.transpose(), np.linalg.inv(temp_S))

temp_res_res = np.dot(temp_res, U.transpose())

k = np.dot(temp_res_res, d)

'''

return k

6.refine_all.py

這是用于微調(diào)參數(shù)的文件

#!usr/bin/env/ python

# _*_ coding:utf-8 _*_

import numpy as np

import math

from scipy import optimize as opt

#微調(diào)所有參數(shù)

def refinall_all_param(A, k, W, real_coor, pic_coor):

#整合參數(shù)

P_init = compose_paramter_vector(A, k, W)

#復(fù)制一份真實(shí)坐標(biāo)

X_double = np.zeros((2 * len(real_coor) * len(real_coor[0]), 3))

Y = np.zeros((2 * len(real_coor) * len(real_coor[0])))

M = len(real_coor)

N = len(real_coor[0])

for i in range(M):

for j in range(N):

X_double[(i * N + j) * 2] = (real_coor[i])[j]

X_double[(i * N + j) * 2 + 1] = (real_coor[i])[j]

Y[(i * N + j) * 2] = (pic_coor[i])[j, 0]

Y[(i * N + j) * 2 + 1] = (pic_coor[i])[j, 1]

#微調(diào)所有參數(shù)

P = opt.leastsq(value,

P_init,

args=(W, real_coor, pic_coor),

Dfun=jacobian)[0]

#raial_error表示利用標(biāo)定后的參數(shù)計(jì)算得到的圖像坐標(biāo)與真實(shí)圖像坐標(biāo)點(diǎn)的平均像素距離

error = value(P, W, real_coor, pic_coor)

raial_error = [np.sqrt(error[2 * i]**2 + error[2 * i + 1]**2) for i in range(len(error) // 2)]

print("total max error: ", np.max(raial_error))

#返回拆解后參數(shù)，分別為內(nèi)參矩陣，畸變矯正系數(shù)，每幅圖對(duì)應(yīng)外參矩陣

return decompose_paramter_vector(P)

#把所有參數(shù)整合到一個(gè)數(shù)組內(nèi)

def compose_paramter_vector(A, k, W):

alpha = np.array([A[0, 0], A[1, 1], A[0, 1], A[0, 2], A[1, 2], k[0], k[1]])

P = alpha

for i in range(len(W)):

R, t = (W[i])[:, :3], (W[i])[:, 3]

#旋轉(zhuǎn)矩陣轉(zhuǎn)換為一維向量形式

zrou = to_rodrigues_vector(R)

w = np.append(zrou, t)

P = np.append(P, w)

return P

#分解參數(shù)集合，得到對(duì)應(yīng)的內(nèi)參，外參，畸變矯正系數(shù)

def decompose_paramter_vector(P):

[alpha, beta, gamma, uc, vc, k0, k1] = P[0:7]

A = np.array([[alpha, gamma, uc],

[0, beta, vc],

[0, 0, 1]])

k = np.array([k0, k1])

W = []

M = (len(P) - 7) // 6

for i in range(M):

m = 7 + 6 * i

zrou = P[m:m+3]

t = (P[m+3:m+6]).reshape(3, -1)

#將旋轉(zhuǎn)矩陣一維向量形式還原為矩陣形式

R = to_rotation_matrix(zrou)

#依次拼接每幅圖的外參

w = np.concatenate((R, t), axis=1)

W.append(w)

W = np.array(W)

return A, k, W

#返回從真實(shí)世界坐標(biāo)映射的圖像坐標(biāo)

def get_single_project_coor(A, W, k, coor):

single_coor = np.array([coor[0], coor[1], coor[2], 1])

#'''

coor_norm = np.dot(W, single_coor)

coor_norm /= coor_norm[-1]

#r = np.linalg.norm(coor)

r = np.linalg.norm(coor_norm)

uv = np.dot(np.dot(A, W), single_coor)

uv /= uv[-1]

#畸變

u0 = uv[0]

v0 = uv[1]

uc = A[0, 2]

vc = A[1, 2]

#u = (uc * r**2 * k[0] + uc * r**4 * k[1] - u0) / (r**2 * k[0] + r**4 * k[1] - 1)

#v = (vc * r**2 * k[0] + vc * r**4 * k[1] - v0) / (r**2 * k[0] + r**4 * k[1] - 1)

u = u0 + (u0 - uc) * r**2 * k[0] + (u0 - uc) * r**4 * k[1]

v = v0 + (v0 - vc) * r**2 * k[0] + (v0 - vc) * r**4 * k[1]

'''

uv = np.dot(W, single_coor)

uv /= uv[-1]

# 透鏡矯正

x0 = uv[0]

y0 = uv[1]

r = np.linalg.norm(np.array([x0, y0]))

k0 = 0

k1 = 0

x = x0 * (1 + r ** 2 * k0 + r ** 4 * k1)

y = y0 * (1 + r ** 2 * k0 + r ** 4 * k1)

#u = A[0, 0] * x + A[0, 2]

#v = A[1, 1] * y + A[1, 2]

[u, v, _] = np.dot(A, np.array([x, y, 1]))

'''

return np.array([u, v])

#返回所有點(diǎn)的真實(shí)世界坐標(biāo)映射到的圖像坐標(biāo)與真實(shí)圖像坐標(biāo)的殘差

def value(P, org_W, X, Y_real):

M = (len(P) - 7) // 6

N = len(X[0])

A = np.array([

[P[0], P[2], P[3]],

[0, P[1], P[4]],

[0, 0, 1]

])

Y = np.array([])

for i in range(M):

m = 7 + 6 * i

#取出當(dāng)前圖像對(duì)應(yīng)的外參

w = P[m:m + 6]

# 不用旋轉(zhuǎn)矩陣的變換是因?yàn)闀?huì)有精度損失

'''

R = to_rotation_matrix(w[:3])

t = w[3:].reshape(3, 1)

W = np.concatenate((R, t), axis=1)

'''

W = org_W[i]

#計(jì)算每幅圖的坐標(biāo)殘差

for j in range(N):

Y = np.append(Y, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))

error_Y = np.array(Y_real).reshape(-1) - Y

return error_Y

#計(jì)算對(duì)應(yīng)jacobian矩陣

def jacobian(P, WW, X, Y_real):

M = (len(P) - 7) // 6

N = len(X[0])

K = len(P)

A = np.array([

[P[0], P[2], P[3]],

[0, P[1], P[4]],

[0, 0, 1]

])

res = np.array([])

for i in range(M):

m = 7 + 6 * i

w = P[m:m + 6]

R = to_rotation_matrix(w[:3])

t = w[3:].reshape(3, 1)

W = np.concatenate((R, t), axis=1)

for j in range(N):

res = np.append(res, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))

#求得x, y方向?qū)[k]的偏導(dǎo)

J = np.zeros((K, 2 * M * N))

for k in range(K):

J[k] = np.gradient(res, P[k])

return J.T

#將旋轉(zhuǎn)矩陣分解為一個(gè)向量并返回，Rodrigues旋轉(zhuǎn)向量與矩陣的變換,最后計(jì)算坐標(biāo)時(shí)并未用到，因?yàn)闀?huì)有精度損失

def to_rodrigues_vector(R):

p = 0.5 * np.array([[R[2, 1] - R[1, 2]],

[R[0, 2] - R[2, 0]],

[R[1, 0] - R[0, 1]]])

c = 0.5 * (np.trace(R) - 1)

if np.linalg.norm(p) == 0:

if c == 1:

zrou = np.array([0, 0, 0])

elif c == -1:

R_plus = R + np.eye(3, dtype='float')

norm_array = np.array([np.linalg.norm(R_plus[:, 0]),

np.linalg.norm(R_plus[:, 1]),

np.linalg.norm(R_plus[:, 2])])

v = R_plus[:, np.where(norm_array == max(norm_array))]

u = v / np.linalg.norm(v)

if u[0] < 0 or (u[0] == 0 and u[1] < 0) or (u[0] == u[1] and u[0] == 0 and u[2] < 0):

u = -u

zrou = math.pi * u

else:

zrou = []

else:

u = p / np.linalg.norm(p)

theata = math.atan2(np.linalg.norm(p), c)

zrou = theata * u

return zrou

#把旋轉(zhuǎn)矩陣的一維向量形式還原為旋轉(zhuǎn)矩陣并返回

def to_rotation_matrix(zrou):

theta = np.linalg.norm(zrou)

zrou_prime = zrou / theta

W = np.array([[0, -zrou_prime[2], zrou_prime[1]],

[zrou_prime[2], 0, -zrou_prime[0]],

[-zrou_prime[1], zrou_prime[0], 0]])

R = np.eye(3, dtype='float') + W * math.sin(theta) + np.dot(W, W) * (1 - math.cos(theta))

return R

7.結(jié)果

拍攝的不同角度，不同高度的圖像

運(yùn)行結(jié)果：

拍照的手機(jī)是華為p9，后置攝像頭是1200萬(wàn)像素。

內(nèi)部參數(shù)矩陣是為：

[ 9.95397796e+02, -5.74043156e+00, 5.30659959e+02, 0.00000000e+00, 1.04963119e+03, 6.55565437e+02, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]

因?yàn)榇a是以一個(gè)方格為一個(gè)單位，沒(méi)有考慮單位長(zhǎng)度，所以要求真實(shí)的參數(shù)應(yīng)該乘一個(gè)單位長(zhǎng)度，博主采用的方格的尺寸是2.98cm的，自己拿excel畫的，get了一個(gè)新技能~~

審核編輯：郭婷