pnp测距
单目相机标定
参考：https://juejin.cn/post/7119686091933745159
得到的相机参数如下：
这里关注的几个参数如划线所示，分别为：世界坐标（0.3mm），平移矩阵，相机内参，图像坐标，旋转矩阵。
其中相机内参只有一个，平移矩阵和旋转矩阵针对每幅图像各有一个。
设定世界坐标系和像素坐标系
1.为什么需要对摄像头进行标定？
摄像头存在畸变，畸变可以拓宽视野，但会影响图像识别和测量的精度。
2.摄像头参数：
1）相机矩阵：包括焦距（fx，fy），光学中心（Cx，Cy），完全取决于相机本身，是相机的固有属性，只需要计算一次，可用矩阵表示如下：[fx, 0, Cx; 0, fy, cy; 0,0,1];
2) 畸变系数：畸变数学模型的5个参数 D = （k1，k2， P1， P2， k3）；
3）相机内参：相机矩阵和畸变系数统称为相机内参，在不考虑畸变的时候，相机矩阵也会被称为相机内参；
4） 相机外参：通过旋转和平移变换将3D的坐标转换为相机2维的坐标，其中的旋转矩阵和平移矩阵就被称为相机的外参；描述的是将世界坐标系转换成相机坐标系的过程。
3.摄像头标定的流程：
相机的标定过程实际上就是在4个坐标系转化的过程中求出相机的内参和外参的过程。这4个坐标系分别是：世界坐标系（描述物体真实位置），相机坐标系（摄像头镜头中心），图像坐标系（图像传感器成像中心，图片中心，影布中心，单位mm），像素坐标系（图像左上角为原点，描述像素的位置，单位是多少行，多少列）。
（1）世界坐标系 → 相机坐标系：等比例缩小，外加旋转平移，称之为刚体变换；求解摄像头外参（旋转和平移矩阵）；
（2）相机坐标系 → 图像坐标系：称为投影；求解相机内参（摄像头矩阵和畸变系数）；
（3）图像坐标系 → 像素坐标系：将图像坐标离散抽样；求解像素转化矩阵（可简单理解为原点从图片中心到左上角，单位厘米变行列）
4.相机标定方法分类：
传统相机标定法、主动视觉相机标定法、相机自标定法。张正友相机标定法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点。张氏标定法使用二维方格组成的标定板进行标定，采集标定板不同位姿图片，提取图片中角点像素坐标，通过单应矩阵计算出相机的内外参数初始值，利用非线性最小二乘法估计畸变系数，最后使用极大似然估计法优化参数。该方法操作简单，而且精度较高，可以满足大部分场合。
————————————————
版权声明：本文为CSDN博主「不雨_亦潇潇」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_43470383/article/details/126206488
代码
#!/usr/bin/env python
import cv2
import numpy as np
import math
# 642.0942 419.8089
# 641.3379 307.9839
# 460.9779 421.5323
# 460.3078 308.0075
# 0 0
# 0 50
# 80 10
# 80 50
# 自己的图像
# 3s
# 0 0
# 0 1071
# 1071 0
# 1071 1071
# 779.8810 485.0207
# 781.0772 568.9719
# 867.3904 483.1395
# 869.1147 567.1812
#世界坐标系 中间定位原点 物体实际大小 单位mm
point3s = np.array(([40,-25,0],[40,25,0],[-40,25,0],[-40,-25,0]),dtype=np.double)
# 像素坐标系。可以用ps识别 就是四点的行列坐标，四个点顺序要跟世界坐标对应
point2s = np.array(([642.0942,419.8089],[641.3379,307.9839],[460.3078,308.0075],[460.9779,421.5323],),dtype=np.double)
# point3s=np.array(([0, 0, 0],[0, 1071, 0],[1071,0, 0],[1071,1071,0]),dtype=np.double)
# point2s=np.array(([779.8118,485.1100],[781.0983,568.9147],[867.0161,483.2972],[868.8392,567.2364]),dtype=np.double)
# 相机内参矩阵 matlab求的需要转制 0
camera = np.array(([849.793090724164,0.457277808187144,529.860900052561],[0,843.036601182289,373.801676979730],[0,0,1]),dtype=np.double)
# 1592.31745225552 0 0
# 57.5714671473469 1463.09510100772 0
# 1047.46009281314 310.456147316488 1
# camera=np.array(([ 1592.31745225552, 57.5714671473469, 1047.46009281314],[0,1463.09510100772, 310.456147316488],[0,0,1]),dtype=np.double)
# 相机畸变系数 k1 k2 k3 p1 p2
dist = np.array(([-0.193812203601170,0.670542577007099,0,0.000329939295738533,-0.00140279835685299]),dtype=np.double)
# dist=np.array(([-0.265646657440131,1.33169555991469,0, 0.132817697344621,-0.00923216493667286]),dtype=np.double)
#dist=dist.T
#dist=np.zeros((5,1))
# found,r,t=cv2.solvePnP(point3s,point2s,camera,dist,flags=cv2.SOLVEPNP_SQPNP) #计算雷达相机外参,r-旋转向量，t-平移向量
found,r,t=cv2.solvePnP(point3s,point2s,camera,dist) #计算雷达相机外参,r-旋转向量，t-平移向量
R=cv2.Rodrigues(r)[0] #旋转向量转旋转矩阵
# -0.9982 0.0056 - 0.0589
# -0.0056 - 1.0000 - 0.0004
# -0.0589 - 0.0001 0.9983
# R = np.array(([-0.9982,-0.0056,-0.0589],[0.0056,-1.0000,-0.0001],[-0.0589,-0.0589,0.9983]))
# t = np.array(([49.7671288089843],[20.5618256699387],[375.298657782898]))
#
# 0.9999 0.0081 -0.0106
# -0.0067 0.9919 0.1267
# 0.0116 -0.1266 0.9919
# R = np.array(([0.9999,-0.0067,0.0116],[0.0081,0.9919,-0.1266],[-0.0106,0.1267,0.9919]))
# -3340.24759865471 2212.59138840833 19889.3443428461
# t = np.array(([-3340.24759865471],[2212.59138840833],[19889.3443428461]))
camera_position=-np.matrix(R).T*np.matrix(t) #相机位置
print(camera_position) #camera_position[2]就是距离
d3=np.array([[-3.14925, -1.54094, -1.06652]])
d2,_=cv2.projectPoints(d3,r,t,camera,dist)#重投影验证
print(r)
print(t)
print(d2)
参考 [https://blog.csdn.net/cocoaqin/article/details/77848588]
(https://blog.csdn.net/cocoaqin/article/details/77848588)
https://www.cnblogs.com/zyly/p/9366080.html#_label0