第七章相机坐标系中的Zc值求解(车道线感知)

小酒馆燃着灯

已于 2024-01-04 18:02:42 修改

阅读量648

点赞数 3

分类专栏：车道线检测文章标签：视觉检测图像处理

于 2023-09-28 20:59:12 首次发布

本文链接：https://blog.csdn.net/weixin_44302770/article/details/133391086

版权

深度学习同时被 3 个专栏收录

213 篇文章 51 订阅

订阅专栏

手写AI

47 篇文章 6 订阅

订阅专栏

车道线检测

30 篇文章 14 订阅

订阅专栏

@[TOC](第七章相机坐标系中的Zc值求解(车道线感知))

一前言

近期参与到了手写AI的车道线检测的学习中去，以此系列笔记记录学习与思考的全过程。车道线检测系列会持续更新，力求完整精炼，引人启示。所需前期知识，可以结合手写AI进行系统的学习。

二推导 $Z_c$

像素坐标转到世界坐标时相机坐标系中的Zc值求解
坐标转换参考：https://blog.csdn.net/weixin_44302770/article/details/133024739

求解具体过程：
前提是世界坐标的原点在地面上
上述公式可以简写为：
$Z_c\begin{bmatrix}u\\v\\1\end{bmatrix}=K(R\begin{bmatrix}X_w\\Y_w\\Z_w\end{bmatrix}+T)$
进一步：
$\left.\begin{bmatrix}X_w\\Y_w\\Z_w\end{bmatrix}=R^{-1}\left(K^{-1}Z_c\begin{bmatrix}u\\v\\1\end{bmatrix}\right.-T\right)=R^{-1}K^{-1}Z_c\begin{bmatrix}u\\v\\1\end{bmatrix}-R^{-1}T$
其中 R，K，T均为已知值。将上述等式中的变量做以下改写，方便描述：
$Mat1=R^{-1}K^{-1}\Big[\begin{matrix}u\\v\\1\end{matrix}\Big],Mat2=R^{-1}T$
只观察世界坐标计算公式中等式两边的第三项，则：
$Z_w=Z_c*Mat1(2,0)-Mat2(2,0)\\\\Z_c=(Z_w+Mat2(2,0))/Mat1(2,0)$
其中，Mat2(2,0)表示3×1矩阵的第三项。如果目标物体在地面则Zw=0；如果目标物体有一定高度，则Zw=实际物体高度，求出Zc后即可根据像素坐标求得世界坐标。

三代码实现

import numpy as np
import cv2

def 2Dto3Dpts(point2D, rVec, tVec, cameraMat, height):
    """
    将给定的2D点投影回真实世界的3D坐标系中
    Args:
        point2D  (list): 包含2D点坐标的数组
        rVec     (numpy.ndarray): 旋转向量
        tVec     (numpy.ndarray): 平移向量
        cameraMat (numpy.ndarray): 在solvePnP中使用的相机矩阵
        height   (float): 在真实世界3D空间中的高度
        
    Returns:
        output_array (numpy.ndarray): 3D点的输出数组，每个点为 (x, y, z) 坐标
    
    """
    point3D = []  # 存储计算出的3D点
    point2D = (np.array(point2D, dtype='float32')).reshape(-1, 2)  # 将输入的2D点坐标转换为NumPy数组，并重塑为2列的矩阵
    numPts = point2D.shape[0]  # 获取2D点的数量
    point2D_op = np.hstack((point2D, np.ones((numPts, 1))))  # 为每个2D点添加齐次坐标 (x, y, 1)
    
    # 将旋转向量转换为旋转矩阵
    rMat = cv2.Rodrigues(rVec)[0]
    
    # 计算旋转矩阵的逆矩阵
    rMat_inv = np.linalg.inv(rMat)
    
    # 计算相机矩阵的逆矩阵
    kMat_inv = np.linalg.inv(cameraMat)
    
    for point in range(numPts):
        uvPoint = point2D_op[point, :].reshape(3, 1)  # 将2D点坐标扩展为列向量
        tempMat = np.matmul(rMat_inv, kMat_inv)  # 将旋转矩阵逆矩阵和相机矩阵逆矩阵相乘
        tempMat1 = np.matmul(tempMat, uvPoint)  # 将结果与2D点进行乘法运算
        tempMat2 = np.matmul(rMat_inv, tVec)  # 将旋转矩阵逆矩阵与平移向量相乘
        s = (height + tempMat2[2]) / tempMat1[2]  # 计算缩放因子
        p = tempMat1 * s - tempMat2  # 计算3D点坐标
        point3D.append(p)  # 将计算出的3D点添加到列表中

    point3D = (np.array(point3D, dtype='float32')).reshape([-1, 1, 3])  # 将3D点列表转换为NumPy数组，并重塑为正确的形状
    return point3D  # 返回计算出的3D点数组