如何从OpenCV计算的单应性矩阵求解相机与物体平面的夹角？

前提条件 #

你必须已知相机内参矩阵K，单应性矩阵本身无法脱离内参直接推导出空间姿态信息。

步骤分解 #

1. 归一化单应性矩阵
   先计算相机内参的逆矩阵K_inv，然后对单应性矩阵做归一化：

   import numpy as np
   # 假设K是已知的3x3相机内参矩阵
   H_inv = np.dot(np.linalg.inv(K), homography_matrix)
   

   此时H_inv的形式为[R | t/n]，其中R是旋转矩阵，t是平移向量，n是物体平面法向量的模长。

2. 提取并修正旋转矩阵
   从H_inv中取出前两列作为旋转矩阵的初始列向量r1、r2：

   r1 = H_inv[:, 0]
   r2 = H_inv[:, 1]
   

   由于单应性矩阵可能存在噪声，r1和r2不一定严格正交，需要用叉乘得到第三列并做正交化修正：

   r3 = np.cross(r1, r2)
   # 重新正交化前两列
   r1 = np.cross(r2, r3)
   # 归一化所有列，得到标准旋转矩阵R
   r1 /= np.linalg.norm(r1)
   r2 /= np.linalg.norm(r2)
   r3 /= np.linalg.norm(r3)
   R = np.column_stack((r1, r2, r3))
   

3. 计算平面夹角

   • 物体平面（假设你的dst_points位于世界坐标系的z=0平面）的法向量在世界坐标系中为n_w = [0, 0, 1]^T，转换到相机坐标系下为n_c = R @ n_w，也就是旋转矩阵的第三列r3。
   • 相机像平面的法向量在相机坐标系中为c_n = [0, 0, 1]^T（相机z轴指向拍摄方向）。

   两个平面的夹角取锐角或直角，计算方式如下：

   import math
   # 计算法向量点积的绝对值（保证夹角为锐角）
   cos_angle = abs(np.dot(c_n, r3))
   # 转换为角度
   plane_angle_deg = math.degrees(math.acos(cos_angle))
   

注意事项 #

• 如果dst_points对应的物体平面不是世界坐标系z=0平面，需要替换为该平面在世界坐标系中的法向量n_w。
• 单应性矩阵的精度依赖于匹配点的质量，建议使用亚像素级精度的点对来提升结果准确性。

内容的提问来源于stack exchange，提问作者Marta Samoilenko