基本矩阵：相机视角的魔法桥梁

import numpy as np
import cv2
import matplotlib.pyplot as plt

# 设置中文字体
plt.rcParams["font.family"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"]

class BasicMatrixDemo:
    """
    基本矩阵演示类：用简单的例子展示基本矩阵的计算和应用
    """
    def __init__(self):
        # 设置随机种子，确保结果可复现
        np.random.seed(42)

    def generate_correspondence_points(self, num_points=8, noise_level=0.5):
        """
        生成对应点对（模拟从3D场景投影到两个相机）
        """
        # 1. 生成随机3D点（在空间前方1-5米处）
        points_3d = np.random.rand(num_points, 3) * 4 + 1  # 1-5米
        points_3d = np.hstack([points_3d, np.ones((num_points, 1))])  # 齐次坐标

        # 2. 设置相机内参
        f = 500  # 焦距
        c = (320, 240)  # 主点
        K = np.array([[f, 0, c[0]],
                      [0, f, c[1]],
                      [0, 0, 1]])

        # 3. 设置相机外参（假设相机1在原点，相机2在右侧0.5米处）
        R1 = np.eye(3)
        t1 = np.zeros(3)
        R2 = np.eye(3)
        t2 = np.array([0.5, 0, 0])

        # 4. 投影到两个相机
        P1 = K @ np.hstack([R1, t1.reshape(3, 1)])
        P2 = K @ np.hstack([R2, t2.reshape(3, 1)])

        # 投影计算
        points1_hom = P1 @ points_3d.T
        points1 = (points1_hom[:2, :] / points1_hom[2, :]).T
        points1 += np.random.randn(num_points, 2) * noise_level  # 添加噪声

        points2_hom = P2 @ points_3d.T
        points2 = (points2_hom[:2, :] / points2_hom[2, :]).T
        points2 += np.random.randn(num_points, 2) * noise_level  # 添加噪声

        return points1.astype(np.float32), points2.astype(np.float32)

    def compute_and_visualize(self):
        """
        计算基本矩阵并可视化结果
        """
        # 生成对应点对
        points1, points2 = self.generate_correspondence_points()

        # 计算基本矩阵
        F, mask = cv2.findFundamentalMat(points1, points2, cv2.FM_8POINT)

        # 仅保留有效的点对
        points1 = points1[mask.ravel() == 1]
        points2 = points2[mask.ravel() == 1]

        print("💘 基本矩阵F已计算完成：")
        print(F)

        # 验证基本矩阵的正确性
        errors = []
        for i in range(len(points1)):
            p1 = np.append(points1[i], 1)  # 转换为齐次坐标
            p2 = np.append(points2[i], 1)

            error = np.dot(p2.T, np.dot(F, p1))
            errors.append(abs(error))

        avg_error = np.mean(errors)
        print(f"📏 平均误差: {avg_error:.6f}")

        # 可视化结果
        plt.figure(figsize=(12, 6))

        # 第一张图像
        plt.subplot(121)
        plt.title('第一张图像的点')
        plt.scatter(points1[:, 0], points1[:, 1], c='r', marker='o')
        plt.xlim(0, 640)
        plt.ylim(480, 0)  # 翻转y轴，符合图像坐标系
        plt.grid(True)

        # 第二张图像和极线
        plt.subplot(122)
        plt.title('第二张图像的点和极线')
        plt.scatter(points2[:, 0], points2[:, 1], c='b', marker='o')

        # 绘制极线
        for i in range(len(points1)):
            # 计算极线: ax + by + c = 0
            p1 = np.append(points1[i], 1)
            line = F @ p1

            # 绘制直线
            a, b, c = line
            x = np.array([0, 640])
            y = (-a * x - c) / b
            plt.plot(x, y, 'g-')

        plt.xlim(0, 640)
        plt.ylim(480, 0)  # 翻转y轴，符合图像坐标系
        plt.grid(True)

        plt.tight_layout()
        plt.show()

if __name__ == "__main__":
    print("🌟 基本矩阵演示开始 🌟")
    demo = BasicMatrixDemo()
    demo.compute_and_visualize()
    print("🎉 基本矩阵演示结束！希望你对基本矩阵有了更深的理解。")