相機標定2d座標轉3d座標

相機標定原理：

可以看看這兩篇：
https://blog.csdn.net/baidu_38172402/article/details/81949447
https://blog.csdn.net/weixin_43206570/article/details/84797361

在圖像測量過程以及機器視覺應用中，爲確定空間物體表面某點的三維幾何位置與其在圖像中對應點之間的相互關係，必須建立相機成像的幾何模型，這些幾何模型參數就是相機參數。

進行攝像機標定的目的：求出相機的內、外參數，以及畸變參數。
標定相機後通常是想做兩件事：一個是由於每個鏡頭的畸變程度各不相同，通過相機標定可以校正這種鏡頭畸變矯正畸變，生成矯正後的圖像；另一個是根據獲得的圖像重構三維場景。

攝像機標定過程，簡單的可以簡單的描述爲通過標定板，可以得到n個對應的世界座標三維點Xi和對應的圖像座標二維點xi，這些三維點到二維點的轉換都可以通過上面提到的相機內參K，相機外參R和t，以及畸變參數D，經過一系列的矩陣變換得到。

棋盤格標定：

import numpy as np
import cv2
import glob

# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((7*10,3), np.float32)
objp[:,:2] = np.mgrid[0:10,0:7].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

images = glob.glob('./pic/14051293/*')
for fname in images:
    print("current image :{:s}".format(fname))
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (10,7),None)

    # If found, add object points, image points (after refining them)
    if ret == True:
        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (10,7), corners2,ret)
        #cv2.imshow('img',img)
        #cv2.waitKey(0)

cv2.destroyAllWindows()

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
print(ret, mtx, dist, rvecs, tvecs)

mean_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
    mean_error += error

print ("total error: ", mean_error/len(objpoints))

img2 = cv2.imread('biaodingjia_14230075_2.png')
h, w = img2.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)
cv2.imwrite('矯正圖.png', dst)

標定球標定

做一個簡單的標註工具

