python+opencv+棋盘格实现相机标定及相对位姿估计

server/2024/12/24 20:57:42/

python+opencv+棋盘格实现相机标定及相对位姿估计

  • 引言
  • 1,使用相机采集含棋盘格图像14张
  • 2,进行相机标定
    • (1)测试软件1标定结果(内参及畸变系数)
    • (2)测试软件2标定结果(内参及畸变系数)
    • (3)Python算法标定及原点位姿获取
      • (3-1)python代码如下(此代码经修改验证,以下方为准)
      • (3-2)标定及位姿结果如下

引言

  相机标定的主要目的是为了求相机的内参及畸变系数等参数,通过相机的内参等可以将像素坐标转换为相机坐标,相机坐标又可通过变换得到世界坐标。
  下面为了验证标定算法的准确性,采用了同一组棋盘格数据,通过与另外两个测试软件标定结果进行对比,其标定结果基本一致。

1,使用相机采集含棋盘格图像14张

在这里插入图片描述

2,进行相机标定

(1)测试软件1标定结果(内参及畸变系数)

https://blog.csdn.net/qq_42951560/article/details/126248810
【原创工具 | OpenCV-CamCalib】一个基于 OpenCV 的自动化相机数据采集和标定程序
在这里插入图片描述

R: 
[0.0013, -0.0684, 1.5544]
[0.0313, -0.0572, 0.6501]
[0.0561, -0.0275, -0.6579]
[-0.0813, -0.1536, -1.4020]
[0.2893, -0.1042, -2.0699]
[-0.1846, -0.0407, -2.1670]
[0.2581, -0.0658, 3.0851]
[0.4249, 0.0644, -3.0719]
[-0.3689, 0.0267, 2.5249]
[0.1895, 0.0745, 1.8568]
[0.1489, -0.2197, 1.6593]
[0.1603, -0.1441, 1.0397]
[-0.2111, -0.0610, -0.0107]
[0.2734, -0.0441, 0.0333]
T: 
[57.2825, -164.7185, 657.7062]
[-39.4535, -154.2506, 653.6776]
[-125.4226, -36.9211, 655.3406]
[-114.6057, 114.1176, 616.0881]
[-71.4379, 43.0685, 624.4835]
[56.3861, 127.9344, 618.4703]
[162.7121, 26.3284, 619.2786]
[25.0943, 24.0527, 654.0335]
[61.2546, 46.1862, 655.6716]
[165.0184, -153.1011, 611.0288]
[64.0926, -122.1281, 613.9142]
[7.0449, -136.1629, 608.7691]
[-115.8937, 13.3168, 642.5956]
[-97.2399, -115.5981, 610.6633]

(2)测试软件2标定结果(内参及畸变系数)

https://blog.csdn.net/Big_Huang/article/details/106166254
基于opencv 和 pyqt5 的相机标定助手的设计
在这里插入图片描述

