利用ArUco码实现相机位姿估计(Python)

这篇具有很好参考价值的文章主要介绍了利用ArUco码实现相机位姿估计(Python)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

环境:python3.7 opencv-contrib-python 4.5.1.48

一、相机标定

相机标定就是求相机的内参矩阵mtx和畸变系数dist

  1. 首先需要利用相机(你需要标定的)拍摄标定板照片,20张左右。拍摄照片的时候各个角度都拍一些,可以变换标定板的位置,但不要大幅度变换相机(不要翻转镜头,上下颠倒相机等等)。
    aruco 位姿估计,数码相机,python,opencv,人工智能
    拍摄的时候看一下照片的属性,这个分辨率需要和后边识别marker时的视频流分辨率保持一致,否侧会测距不准。(博主踩过的大坑这是)
    aruco 位姿估计,数码相机,python,opencv,人工智能
  2. 我标定的时候把重投影误差大于0.3个像素的删除了,这样标定的比较准一些。
import numpy as np
import cv2
import glob

cap = cv2.VideoCapture(1)

# 设置视频流的分辨率为 1280x720
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

# 设置用于相机标定的迭代算法的终止条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)


# 设置棋盘格的规格
chessboard_size = (11, 8)  # 棋盘格内角点数目

# 初始化对象点和图像点列表
objpoints = []  # 3D points in real world space
imgpoints = []  # 2D points in image plane.

# 准备棋盘格对象点
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)

# 遍历所有的图像文件路径
images = glob.glob('checkerboard1/*.jpg')

# 遍历所有的图像文件路径
for fname in images:
    img = cv2.imread(fname)  # 读取当前图像文件
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)  # 将当前图像转换为灰度图

    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    # 如果找到棋盘格角点
    if ret:
        # 将棋盘格角点添加到imgpoints列表中
        imgpoints.append(corners)
        # 添加相应的3D对象点
        objpoints.append(objp)



# 使用对象点和图像点进行相机标定,得到相机的内参矩阵、畸变系数以及旋转矩阵和平移矩阵
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

# 剔除重投影误差大于0.3的图像
new_objpoints = []
new_imgpoints = []
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)
    if error <= 0.3:
        new_objpoints.append(objpoints[i])
        new_imgpoints.append(imgpoints[i])

# 使用剔除后的点重新进行相机标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(new_objpoints, new_imgpoints, gray.shape[::-1], None, None)