import cv2
import numpy as np
image_name = "you12"
img = cv2.imread('/home/deepglint/工作/競走demo_2/標定/'+image_name+".bmp")
a =[]
b = []
def on_EVENT_LBUTTONDOWN(event, x, y,flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        xy = "%d,%d" % (x, y)
        a.append(x)
        b.append(y)
        cv2.circle(img, (x, y), 1, (255, 0, 0), thickness=-1)
        cv2.putText(img, xy, (x, y), cv2.FONT_HERSHEY_PLAIN,
                    1.0, (0, 0, 0), thickness=1)
        cv2.imshow("image", img)
    if event == cv2.EVENT_RBUTTONDOWN:
        print("remove",a[-1],b[-1])
        a.pop()
        b.pop()
        cv2.imshow("image", img)
    
cv2.namedWindow("image",0)
cv2.setMouseCallback("image", on_EVENT_LBUTTONDOWN)
cv2.resizeWindow("image", 1600, 900)
cv2.imshow("image", img)
cv2.waitKey(0)
print(a[0],b[0])

img[b[0]:b[1],a[0]:a[1],:] = 0   #注意是 行，列（y軸的，X軸）
cv2.imshow("image", img)
cv2.imwrite(image_name+"_point.png",img)
cv2.waitKey(0)
with open(image_name+".txt","w") as f:
    f.write("[")
    for i in range(len(a)):
        if i != len(a)-1:
            f.write("[["+str(a[i])+","+str(b[i])+"]],")
        else:
            f.write("[["+str(a[i])+","+str(b[i])+"]]")
    f.write("]")

棋盤格標內參，單個相機標定外參

import numpy as np
import cv2
import glob
from numpy import *
import json
from itertools import chain

json_file = open("Camera_parameters.json",'w',encoding='utf-8')

print("----------------18559989-----------------")#2,zhong
id1 = 18559989
mtx_1 = np.array([[3.19018347e+03 ,0.00000000e+00 ,9.27630883e+02],
 [0.00000000e+00 ,3.19295595e+03 ,5.47641169e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_1 = np.array([[-7.32795198e-01] ,[-5.34250952e-01]  ,[8.76286125e-04]  ,[5.17055837e-03],[1.34585035e+02]])
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.array([[0.8021,0.6890,1.0501],[0.4615,0.4000,1.2796],[-0.0358,-0.0188,1.6063],[0.8095,1.2067,1.0496],[0.4743,1.4983,1.2833],[-0.0189,1.9240,1.6090],[1.4159,1.2045,1.0442],[1.7622,1.4936,1.2601],[2.2697,1.9143,1.5651],[1.4183,0.6867,1.0532],[1.7558,0.3958,1.2844],[2.2515,-0.0242,1.6137],[0.8038,0.6895,0.6182],[0.4732,0.4085,0.3692],[0.0000,0.0000,0.0000],[0.8115,1.2007,0.6173],[0.4779,1.4798,0.3682],[0.0000,1.8878,0.0000],[1.4075,1.1952,0.6101],[1.7376,1.4692,0.3489],[2.2130,1.8652,-0.0369],[1.4108,0.6858,0.6171],[1.7346,0.3986,0.3662],[2.2048,-0.0176,0.0000]],dtype=np.float32)
points_1 = np.array([[[504,377]],[[627,311]],[[810,217]],[[437,385]],[[477,332]],[[532,264]],[[293,383]],[[181,326]],[[28,247]],[[355,374]],[[306,299]],[[230,182]],[[503,499]],[[623,568]],[[799,672]],[[437,503]],[[476,569]],[[530,658]],[[295,502]],[[186,572]],[[38,673]],[[355,497]],[[307,569]],[[229,680]]],dtype=np.float32)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# Arrays to store object points and image points from all the images.
objpoints_1 = [] # 3d point in real world space
imgpoints_1 = [] # 2d points in image plane.

objpoints_1.append(objp*1000)
imgpoints_1.append(points_1)
imgpointssinglegroup_1 = imgpoints_1[0]

imgpointssinglegroup_undis_1 = imgpointssinglegroup_1
print(objpoints_1[0].shape,imgpointssinglegroup_undis_1.shape)
ret, rotation_1, translation_1 = cv2.solvePnP(objpoints_1[0], imgpointssinglegroup_undis_1, mtx_1, dist_1)
R_mtx_1 = cv2.Rodrigues(rotation_1)[0]
pmat_1 = np.column_stack((R_mtx_1,translation_1))
print("R: ",rotation_1)
print("Rmtx: ",R_mtx_1)
print("T: ",translation_1)
print("pmap:",pmat_1)

imgpoints2, _ = cv2.projectPoints(objpoints_1[0], rotation_1, translation_1, mtx_1, dist_1)
error = cv2.norm(imgpointssinglegroup_undis_1,imgpoints2, cv2.NORM_L2)/len(imgpoints2)
print("total error: ", error)


print("----------------19333252-----------------")#1,zuo
id2 = 19333252
mtx_2 = np.array([[3.25421978e+03 ,0.00000000e+00 ,8.85560418e+02],
 [0.00000000e+00 ,3.25543566e+03 ,5.45877711e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_2 = np.array([[-8.16821482e-01]  ,[7.07530775e+00] ,[-1.70896673e-03] ,[-9.39123422e-04] ,[-9.97045814e+01]])
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.array([[0.8021,0.6890,1.0501],[0.4615,0.4000,1.2796],[-0.0358,-0.0188,1.6063],[0.8095,1.2067,1.0496],[0.4743,1.4983,1.2833],[-0.0189,1.9240,1.6090],[1.4159,1.2045,1.0442],[1.7622,1.4936,1.2601],[2.2697,1.9143,1.5651],[1.4183,0.6867,1.0532],[1.7558,0.3958,1.2844],[2.2515,-0.0242,1.6137],[0.8038,0.6895,0.6182],[0.4732,0.4085,0.3692],[0.0000,0.0000,0.0000],[0.8115,1.2007,0.6173],[0.4779,1.4798,0.3682],[0.0000,1.8878,0.0000],[1.4075,1.1952,0.6101],[1.7376,1.4692,0.3489],[2.2130,1.8652,-0.0369],[1.4108,0.6858,0.6171],[1.7346,0.3986,0.3662],[2.2048,-0.0176,0.0000]],dtype=np.float32)
points_2 = np.array([[[759,384]],[[876,314]],[[1055,212]],[[723,392]],[[789,338]],[[881,267]],[[561,390]],[[458,334]],[[321,259]],[[590,380]],[[511,306]],[[390,191]],[[757,506]],[[871,578]],[[1041,689]],[[722,511]],[[787,578]],[[875,671]],[[562,511]],[[461,581]],[[327,681]],[[590,506]],[[510,577]],[[388,688]]],dtype=np.float32)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints_2 = [] # 2d points in image plane.

objpoints.append(objp*1000)
imgpoints_2.append(points_2)
imgpointssinglegroup_2 = imgpoints_2[0]

#imgpointssinglegroup_undis = cv2.undistortPoints(imgpointssinglegroup,mtx,dist)
imgpointssinglegroup_undis_2 = imgpointssinglegroup_2
print(objpoints[0].shape,imgpointssinglegroup_undis_2.shape)
ret, rotation_2, translation_2 = cv2.solvePnP(objpoints[0], imgpointssinglegroup_undis_2, mtx_2, dist_2)
R_mtx_2 = cv2.Rodrigues(rotation_2)[0]
pmat_2 = np.column_stack((R_mtx_2,translation_2))
print("R: ",rotation_2)
print("Rmtx: ",R_mtx_2)
print("T: ",translation_2)

imgpoints2, _ = cv2.projectPoints(objpoints[0], rotation_2, translation_2, mtx_2, dist_2)
error = cv2.norm(imgpointssinglegroup_undis_2,imgpoints2, cv2.NORM_L2)/len(imgpoints2)
print("total error: ", error)


print("----------------19356007-----------------")#0,you
id3 = 19356007
mtx_3 = np.array([[3.25369384e+03 ,0.00000000e+00 ,9.38636095e+02],
 [0.00000000e+00 ,3.25583436e+03 ,5.70734462e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_3 = np.array([[-9.46398539e-01]  ,[1.93700145e+01] ,[-4.01596453e-03] ,[-1.28586025e-03],[-5.20072643e+02]])
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.array([[0.8021,0.6890,1.0501],[0.4615,0.4000,1.2796],[-0.0358,-0.0188,1.6063],[0.8095,1.2067,1.0496],[0.4743,1.4983,1.2833],[-0.0189,1.9240,1.6090],[1.4159,1.2045,1.0442],[1.7622,1.4936,1.2601],[2.2697,1.9143,1.5651],[1.4183,0.6867,1.0532],[1.7558,0.3958,1.2844],[2.2515,-0.0242,1.6137],[0.8038,0.6895,0.6182],[0.4732,0.4085,0.3692],[0.0000,0.0000,0.0000],[0.8115,1.2007,0.6173],[0.4779,1.4798,0.3682],[0.0000,1.8878,0.0000],[1.4075,1.1952,0.6101],[1.7376,1.4692,0.3489],[2.2130,1.8652,-0.0369],[1.4108,0.6858,0.6171],[1.7346,0.3986,0.3662],[2.2048,-0.0176,0.0000]],dtype=np.float32)
points_3 = np.array([[[1019,340]],[[1118,267]],[[1271,161]],[[1013,350]],[[1097,293]],[[1212,219]],[[848,354]],[[760,300]],[[641,230]],[[847,343]],[[746,272]],[[594,168]],[[1019,461]],[[1119,531]],[[1268,638]],[[1012,465]],[[1099,530]],[[1213,621]],[[852,469]],[[767,540]],[[654,639]],[[849,465]],[[752,535]],[[605,644]]],dtype=np.float32)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints_3 = [] # 2d points in image plane.

objpoints.append(objp*1000)
imgpoints_3.append(points_3)
imgpointssinglegroup_3 = imgpoints_3[0]

#imgpointssinglegroup_undis = cv2.undistortPoints(imgpointssinglegroup,mtx,dist)
imgpointssinglegroup_undis_3 = imgpointssinglegroup_3
print(objpoints[0].shape,imgpointssinglegroup_undis_3.shape)
ret, rotation_3, translation_3 = cv2.solvePnP(objpoints[0], imgpointssinglegroup_undis_3, mtx_3, dist_3)
print(ret)
R_mtx_3 = cv2.Rodrigues(rotation_3)[0]
pmat_3 = np.column_stack((R_mtx_3,translation_3))
print("R: ",rotation_3)
print("Rmtx: ",R_mtx_3)
print("T: ",translation_3)

imgpoints2, _ = cv2.projectPoints(objpoints[0], rotation_3, translation_3, mtx_3, dist_3)
error = cv2.norm(imgpointssinglegroup_undis_3,imgpoints2, cv2.NORM_L2)/len(imgpoints2)
print("total error: ", error)

tmp = {"cameras":[{"id":str(id1),"kmat":list(chain.from_iterable(mtx_1.tolist())),"dvec":list(chain.from_iterable(dist_1)),"pmat":list(chain.from_iterable(pmat_1))},{"id":str(id2),"kmat":list(chain.from_iterable(mtx_2.tolist())),"dvec":list(chain.from_iterable(dist_2)),"pmat":list(chain.from_iterable(pmat_2))},{"id":str(id3),"kmat":list(chain.from_iterable(mtx_3.tolist())),"dvec":list(chain.from_iterable(dist_3)),"pmat":list(chain.from_iterable(pmat_3))}]}

str_content=json.dumps(tmp,indent=5)
json_file.write(str_content)

棋盤格標內參，兩個相機標定外參

import numpy as np
import cv2
#import glob
from numpy import *
## image2,zhong,points1  ,image1,zuo,points,points2  ,image0,you,points3
# termination criteria
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-10)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
mtx_1 = np.array([[3.19018347e+03 ,0.00000000e+00 ,9.27630883e+02],
 [0.00000000e+00 ,3.19295595e+03 ,5.47641169e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_1 = np.array([[-7.32795198e-01] ,[-5.34250952e-01]  ,[8.76286125e-04]  ,[5.17055837e-03],[1.34585035e+02]])

mtx_2 = np.array([[3.25421978e+03 ,0.00000000e+00 ,8.85560418e+02],
 [0.00000000e+00 ,3.25543566e+03 ,5.45877711e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_2 = np.array([[-8.16821482e-01]  ,[7.07530775e+00] ,[-1.70896673e-03] ,[-9.39123422e-04] ,[-9.97045814e+01]])

mtx_3 = np.array([[3.25369384e+03 ,0.00000000e+00 ,9.38636095e+02],
 [0.00000000e+00 ,3.25583436e+03 ,5.70734462e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_3 = np.array([[-9.46398539e-01]  ,[1.93700145e+01] ,[-4.01596453e-03] ,[-1.28586025e-03],[-5.20072643e+02]])

objp = np.array([[0.8021,0.6890,1.0501],[0.4615,0.4000,1.2796],[-0.0358,-0.0188,1.6063],[0.8095,1.2067,1.0496],[0.4743,1.4983,1.2833],[-0.0189,1.9240,1.6090],[1.4159,1.2045,1.0442],[1.7622,1.4936,1.2601],[2.2697,1.9143,1.5651],[1.4183,0.6867,1.0532],[1.7558,0.3958,1.2844],[2.2515,-0.0242,1.6137],[0.8038,0.6895,0.6182],[0.4732,0.4085,0.3692],[0.0000,0.0000,0.0000],[0.8115,1.2007,0.6173],[0.4779,1.4798,0.3682],[0.0000,1.8878,0.0000],[1.4075,1.1952,0.6101],[1.7376,1.4692,0.3489],[2.2130,1.8652,-0.0369],[1.4108,0.6858,0.6171],[1.7346,0.3986,0.3662],[2.2048,-0.0176,0.0000]],dtype=np.float32)

points_3 = np.array([[[1635,330]],[[1729,260]],[[1870,158]],[[1629,339]],[[1708,284]],[[1814,213]],[[1471,339]],[[1384,284]],[[1264,212]],[[1469,329]],[[1371,257]],[[1217,150]],[[1636,449]],[[1729,520]],[[1866,629]],[[1631,454]],[[1709,521]],[[1814,611]],[[1474,455]],[[1391,525]],[[1276,621]],[[1472,452]],[[1377,521]],[[1229,630]]],dtype=np.float32)
points_2 = np.array([[[1045,387]],[[1158,327]],[[1323,243]],[[983,395]],[[1023,345]],[[1077,281]],[[845,390]],[[737,335]],[[586,259]],[[903,384]],[[853,313]],[[776,204]],[[1043,499]],[[1154,564]],[[1311,660]],[[982,502]],[[1021,563]],[[1073,649]],[[902,497]],[[852,561]],[[774,663]],[[846,500]],[[740,565]],[[595,659]]],dtype=np.float32)
points_1 = np.array([[[1368,390]],[[1478,325]],[[1640,232]],[[1333,398]],[[1396,346]],[[1481,280]],[[1177,394]],[[1075,338]],[[936,262]],[[1207,385]],[[1129,312]],[[1009,200]],[[1366,508]],[[1473,577]],[[1627,684]],[[1331,511]],[[1394,577]],[[1474,667]],[[1178,510]],[[1078,576]],[[941,672]],[[1205,505]],[[1128,572]],[[1007,679]]],dtype=np.float32)

objpoints = objp.reshape(1,objp.shape[0],-1)*1000
points_3 = points_3.reshape(1,points_3.shape[0],-1)
points_2 = points_2.reshape(1,points_2.shape[0],-1)
points_1 = points_1.reshape(1,points_1.shape[0],-1)

#M1, M2 = np.zeros((3,3)), np.zeros((3,3))
M1, M2 = np.array([[3876.38938,0,499.629394],[0,3876.38938,494.274083],[0,0,1]],dtype=np.float32), np.array([[4206.25198,0,1164.35265],[0,4206.25198,479.064956],[0,0,1]],dtype=np.float32)
d1, d2 = np.zeros((3,1)), np.zeros((3,1))
#ret= cv2.stereoCalibrate(objpoints, imgpoints_l,imgpoints_c,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_FIX_INTRINSIC+cv2.CALIB_FIX_ASPECT_RATIO)
#ret= cv2.stereoCalibrate(objpoints, imgpoints_l,imgpoints_c,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_FIX_PRINCIPAL_POINT+cv2.CALIB_FIX_FOCAL_LENGTH+cv2.CALIB_USE_INTRINSIC_GUESS)
ret= cv2.stereoCalibrate(objpoints, points_3,points_2,mtx_3,dist_3,mtx_2,dist_2,(1920,1080),None,None,None,None,cv2.CALIB_FIX_INTRINSIC)
print("右左：",ret)
ret= cv2.stereoCalibrate(objpoints, points_3,points_1,mtx_3,dist_3,mtx_1,dist_1,(1920,1080),None,None,None,None,cv2.CALIB_FIX_INTRINSIC)
print("右中",ret)
ret= cv2.stereoCalibrate(objpoints, points_1,points_2,mtx_1,dist_1,mtx_2,dist_2,(1920,1080),None,None,None,None,cv2.CALIB_FIX_INTRINSIC)
print("中左",ret)

標定球標內參、外參

import numpy as np
import cv2
#import glob
from numpy import *

# termination criteria
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-10)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)


objp = np.array([[0.8021,0.6890,1.0501],[0.4615,0.4000,1.2796],[-0.0358,-0.0188,1.6063],[0.8095,1.2067,1.0496],[0.4743,1.4983,1.2833],[-0.0189,1.9240,1.6090],[1.4159,1.2045,1.0442],[1.7622,1.4936,1.2601],[2.2697,1.9143,1.5651],[1.4183,0.6867,1.0532],[1.7558,0.3958,1.2844],[2.2515,-0.0242,1.6137],[0.8038,0.6895,0.6182],[0.4732,0.4085,0.3692],[0.0000,0.0000,0.0000],[0.8115,1.2007,0.6173],[0.4779,1.4798,0.3682],[0.0000,1.8878,0.0000],[1.4075,1.1952,0.6101],[1.7376,1.4692,0.3489],[2.2130,1.8652,-0.0369],[1.4108,0.6858,0.6171],[1.7346,0.3986,0.3662],[2.2048,-0.0176,0.0000]],dtype=np.float32)

corner_you = np.array([[[1019,340]],[[1118,267]],[[1271,161]],[[1013,350]],[[1097,293]],[[1212,219]],[[848,354]],[[760,300]],[[641,230]],[[847,343]],[[746,272]],[[594,168]],[[1019,461]],[[1119,531]],[[1268,638]],[[1012,465]],[[1099,530]],[[1213,621]],[[852,469]],[[767,540]],[[654,639]],[[849,465]],[[752,535]],[[605,644]]],dtype=np.float32)
corner_zhong = np.array([[[759,384]],[[876,314]],[[1055,212]],[[723,392]],[[789,338]],[[881,267]],[[561,390]],[[458,334]],[[321,259]],[[590,380]],[[511,306]],[[390,191]],[[757,506]],[[871,578]],[[1041,689]],[[722,511]],[[787,578]],[[875,671]],[[562,511]],[[461,581]],[[327,681]],[[590,506]],[[510,577]],[[388,688]]],dtype=np.float32)
# Arrays to store object points and image points from all the images.
corner_zuo = np.array([[[504,377]],[[627,311]],[[810,217]],[[437,385]],[[477,332]],[[532,264]],[[293,383]],[[181,326]],[[28,247]],[[355,374]],[[306,299]],[[230,182]],[[503,499]],[[623,568]],[[799,672]],[[437,503]],[[476,569]],[[530,658]],[[295,502]],[[186,572]],[[38,673]],[[355,497]],[[307,569]],[[229,680]]],dtype=np.float32)
objpoints = objp.reshape(1,objp.shape[0],-1)*1000
imgpoints_you = corner_you.reshape(1,corner_you.shape[0],-1)
imgpoints_zhong = corner_zhong.reshape(1,corner_zhong.shape[0],-1)
imgpoints_zuo = corner_zuo.reshape(1,corner_zuo.shape[0],-1)
#M1, M2 = np.zeros((3,3)), np.zeros((3,3))
M1, M2 = np.array([[3876.38938,0,499.629394],[0,3876.38938,494.274083],[0,0,1]],dtype=np.float32), np.array([[4206.25198,0,1164.35265],[0,4206.25198,479.064956],[0,0,1]],dtype=np.float32)
d1, d2 = np.zeros((3,1)), np.zeros((3,1))
#ret= cv2.stereoCalibrate(objpoints, imgpoints_l,imgpoints_c,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_FIX_INTRINSIC+cv2.CALIB_FIX_ASPECT_RATIO)
#ret= cv2.stereoCalibrate(objpoints, imgpoints_l,imgpoints_c,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_FIX_PRINCIPAL_POINT+cv2.CALIB_FIX_FOCAL_LENGTH+cv2.CALIB_USE_INTRINSIC_GUESS)
ret= cv2.stereoCalibrate(objpoints, imgpoints_zuo,imgpoints_zhong,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_USE_INTRINSIC_GUESS)
print(ret)
print("------------------")
#1中 2 左 3右 
mtx_1 = np.array([[3.19018347e+03 ,0.00000000e+00 ,9.27630883e+02],
 [0.00000000e+00 ,3.19295595e+03 ,5.47641169e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_1 = np.array([[-7.32795198e-01] ,[-5.34250952e-01]  ,[8.76286125e-04]  ,[5.17055837e-03],[1.34585035e+02]])

mtx_2 = np.array([[3.25421978e+03 ,0.00000000e+00 ,8.85560418e+02],
 [0.00000000e+00 ,3.25543566e+03 ,5.45877711e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_2 = np.array([[-8.16821482e-01]  ,[7.07530775e+00] ,[-1.70896673e-03] ,[-9.39123422e-04] ,[-9.97045814e+01]])

mtx_3 = np.array([[3.25369384e+03 ,0.00000000e+00 ,9.38636095e+02],
 [0.00000000e+00 ,3.25583436e+03 ,5.70734462e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]])
dist_3 = np.array([[-9.46398539e-01]  ,[1.93700145e+01] ,[-4.01596453e-03] ,[-1.28586025e-03],[-5.20072643e+02]])
print("-------左右---------")
#ret2= cv2.stereoCalibrate(objpoints, imgpoints_zuo,imgpoints_zhong,mtx_2,dist_2,ret[3],ret[4],(1920,1080),None,None,None,None,cv2.CALIB_FIX_INTRINSIC)
#print(ret2)
ret2= cv2.stereoCalibrate(objpoints, imgpoints_zuo,imgpoints_you,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_USE_INTRINSIC_GUESS)
print(ret2)
print("--------右中--------")
ret3= cv2.stereoCalibrate(objpoints, imgpoints_you,imgpoints_zhong,M1,d1,M2,d2,(1920,1080),None,None,None,None,cv2.CALIB_USE_INTRINSIC_GUESS)
print(ret3)

2d轉3d

import cv2
import numpy as np
point_2D1 = np.array([[17.4485,50], [709.7993,80]])
point_2D2 = np.array([[17.4382,50], [709.8409,80]])
Proj_Matrices1 = np.array([[1036.937, -22.8371, -28.3254, -5607.7], [23.0587, 1043.1, 3.1815, -633.4485], [650.4355, 373.6, -15.3504, -3706.5] ])
Proj_Matrices2 = np.array([ [1037.5, -6.9927, -10.0190, -4780.7], [6.9747, 1043.3, -5.8867, -731.9206], [644.7895, 383.4982, -3231.1,100]])

OutputArray = np.zeros((3,1))
Points_3D = cv2.triangulatePoints(Proj_Matrices1, Proj_Matrices2,point_2D1, point_2D2,OutputArray)
print(Points_3D)# 4維，w，x，y，z，真實的三維是x/w，y/w，z/w