相机内部矩阵:
[[1.09803736e+03 0.00000000e+00 6.63737765e+02]
[0.00000000e+00 1.09791044e+03 4.38657425e+02]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
畸变参数:
[[ 0.00218144 -0.00925042 0.00024789 -0.0002825 0.20841267]]
旋转矩阵:
[array([[ 1.33903055e-03],
[-6.83598890e-02],
[ 1.55438056e+00]]), array([[ 0.03125883],
[-0.05715461],
[ 0.65011621]]), array([[ 0.05605446],
[-0.02753977],
[-0.65790633]]), array([[-0.08126108],
[-0.15363931],
[-1.40200001]]), array([[ 0.28931372],
[-0.10420944],
[-2.06993946]]), array([[-0.18457149],
[-0.0407243 ],
[-2.16699213]]), array([[ 0.25805958],
[-0.06580844],
[ 3.08507844]]), array([[ 0.42490026],
[ 0.06435579],
[-3.07186989]]), array([[-0.36890203],
[ 0.02674808],
[ 2.52486102]]), array([[0.18946187],
[0.07454542],
[1.85677179]]), array([[ 0.14889827],
[-0.21970662],
[ 1.65930809]]), array([[ 0.16029379],
[-0.14407197],
[ 1.03966592]]), array([[-0.21111973],
[-0.06096786],
[-0.01069444]]), array([[ 0.27336398],
[-0.04413062],
[ 0.03325271]])]
平移矩阵:
[array([[ 57.28312236],
[-164.71792893],
[ 657.70732026]]), array([[ -39.45288044],
[-154.25005049],
[ 653.67874071]]), array([[-125.42200989],
[ -36.92050016],
[ 655.34164677]]), array([[-114.60511114],
[ 114.11816637],
[ 616.08898813]]), array([[-71.43727126],
[ 43.06902124],
[624.48437836]]), array([[ 56.38668594],
[127.93492448],
[618.4712321 ]]), array([[162.7126308 ],
[ 26.32896621],
[619.27933957]]), array([[ 25.09489496],
[ 24.05330674],
[654.03439771]]), array([[ 61.25519273],
[ 46.18683531],
[655.67243968]]), array([[ 165.01894101],
[-153.10050991],
[ 611.02969995]]), array([[ 64.09318336],
[-122.12753319],
[ 613.9153159 ]]), array([[ 7.04548205],
[-136.16238216],
[ 608.7700887 ]]), array([[-115.89311233],
[ 13.31740073],
[ 642.59649204]]), array([[ -97.23935687],
[-115.59753028],
[ 610.66435603]])]

(3)Python算法标定及原点位姿获取

参考1:
https://blog.csdn.net/qq_29931565/article/details/119395353
【OpenCV】OpenCV-Python实现相机标定+利用棋盘格相对位姿估计

在这里插入图片描述

参考2:

python_135">(3-1)python代码如下(此代码经修改验证,以下方为准)

import numpy as np
import glob
import cv2
import math#当前验证此算法的标定结果与其他标定基本一致#1,相机标定获取内参及畸变系数
#角点个数
w = 11
h = 8
b_w = 20  #棋盘格边长20mm
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)objp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)  # 将世界坐标系建在标定板上,所有点的Z坐标全部为0,所以只需要赋值x和y
objp = b_w * objp  # 打印棋盘格一格的边长为2.6cm
#print(objp)
obj_points = []  # 存储3D点
img_points = []  # 存储2D点#images = glob.glob("E:/image/*.png")  # 黑白棋盘的图片路径def get_image_paths(folder_path):# 使用通配符筛选出所有jpg/png图片return glob.glob(f"{folder_path}/**/*.jpg", recursive=True)# 如果需要包括其他格式的图片,可以在这里添加,例如:png# return glob.glob(f"{folder_path}/**/*.jpg", recursive=True) + \#        glob.glob(f"{folder_path}/**/*.png", recursive=True)# 使用示例
#folder_path = "G:/3dversion/weiziguji/8mm/"  # 替换为你的文件夹路径
folder_path = "C:\\Users\\zhaocai\\Pictures\\test"
images = get_image_paths(folder_path)size = None
for fname in images:img = cv2.imread(fname)gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)size = gray.shape[::-1]ret, corners = cv2.findChessboardCorners(gray, (w, h), None)if ret:obj_points.append(objp)     #世界坐标系中的三维点始终不变#此处的winsize(会影响到畸变系数)corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),(cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))#寻找棋盘格角点,若是有则进行保存if [corners2]:img_points.append(corners2)else:img_points.append(corners)cv2.drawChessboardCorners(img, (w, h), corners, ret)  # 记住,OpenCV的绘制函数一般无返回值#cv2.imshow("demo",img)#cv2.waitKey(0)# print(obj_points)
# print(img_points)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
result = "摄像机矩阵:\n {}\n 畸变参数:\n {}\n 旋转矩阵:\n {}\n 平移矩阵:\n {}".format(mtx, dist, rvecs, tvecs)
print(result)# 内参数矩阵、畸变系数
Camera_intrinsic = {"mtx": mtx, "dist": dist, }#2,获取当前位姿(原点位姿)
obj_points = objp  # 存储3D点
img_points = []  # 存储2D点for fname in images:#_, frame = camera.read()frame = cv2.imread(fname)gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)size = gray.shape[::-1]ret, corners = cv2.findChessboardCorners(gray, (w, h), None)if ret:  # 画面中有棋盘格img_points = np.array(corners)cv2.drawChessboardCorners(frame, (w, h), corners, ret)# rvec: 旋转向量 tvec: 平移向量_, rvec, tvec = cv2.solvePnP(obj_points, img_points, Camera_intrinsic["mtx"], Camera_intrinsic["dist"])  # 解算位姿# print(rvec)#print(tvec)distance = math.sqrt(tvec[0][0] ** 2 + tvec[1][0] ** 2 + tvec[2][0] ** 2)  # 计算距离,距离相机的距离#将旋转向量转换成欧拉角(绕x轴转动pitch,绕y轴转动yaw,绕z轴转动roll)rvec_matrix = cv2.Rodrigues(rvec)[0]  # 旋转向量->旋转矩阵proj_matrix = np.hstack((rvec_matrix, tvec))  # hstack: 水平合并eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角#print(eulerAngles)pitch, yaw, roll = eulerAngles[0][0], eulerAngles[1][0], eulerAngles[2][0]cv2.putText(frame, "dist: %.2fmm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (distance, yaw, pitch, roll), (10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)cv2.imshow('frame', frame)if cv2.waitKey(1) & 0xFF == 27:  # 按ESC键退出breakelse:  # 画面中没有棋盘格cv2.putText(frame, "Unable to Detect Chessboard", (20, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 1.3,(0, 0, 255), 3)cv2.imshow('frame', frame)if cv2.waitKey(1) & 0xFF == 27:  # 按ESC键退出break#3,画坐标轴和立方体
len = b_w
def draw(img, corners, imgpts, imgpts2):#line必须转为int型才能绘制corners = np.int32(corners)imgpts2 = np.int32(imgpts2)corner = tuple(corners[0].ravel())  # ravel()方法将数组维度拉成一维数组# img要画的图像,corner起点,tuple终点,颜色,粗细img = cv2.line(img, corner, tuple(imgpts2[0].ravel()), (255, 0, 0), 8)img = cv2.line(img, corner, tuple(imgpts2[1].ravel()), (0, 255, 0), 8)img = cv2.line(img, corner, tuple(imgpts2[2].ravel()), (0, 0, 255), 8)font = cv2.FONT_HERSHEY_SIMPLEXcv2.putText(img, 'X', tuple(imgpts2[0].ravel() + 2), font, 1, (255, 0, 0), 2, cv2.LINE_AA)cv2.putText(img, 'Y', tuple(imgpts2[1].ravel() + 2), font, 1, (0, 255, 0), 2, cv2.LINE_AA)cv2.putText(img, 'Z', tuple(imgpts2[2].ravel() + 2), font, 1, (0, 0, 255), 2, cv2.LINE_AA)imgpts = np.int32(imgpts).reshape(-1, 2)  # draw ground floor in greenfor i, j in zip(range(4), range(4, 8)):  # 正方体顶点逐个连接img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255, 215, 0), 3)  # draw top layer in red color# imgpts[4:]是八个顶点中上面四个顶点# imgpts[:4]是八个顶点中下面四个顶点# 用函数drawContours画出上下两个盖子,它的第一个参数是原始图像,第二个参数是轮廓,一个python列表,第三个参数是轮廓的索引(当设置为-1时绘制所有轮廓)img = cv2.drawContours(img, [imgpts[4:]], -1, (255, 215, 0), 3)img = cv2.drawContours(img, [imgpts[:4]], -1, (255, 215, 0), 3)return imgobjp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w * len:len, 0:h * len:len].T.reshape(-1, 2)
axis = np.float32([[0, 0, 0], [0, 2 * len, 0], [2 * len, 2 * len, 0], [2 * len, 0, 0],[0, 0, -2 * len], [0, 2 * len, -2 * len], [2 * len, 2 * len, -2 * len], [2 * len, 0, -2 * len]])
axis2 = np.float32([[3 * len, 0, 0], [0, 3 * len, 0], [0, 0, -3 * len]]).reshape(-1, 3)
# images = glob.glob('*.jpg')
i = 1
for fname in images:img = cv2.imread(fname)# cv2.imshow('世界坐标系与小盒子', img)# cv2.waitKey(0)#img = cv2.resize(img, None, fx=0.4, fy=0.4, interpolation=cv2.INTER_CUBIC)gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)# 找到棋盘格角点# 寻找角点,存入corners,ret是找到角点的flagret, corners = cv2.findChessboardCorners(gray, (w, h), None)if ret is True:corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)#print(corners2)# 求解物体位姿的需要_, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)# projectPoints()根据所给的3D坐标和已知的几何变换来求解投影后的2D坐标。# imgpts是整体的8个顶点imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)# imgpts2是三个坐标轴的x,y,z划线终点imgpts2, _ = cv2.projectPoints(axis2, rvecs, tvecs, mtx, dist)#绘制方格img = draw(img, corners2, imgpts, imgpts2)#绘制x、y、zdistance = math.sqrt(tvec[0][0] ** 2 + tvec[1][0] ** 2 + tvec[2][0] ** 2)  # 计算距离# print(distance)rvec_matrix = cv2.Rodrigues(rvec)[0]  # 旋转向量->旋转矩阵proj_matrix = np.hstack((rvec_matrix, tvec))  # hstack: 水平合并eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角# print(eulerAngles)pitch, yaw, roll = eulerAngles[0][0], eulerAngles[1][0], eulerAngles[2][0]p0 = tuple(corners[0].ravel())cv2.putText(img, "x: %.2f, y: %.2f, dist: %.2fmm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (p0[0],p0[1],distance, yaw, pitch, roll),(10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)cv2.imshow('世界坐标系与小盒子', img)#cv2.imwrite(str(i) + '.png', img)cv2.waitKey(0)i += 1cv2.destroyAllWindows()
print("完毕")