# 计算重投影误差
mean_error = 0
for i in range(len(new_objpoints)):
    imgpoints2, _ = cv2.projectPoints(new_objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(new_imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
    mean_error += error

print("\剔除后相机内参:")
print(mtx)
print("\剔除后畸变系数")
print(dist)
# 输出重投影误差
print("\剔除后重投影", mean_error / len(new_objpoints))

3.输出结果
aruco 位姿估计,数码相机,python,opencv,人工智能
记住内参矩阵和畸变系数

二、计算相机位姿

1.导入库

import numpy as np
import cv2
import cv2.aruco as aruco
import math

2.捕捉视频流
设置视频流的分辨率(跟前边标定相机时拍摄照片的分辨率保持一致

cap = cv2.VideoCapture(1)

# 设置视频流的分辨率为 1280x720
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

3.ArUco字典对象(我设置的是5*5)

aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_100)

4.写计算位姿的函数

def estimate_pose(frame, mtx, dist):
    # 将图像转换为灰度图
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    parameters = aruco.DetectorParameters_create()

    # 检测图像中的ArUco标记
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters, cameraMatrix=mtx, distCoeff=dist)
    font = cv2.FONT_HERSHEY_SIMPLEX

    # 如果检测到ArUco标记
    if ids is not None and len(ids) > 0:
        for i in range(len(ids)):  # 遍历每个检测到的ArUco标记
            # 估计ArUco标记的姿态(旋转向量和平移向量)
            rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners[i], 0.10, mtx, dist)

            R, _ = cv2.Rodrigues(rvec)  # 将旋转向量(rvec)转换为旋转矩阵(R)3*3
            R_inv = np.transpose(R)  # 旋转矩阵是正交矩阵,所以它的逆矩阵等于它的转置 R_inv 表示从相机坐标系到标记坐标系的旋转。
            if tvec.shape != (3, 1):
                tvec_re = tvec.reshape(3, 1)
                #print('-------tvec_re------')
                #print(tvec_re)

            tvec_inv = -R_inv @ tvec_re
            tvec_inv1 = np.transpose(tvec_inv)
            pos = tvec_inv1[0]
            print(pos)
            distance = math.sqrt(pos[0]**2 + pos[1]**2 + pos[2]**2)
            rad = pos[0]/pos[2]
            angle_in_radians = math.atan(rad)
            angle_in_degrees = math.degrees(angle_in_radians)


            # 绘制ArUco标记的坐标轴
            cv2.drawFrameAxes(frame, mtx, dist, rvec, tvec, 0.05)

            tvec_inv_str = " ".join([f"{num:.2f}m" for num in tvec_inv1.flatten()])
            cv2.putText(frame, "tvec_inv: " + tvec_inv_str, (0, 80), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
            cv2.putText(frame, 'distance:' + str(round(distance, 4)) + str('m'), (0, 110), cv2.FONT_HERSHEY_SIMPLEX, 1,
                        (0, 255, 0), 2,
                        cv2.LINE_AA)
            cv2.putText(frame, "degree: " + str(angle_in_degrees), (0, 150), font, 1, (0, 0, 255), 2, cv2.LINE_AA)

            # 获取ArUco标记相对于相机坐标系的位置
            # pos_str_cam = f"X: {tvec_inv1[0][0][0]:.2f}m, Y: {tvec_inv1[0][0][1]:.2f}m, Z: {tvec[0][0][2]:.2f}m"

            # 在图像上标注ArUco标记的相对于相机坐标系的位置信息
            # cv2.putText(frame, pos_str_cam, (int(corners[i][0][0][0]), int(corners[i][0][0][1]) - 10),
            #             font, 0.5, (0, 255, 0), 1, cv2.LINE_AA)



        # 在图像上绘制检测到的ArUco标记
        aruco.drawDetectedMarkers(frame, corners)

    # 如果没有检测到ArUco标记,则在图像上显示"No Ids"
    else:
        cv2.putText(frame, "No Ids", (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA)

    return frame  # 返回处理后的图像


注意:aruco.estimatePoseSingleMarkers()函数输出的rvec和tvec是marker的中点相对于相机坐标系的旋转向量和平移向量。而我们要求的是相机相对于marker坐标系的位姿。所以需要将rvec和tvec进行一下坐标系变换。如下:

 			R, _ = cv2.Rodrigues(rvec)  # 将旋转向量(rvec)转换为旋转矩阵(R)3*3
            R_inv = np.transpose(R)  # 旋转矩阵是正交矩阵,所以它的逆矩阵等于它的转置 R_inv 表示从相机坐标系到标记坐标系的旋转。
            if tvec.shape != (3, 1):
                tvec_re = tvec.reshape(3, 1)
                #print('-------tvec_re------')
                #print(tvec_re)

            tvec_inv = -R_inv @ tvec_re
            tvec_inv1 = np.transpose(tvec_inv)
            pos = tvec_inv1[0]
            print(pos)
while True:
    ret, frame = cap.read()
    if not ret:
        break
    mtx = np.array([
        [711.77689507, 0, 672.53236606],
        [0, 711.78573804, 313.37884074],
        [0, 0, 1],
    ])
    dist = np.array( [1.26638295e-01, -1.16132908e-01, -2.24690373e-05, -2.25867957e-03, -6.76164003e-02] )
    # 调用estimate_pose函数对当前帧进行姿态估计和标记检测
    frame = estimate_pose(frame, mtx, dist)
    new_width = 1280
    new_height = 720
    frame = cv2.resize(frame, (new_width, new_height))
    cv2.imshow('frame', frame)

    # 等待按键输入,如果按下键盘上的'q'键则退出循环
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放VideoCapture对象
cap.release()
cv2.destroyAllWindows()

三、效果

aruco 位姿估计,数码相机,python,opencv,人工智能文章来源地址https://www.toymoban.com/news/detail-861387.html

到了这里,关于利用ArUco码实现相机位姿估计(Python)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • aruco二维码检测原理详解与基于opencv的代码实现(自己详细整理)

    aruco二维码检测原理讲解及基于opencv的代码和ros功能包实现 20240403 aruco二维码介绍 aruco又称为aruco标记、aruco标签、aruco二维码等,其中 CharucoBoard GridBoard AprilTag 原理相通,只是生成字典不同,而AprilTag用于机器人领域或可编程摄像头比较多,而aruco CharucoBoard GridBoard则用于AR应用

    2024年04月13日
    浏览(9)
  • 通过Apriltag码估计物体在相机坐标系下的位姿

    下面的python脚本实现了通过Apriltag码(tag16h5)估计物体在相机坐标系下的位姿的功能。 世界坐标系原点为id=10的tag中心,物体坐标系各坐标轴朝向和世界坐标系相同,仅存在一个平移变换obj2world_T。 参考:Apriltag使用之二:方位估计(定位)

    2024年02月14日
    浏览(10)
  • OpenCV学习——ArUco模块

            ArUco模块是OpenCV的contrib拓展库中一个模块,需要安装OpenCV的 contrib拓展库 才能正常使用。         ArUco 标记是由 宽黑色边框 和 确定其标识符(id)的内部二进制矩阵 组成的正方形标记,如图1所示。通俗地说,ArUco标记其实就是一种编码,就和我们日常生活中

    2024年02月17日
    浏览(12)
  • ArUco

       姿态估计 (Pose estimation)在 计算机视觉 领域扮演着十分重要的角色:如机器人导航、目标跟踪和相机定标等。所谓姿态估计问题就是要确定某个三维物体的方位指向问题,也就是确定以该物体为中心原点的一个坐标系。这一过程的基础是找到现实世界和图像投影之间的

    2024年02月16日
    浏览(25)
  • easy_aruco包代码逻辑梳理

    easy_aruco源码 OpenCV tutorial 请配合源码阅读该文章 ROS 节点的配置 这里面设置了easy_aruco_node的一些参数,例如类型、相机的命名空间、参照系等等,这里是他的用法,接下来进入逻辑的分析 在easy_aruco/src下存在该文件,根据上面的launch文件的配置,设置了 object_type ,并且根据

    2023年04月11日
    浏览(6)
  • Aruco标签定位、测距、三维位置估算 opencv应用

       准备: + aruco标签6*6 + 相机标定文件(参考我的上篇文章) + 摄像头 + 摄像头要一开始对准aruco标签才开始运行

    2024年02月14日
    浏览(13)
  • No module named “cv2.aruco“

    先贴上参考链接 这个不用说,官方发布的标准版 OpenCV-Python-Headless是一种无界面的版本,可用于运行在服务器上的计算机视觉应用程序。它的用法和OpenCV-Python类似,但是在使用时不会显示图像。这对于运行在无图形界面的服务器上的应用程序很方便,因为它可以节省资源。参

    2024年02月05日
    浏览(12)
  • JAKA+aruco+realsense+眼在手外的手眼标定

    (机械臂的运行节点商家一般会给,注意看使用说明里面的信息,容易踩雷)     用户使用说明里面会告诉你表示机器人位姿的(我的是toolpoint,这里有个大雷,后面说) (老师的驱动我用不了,就略过了,只是用aruco和calibrate两部分,眼在手外和手上的都能用,老师的是

    2024年02月21日
    浏览(14)
  • 机器学习与图像识别(二)—— OpenCV中aruco算法的坑

    OpenCV是一个优秀的开源视觉处理软件框架,也是计算机视觉学习道路上必须掌握的一套工具,奈何其版本兼容性上实在是一言难尽。本文主要介绍不同版本下使用aruco算法遇到的坑与排雷方法。如果你也刚好遇到类似的问题并且靠着本文的处理方式解决掉你的麻烦,不妨搜一

    2024年03月22日
    浏览(16)
  • AttributeError: module ‘cv2.aruco‘ has no attribute ‘GridBoard_create‘报错解决

    使用Opencv的Python版本,运行: 报错: 网上查了很多,说是把opencv版本降低。其实这就是版本的问题, pip show opencv-python 发现: 上述函数接口在4.7.x版本之后有一些变化,改变接口函数即可,无需降低版本等操作。 改变函数即可,不需变化版本: 其中,frame为之前的imread读入

    2024年02月11日
    浏览(27)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包