相机标定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