(3-2)标定及位姿结果如下

12920

输入时w=11,h=8,b-w=20
在这里插入图片描述
在这里插入图片描述

G:\3dversion\weiziguji\.venv\Scripts\python.exe G:\3dversion\weiziguji\XiangJiBiaoDing.py 
摄像机矩阵:[[1.09803736e+03 0.00000000e+00 6.63737765e+02][0.00000000e+00 1.09791044e+03 4.38657425e+02][0.00000000e+00 0.00000000e+00 1.00000000e+00]]畸变参数:[[ 0.00218144 -0.00925043  0.00024789 -0.0002825   0.20841267]]旋转矩阵:(array([[ 1.33903054e-03],[-6.83598890e-02],[ 1.55438056e+00]]), array([[ 0.03125883],[-0.05715461],[ 0.65011621]]), array([[ 0.05605446],[-0.02753977],[-0.65790633]]), array([[-0.08126108],[-0.15363931],[-1.40200001]]), array([[ 0.28931372],[-0.10420944],[-2.06993946]]), array([[-0.18457149],[-0.0407243 ],[-2.16699213]]), array([[ 0.25805958],[-0.06580844],[ 3.08507844]]), array([[ 0.42490026],[ 0.06435579],[-3.07186989]]), array([[-0.36890203],[ 0.02674808],[ 2.52486102]]), array([[0.18946187],[0.07454542],[1.85677179]]), array([[ 0.14889827],[-0.21970662],[ 1.65930809]]), array([[ 0.16029379],[-0.14407197],[ 1.03966592]]), array([[-0.21111973],[-0.06096786],[-0.01069444]]), array([[ 0.27336398],[-0.04413062],[ 0.03325271]]))平移矩阵:(array([[  57.28312237],[-164.71792893],[ 657.70732026]]), array([[ -39.45288044],[-154.25005049],[ 653.67874071]]), array([[-125.42200989],[ -36.92050016],[ 655.34164677]]), array([[-114.60511114],[ 114.11816638],[ 616.08898813]]), array([[-71.43727126],[ 43.06902124],[624.48437836]]), array([[ 56.38668594],[127.93492448],[618.47123209]]), array([[162.7126308 ],[ 26.32896621],[619.27933956]]), array([[ 25.09489497],[ 24.05330674],[654.03439771]]), array([[ 61.25519273],[ 46.18683531],[655.67243967]]), array([[ 165.01894101],[-153.1005099 ],[ 611.02969995]]), array([[  64.09318337],[-122.12753319],[ 613.9153159 ]]), array([[   7.04548206],[-136.16238216],[ 608.7700887 ]]), array([[-115.89311233],[  13.31740073],[ 642.59649204]]), array([[ -97.23935687],[-115.59753028],[ 610.66435603]]))

在这里插入图片描述

此时可以求得x、y、z,rx(pitch)、ry(yaw)、rz(roll)的位姿

有了此位姿即可进行下一步


http://www.ppmy.cn/server/152866.html

相关文章

Android Framework 中的 AV/Camera 技术架构详解

Android Framework 中的 AV/Camera 技术架构详解 引言 Android 操作系统作为全球最流行的移动操作系统之一,其强大的多媒体处理能力是其成功的关键因素之一。其中,摄像头(Camera)模块是 Android 系统中最为核心的部分之一,负责处理图像和视频的捕获、处理和传输。本文将…

typora数学符号

typora数学符号 Typora 是一个支持 LaTeX 数学公式的优秀 Markdown 编辑器,可以直接编写数学公式并实时渲染。以下是如何在 Typora 中使用数学公式的详细指南: 1. 启用数学公式支持 默认情况下,Typora 支持 LaTeX 格式的数学公式&#xff0…

Python 练习

一、列表练习 1、求偶数元素的和[1,2,1,2,3,3,6,5,8] 1 2 3 4 5 6 list01 [1, 2, 1, 2, 3, 3, 6, 5, 8] sum 0 for i in list01: if int(i) % 2 0: sum sum i print(f"列表中所有偶数和是: {sum}") 2、计算 1 - 2 3 - 4 ... 99 中除88以外…

ElasticSearch 使用教程

ElasticSearch 使用教程 1. ElasticSearch简介 ElasticSearch是一个基于Lucene的搜索服务器,提供了一个分布式多用户能力的全文搜索引擎,基于RESTful web接口。它是用Java开发的,并作为Apache许可条款下的开放源码发布,是当前流…

Spring Boot 启动后的初始化数据加载原理解析与实战应用

系统初始化操作是一个非常常见的需求。通常,应用在启动后需要执行一些重要的初始化任务,例如加载全局配置、初始化数据库表、预热缓存、启动后台任务等。而如何选择合适的技术方案,在不同的场景下保证初始化任务的高效执行,尤其在…

【VUE】13、安装nrm管理多个npm源

nrm(npm registry manager)是一个 npm 源管理器,它允许用户快速地在不同的 npm 源之间进行切换,以提高包管理的速度和效率。以下是对 nrm 使用的详细介绍: 1、安装nrm 在使用 nrm 之前,需要先确保已经安装…

halcon单相机+机器人*眼在手外标定心得

目的 得到相机坐标系下的点与机器人底座base的转换关系,camera_in_base 两个不确定的定量 1,相机与机器人底座base之间的相对位置是固定的,既camera_in_base 2,机械手末端与标定物 tool_in_obj是固定的 辅助确定量 工作台与相…

基于微信小程序的短视频系统(SpringBoot)+文档

💗博主介绍💗:✌在职Java研发工程师、专注于程序设计、源码分享、技术交流、专注于Java技术领域和毕业设计✌ 温馨提示:文末有 CSDN 平台官方提供的老师 Wechat / QQ 名片 :) Java精品实战案例《700套》 2025最新毕业设计选题推荐…