{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "3cd339c6",
   "metadata": {
    "papermill": {
     "duration": 0.005976,
     "end_time": "2023-08-15T11:35:50.240316",
     "exception": false,
     "start_time": "2023-08-15T11:35:50.234340",
     "status": "completed"
    },
    "tags": []
   },
   "source": [
    "## 代码实现\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "961bf9d5",
   "metadata": {
    "papermill": {
     "duration": 0.006026,
     "end_time": "2023-08-15T11:35:50.252646",
     "exception": false,
     "start_time": "2023-08-15T11:35:50.246620",
     "status": "completed"
    },
    "tags": []
   },
   "source": [
    "上面我们已经了解了对极约束的基本概念和数学推导，以及如何通过对极约束计算基础矩阵和进行三角测量。接下来这一节我们将动手学如何基于两张给定的不同视角图像计算它们之间的基础矩阵并进行三角测量。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "4e460945",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:50.267464Z",
     "iopub.status.busy": "2023-08-15T11:35:50.266994Z",
     "iopub.status.idle": "2023-08-15T11:35:52.232906Z",
     "shell.execute_reply": "2023-08-15T11:35:52.230915Z"
    },
    "papermill": {
     "duration": 1.976712,
     "end_time": "2023-08-15T11:35:52.235767",
     "exception": false,
     "start_time": "2023-08-15T11:35:50.259055",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Cloning into 'Hands-on-CV'...\n",
      "remote: Enumerating objects: 9, done.\u001b[K\n",
      "remote: Counting objects: 100% (9/9), done.\u001b[K\n",
      "remote: Compressing objects: 100% (4/4), done.\u001b[K\n",
      "remote: Total 9 (delta 1), reused 5 (delta 0), pack-reused 0 (from 0)\u001b[K\n",
      "Receiving objects: 100% (9/9), done.\n",
      "Resolving deltas: 100% (1/1), done.\n"
     ]
    }
   ],
   "source": [
    "# 首先clone对应仓库\n",
    "!git clone https://github.com/boyu-ai/Hands-on-CV.git"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "e0f33750",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:52.254528Z",
     "iopub.status.busy": "2023-08-15T11:35:52.253793Z",
     "iopub.status.idle": "2023-08-15T11:35:52.577554Z",
     "shell.execute_reply": "2023-08-15T11:35:52.576253Z"
    },
    "papermill": {
     "duration": 0.336552,
     "end_time": "2023-08-15T11:35:52.580361",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.243809",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "import os, cv2, random\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "%matplotlib inline\n",
    "\n",
    "class FeatureExtractor:\n",
    "    \"\"\"\n",
    "    构建类用于提取SIFT特征。\n",
    "    \"\"\"\n",
    "    def __init__(self, image):\n",
    "        \"\"\"\n",
    "        初始化FeatureExtractor类。\n",
    "        \n",
    "        参数:\n",
    "        - image: (np.ndarray): RGB 形式的图像。\n",
    "        \"\"\"\n",
    "        self.image = image\n",
    "        # 将图像转换为灰度图\n",
    "        self.gray = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)\n",
    "        self.keypoints = None\n",
    "        self.descriptors = None\n",
    "\n",
    "    def extract_features(self):\n",
    "        \"\"\"\n",
    "        使用OpenCV的内置函数提取图像中的SIFT特征。\n",
    "        \n",
    "        返回:\n",
    "        - keys: (list): 特征点列表。\n",
    "        - descriptors: (np.ndarray): 描述子。\n",
    "        \"\"\"\n",
    "        sift = cv2.SIFT_create()\n",
    "        keys, descriptors = sift.detectAndCompute(self.gray, None)\n",
    "\n",
    "        # 如果特征点数量小于20个，返回None\n",
    "        if len(keys) <= 20:\n",
    "            return None, None\n",
    "        else:\n",
    "            self.keypoints = keys\n",
    "            self.descriptors = descriptors\n",
    "            return keys, descriptors\n",
    "\n",
    "def vis_imgs(imgs):\n",
    "    \"\"\"\n",
    "    可视化多张图像。\n",
    "\n",
    "    参数:\n",
    "    - imgs: 要可视化的图像列表。\n",
    "    \"\"\"\n",
    "    # 计算图像数量\n",
    "    num_imgs = len(imgs)\n",
    "    # 创建画布\n",
    "    fig = plt.figure(figsize=(10, 10))\n",
    "    # 遍历每一张图像\n",
    "    for i in range(num_imgs):\n",
    "        # 添加子图\n",
    "        ax = fig.add_subplot(1, num_imgs, i + 1)\n",
    "        # 显示图像\n",
    "        ax.imshow(imgs[i])\n",
    "        # 关闭坐标轴\n",
    "        ax.axis('off')\n",
    "    # 显示画布\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e2d7342f",
   "metadata": {
    "papermill": {
     "duration": 0.007309,
     "end_time": "2023-08-15T11:35:52.595266",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.587957",
     "status": "completed"
    },
    "tags": []
   },
   "source": [
    "为了方便，这里直接提供了照相机的内参（来自照相机本身的数据），当然也可以用在第 16 章中教过的标定获得的内参数据。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "0f3f53eb",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:52.612338Z",
     "iopub.status.busy": "2023-08-15T11:35:52.611754Z",
     "iopub.status.idle": "2023-08-15T11:35:52.619428Z",
     "shell.execute_reply": "2023-08-15T11:35:52.618129Z"
    },
    "papermill": {
     "duration": 0.018802,
     "end_time": "2023-08-15T11:35:52.621681",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.602879",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "def load_instrinsic(path):\n",
    "    \"\"\"\n",
    "    从文件中直接读取内参。\n",
    "    \"\"\"\n",
    "    return np.loadtxt(path).astype(np.float32)\n",
    "\n",
    "def construct_img_info(img_root):\n",
    "    \"\"\"\n",
    "    从图像中获取特征信息。\n",
    "    \"\"\"\n",
    "    imgs = []\n",
    "    feats = []\n",
    "    K = []\n",
    "    for _, name in enumerate(os.listdir(img_root)):\n",
    "        if '.jpg' in name or '.JPG' or '.png' in name:\n",
    "            # 读取图像\n",
    "            path = os.path.join(img_root, name)\n",
    "            img = cv2.cvtColor(cv2.imread(path), cv2.COLOR_BGR2RGB)\n",
    "            imgs.append(img)\n",
    "\n",
    "            # 提取特征\n",
    "            feature_ext = FeatureExtractor(img)\n",
    "            kpt, des = feature_ext.extract_features()\n",
    "\n",
    "            # 读取内参\n",
    "            K = load_instrinsic(os.path.join(\n",
    "                os.path.dirname(img_root), 'K.txt'))\n",
    "            feats.append({'kpt': kpt, 'des': des})\n",
    "    return imgs, feats, K"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b0ed316a",
   "metadata": {
    "papermill": {
     "duration": 0.008034,
     "end_time": "2023-08-15T11:35:52.637095",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.629061",
     "status": "completed"
    },
    "tags": []
   },
   "source": [
    "接下来我们计算两张图像间的对应关系，查询并匹配两者之间的特征。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "c8005eb6",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:52.656218Z",
     "iopub.status.busy": "2023-08-15T11:35:52.655674Z",
     "iopub.status.idle": "2023-08-15T11:35:52.668301Z",
     "shell.execute_reply": "2023-08-15T11:35:52.667454Z"
    },
    "papermill": {
     "duration": 0.025242,
     "end_time": "2023-08-15T11:35:52.670992",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.645750",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "def get_matches(des_query, des_train):\n",
    "    \"\"\"\n",
    "    匹配相关图像特征。\n",
    "\n",
    "    参数:\n",
    "    - des_query: (np.ndarray): 查询描述子。\n",
    "    - des_train: (np.ndarray): 训练描述子。\n",
    "\n",
    "    返回:\n",
    "    - goods: (list): 匹配结果。\n",
    "    \"\"\"\n",
    "    # 创建匹配器\n",
    "    bf = cv2.BFMatcher(cv2.NORM_L2)\n",
    "    # 获取匹配结果\n",
    "    matches = bf.knnMatch(des_query, des_train, k=2)\n",
    "    # 创建goods列表用于存储有用的匹配结果\n",
    "    goods = []\n",
    "    # 遍历所有匹配结果\n",
    "    for m, m_ in matches:\n",
    "        # 设置阈值为0.65，保留更多的特征\n",
    "        if m.distance < 0.65 * m_.distance:\n",
    "            goods.append(m)\n",
    "    return goods\n",
    "\n",
    "def get_match_point(p, p_, matches):\n",
    "    \"\"\"\n",
    "    寻找匹配的关键点。\n",
    "\n",
    "    参数:\n",
    "    - p: (list[cv2.KeyPoint]): 查询关键点。\n",
    "    - p_: (list[cv2.KeyPoint]): 训练关键点。\n",
    "    - matches: (list[cv2.DMatch]): 匹配信息。\n",
    "\n",
    "    返回:\n",
    "    - points_query: (np.ndarray): 查询关键点。\n",
    "    - points_train: (np.ndarray): 训练关键点。\n",
    "    \"\"\"\n",
    "    # 从查询关键点中找到匹配的关键点\n",
    "    points_query = np.asarray([p[m.queryIdx].pt for m in matches])\n",
    "    # 从训练关键点中找到匹配的关键点\n",
    "    points_train = np.asarray([p_[m.trainIdx].pt for m in matches])\n",
    "    # 返回匹配的查询和训练关键点\n",
    "    return points_query, points_train\n",
    "\n",
    "def homoco_pts_2_euco_pts(pts):\n",
    "    \"\"\"\n",
    "    齐次坐标转化为欧几里得坐标。\n",
    "    \"\"\"\n",
    "    if len(pts.shape) == 1:\n",
    "        pts = pts.reshape(1, -1)\n",
    "    res = pts / pts[:, -1, None]\n",
    "    return res[:, :-1].squeeze()\n",
    "\n",
    "def euco_pts_2_homoco_pts(pts):\n",
    "    \"\"\"\n",
    "    欧几里得坐标转化为齐次坐标。\n",
    "    \"\"\"\n",
    "    if len(pts.shape) == 1:\n",
    "        pts = pts.reshape(1, -1)\n",
    "    one = np.ones(pts.shape[0])\n",
    "    res = np.c_[pts, one]\n",
    "    return res.squeeze()\n",
    "\n",
    "def normalize(pts, T=None):\n",
    "    \"\"\"\n",
    "    对点集进行归一化。\n",
    "    \"\"\"\n",
    "    # 如果T参数为空\n",
    "    if T is None:\n",
    "        # 求点集的平均值\n",
    "        u = np.mean(pts, 0)\n",
    "        # 求点集中每个点与原点之间的距离之和\n",
    "        d = np.sum(np.sqrt(np.sum(np.power(pts, 2), 1)))\n",
    "        # 计算归一化矩阵\n",
    "        T = np.array([\n",
    "            [np.sqrt(2) / d, 0, -(np.sqrt(2) / d * u[0])],\n",
    "            [0, np.sqrt(2) / d, -(np.sqrt(2) / d * u[1])],\n",
    "            [0, 0, 1]\n",
    "        ])\n",
    "    # 将点集进行归一化\n",
    "    return homoco_pts_2_euco_pts(np.matmul(T, \n",
    "                euco_pts_2_homoco_pts(pts).T).T), T"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0c38f6b0",
   "metadata": {
    "papermill": {
     "duration": 0.00745,
     "end_time": "2023-08-15T11:35:52.686565",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.679115",
     "status": "completed"
    },
    "tags": []
   },
   "source": [
    "到这里我们就已经得到了两张图之间的对应像素关系，接下来使用八点法计算基础矩阵。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "84a79964",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:52.703431Z",
     "iopub.status.busy": "2023-08-15T11:35:52.703074Z",
     "iopub.status.idle": "2023-08-15T11:35:52.721160Z",
     "shell.execute_reply": "2023-08-15T11:35:52.720052Z"
    },
    "papermill": {
     "duration": 0.029257,
     "end_time": "2023-08-15T11:35:52.723426",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.694169",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "def estimate_fundamental(pts1, pts2, num_sample=8):\n",
    "    \"\"\"\n",
    "    计算基础矩阵。\n",
    "\n",
    "    参数:\n",
    "    - pts1: (np.ndarray)：匹配特征所得到的训练点集。\n",
    "    - pts2: (np.ndarray)：匹配特征所得到的查询点集。\n",
    "\n",
    "    返回:\n",
    "    - f: (np.ndarray): 基础矩阵。\n",
    "    \"\"\"\n",
    "    n = pts1.shape[0]\n",
    "    pts_index = range(n)\n",
    "    sample_index = random.sample(pts_index, num_sample)\n",
    "    p1 = pts1[sample_index, :]\n",
    "    p2 = pts2[sample_index, :]\n",
    "    n = len(sample_index)\n",
    "    # 归一化点集坐标\n",
    "    p1_norm, T1 = normalize(p1, None)\n",
    "    p2_norm, T2 = normalize(p2, None)\n",
    "    w = np.zeros((n, 9))\n",
    "    # 构建A矩阵\n",
    "    for i in range(n):\n",
    "        w[i, 0] = p1_norm[i, 0] * p2_norm[i, 0]\n",
    "        w[i, 1] = p1_norm[i, 1] * p2_norm[i, 0]\n",
    "        w[i, 2] = p2_norm[i, 0]\n",
    "        w[i, 3] = p1_norm[i, 0] * p2_norm[i, 1]\n",
    "        w[i, 4] = p1_norm[i, 1] * p2_norm[i, 1]\n",
    "        w[i, 5] = p2_norm[i, 1]\n",
    "        w[i, 6] = p1_norm[i, 0]\n",
    "        w[i, 7] = p1_norm[i, 1]\n",
    "        w[i, 8] = 1\n",
    "    # svd分解\n",
    "    U, sigma, VT = np.linalg.svd(w)\n",
    "    f = VT[-1, :].reshape(3, 3)\n",
    "    U, sigma, VT = np.linalg.svd(f)\n",
    "    sigma[2] = 0\n",
    "    f = U.dot(np.diag(sigma)).dot(VT)\n",
    "    # 逆归一化\n",
    "    f = T2.T.dot(f).dot(T1)\n",
    "\n",
    "    ### 随机计算的F矩阵误差较大，使用OpenCV中的函数(RANSAC)计算F矩阵\n",
    "    pts1 = pts1.astype(np.float32)\n",
    "    pts2 = pts2.astype(np.float32)\n",
    "    f, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)\n",
    "\n",
    "    return f\n",
    "\n",
    "def convert_F_to_E(F_single, K):\n",
    "    \"\"\"\n",
    "    根据F矩阵计算E矩阵。\n",
    "    \"\"\"\n",
    "    inverse_K = np.linalg.inv(K)\n",
    "    E_single = inverse_K.T.dot(F_single).dot(inverse_K)\n",
    "    return E_single\n",
    "\n",
    "def get_Rt_from_E(E_single,K,pts1,pts2):\n",
    "    \"\"\"\n",
    "    根据E矩阵计算Rt，使用OpenCV的函数recoverPose。\n",
    "    \"\"\"\n",
    "    # OpenCV的recoverPose函数已经自动去除了不合理的3个解\n",
    "    _, R, t, _ = cv2.recoverPose(E_single, pts1, pts2, K) \n",
    "    return R, t\n",
    "\n",
    "def build_F_E_matrix(feats, K):\n",
    "    \"\"\"\n",
    "    计算基础矩阵F，本质矩阵E，从E中分离出Rt并返回。\n",
    "    \"\"\"\n",
    "    pair = dict()\n",
    "    match = dict()\n",
    "    Rts = dict()\n",
    "\n",
    "    for i in range(len(feats)):\n",
    "        for j in range(i + 1, len(feats)):\n",
    "            matches = get_matches(\n",
    "                feats[i]['des'], feats[j]['des'])\n",
    "            pts1, pts2 = get_match_point(\n",
    "                feats[i]['kpt'], feats[j]['kpt'], matches)\n",
    "            assert pts1.shape == pts2.shape\n",
    "            \n",
    "            # 至少需要8个点来计算F矩阵\n",
    "            if pts1.shape[0] < 8:\n",
    "                continue\n",
    "            # 计算F矩阵\n",
    "            F_single = estimate_fundamental(pts1, pts2)\n",
    "            # 根据F矩阵计算E矩阵\n",
    "            E_single = convert_F_to_E(F_single, K)\n",
    "            # 从E矩阵中得到相对位姿\n",
    "            R, t = get_Rt_from_E(E_single, K, pts1, pts2)\n",
    "\n",
    "            if pts1.shape[0] < 8:\n",
    "                continue\n",
    "\n",
    "            pair.update({(i, j): {'pts1': pts1, 'pts2': pts2}})\n",
    "            match.update({(i, j): {'match': matches}})\n",
    "            Rts.update({(i, j): {'R': R, 't': t}})\n",
    "\n",
    "    return F_single, E_single, pair, match, Rts"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "01e53448",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:52.740440Z",
     "iopub.status.busy": "2023-08-15T11:35:52.740021Z",
     "iopub.status.idle": "2023-08-15T11:35:57.806322Z",
     "shell.execute_reply": "2023-08-15T11:35:57.805554Z"
    },
    "papermill": {
     "duration": 5.077515,
     "end_time": "2023-08-15T11:35:57.808216",
     "exception": false,
     "start_time": "2023-08-15T11:35:52.730701",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 372736 characters]",
      "text/plain": [
       "<Figure size 1000x1000 with 2 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "基础矩阵为:\n",
      " [[ 5.17116489e-06 -9.35405231e-06 -1.13783351e-03]\n",
      " [ 5.94850943e-06  1.16772622e-05 -5.72937517e-03]\n",
      " [-2.94980264e-03  3.55455633e-03  1.00000000e+00]]\n",
      "本质矩阵为:\n",
      " [[ 1.80790150e-11 -3.27683889e-11 -2.12631708e-06]\n",
      " [ 2.08383558e-11  4.09888034e-11 -1.07495911e-05]\n",
      " [-5.52573986e-06  6.66134548e-06  1.00331656e+00]]\n",
      "第一副图像对应的矩阵为:\n",
      " [[534.81896973   0.         320.           0.        ]\n",
      " [  0.         533.75       213.           0.        ]\n",
      " [  0.           0.           1.           0.        ]]\n",
      "第二幅图像对应的矩阵为:\n",
      " [[ 6.02699217e+02 -1.32348682e+02 -8.75717417e+01  5.27600711e+02]\n",
      " [ 1.51538241e+02 -1.73321278e+02  5.26549104e+02  8.74049809e+01]\n",
      " [ 3.23134701e-01 -9.46352981e-01  1.82851868e-05  3.84515925e-06]]\n"
     ]
    }
   ],
   "source": [
    "img_root = 'Hands-on-CV/第19章 三维重建/images'\n",
    "imgs, feats, K = construct_img_info(img_root)\n",
    "# 可视化图像\n",
    "vis_imgs(imgs)\n",
    "\n",
    "F, E, pair, match, Rts = build_F_E_matrix(feats, K)\n",
    "print(\"基础矩阵为:\\n\", F)\n",
    "print(\"本质矩阵为:\\n\", E)\n",
    "\n",
    "# 不妨假定第一张图为世界坐标系\n",
    "R_t_0 = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0]])\n",
    "R_t_1 = np.empty((3,4))\n",
    "R_t_1[:,:3] = Rts[(0,1)]['R']\n",
    "R_t_1[:,3] = Rts[(0,1)]['t'].reshape(3)\n",
    "P_0 = K.dot(R_t_0)\n",
    "P_1 = K.dot(R_t_1)\n",
    "print(\"第一副图像对应的矩阵为:\\n\", P_0)\n",
    "print(\"第二幅图像对应的矩阵为:\\n\", P_1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "49406d8e",
   "metadata": {},
   "source": [
    "在得到了基础矩阵之后，我们就可以通过三角测量计算出三维空间点的坐标。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "15934bd1",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "过滤前的匹配点数量为:  16\n",
      "过滤后的匹配点数量为:  13\n",
      "得到的三维点为:\n",
      " [[-0.60458395 -0.01198244  0.0969504 ]\n",
      " [-0.58099777 -0.21461379  0.05341815]\n",
      " [-0.47523613 -0.39286399 -0.15945556]\n",
      " [-0.56740703 -0.24286074 -0.00477856]\n",
      " [-0.49754951 -0.36653935 -0.14429421]\n",
      " [-0.47079379 -0.40467599 -0.19146369]\n",
      " [-0.55889738 -0.26988774 -0.09548055]\n",
      " [-0.60761656 -0.14041958 -0.12032324]\n",
      " [-0.56458993 -0.29775277 -0.2171921 ]\n",
      " [-0.6082215  -0.14859759 -0.13404173]\n",
      " [-0.43299244 -0.4819224  -0.33418579]\n",
      " [-0.5657818  -0.3328847  -0.29034157]\n",
      " [-0.5657818  -0.3328847  -0.29034157]]\n",
      "检测到的点占图像像素的比例为: 0.004757%\n"
     ]
    }
   ],
   "source": [
    "def filter_matches_by_epipolar(pts1, pts2, F, threshold=3.0):\n",
    "    \"\"\"\n",
    "    使用对极约束过滤匹配点\n",
    "    \n",
    "    参数:\n",
    "    - pts1, pts2: 匹配点对。\n",
    "    - F: 基础矩阵。\n",
    "    - threshold: 对极误差阈值。\n",
    "        \n",
    "    返回:\n",
    "    - pts1_filtered, pts2_filtered: 过滤后的匹配点。\n",
    "    \"\"\"\n",
    "    pts1_reshaped = pts1.T.reshape(-1,1,2)\n",
    "    pts2_reshaped = pts2.T.reshape(-1,1,2)\n",
    "    \n",
    "    # 计算两幅图像的极线\n",
    "    lines1 = cv2.computeCorrespondEpilines(pts2_reshaped, 2, F)\n",
    "    lines2 = cv2.computeCorrespondEpilines(pts1_reshaped, 1, F)\n",
    "    \n",
    "    dist1 = np.abs(np.sum(lines1.reshape(-1,3) * \n",
    "                 np.hstack((pts1.T, np.ones((pts1.shape[1],1)))), axis=1))\n",
    "    dist2 = np.abs(np.sum(lines2.reshape(-1,3) * \n",
    "                 np.hstack((pts2.T, np.ones((pts2.shape[1],1)))), axis=1))\n",
    "    \n",
    "    mask = (dist1 < threshold) & (dist2 < threshold)\n",
    "    return pts1[:,mask], pts2[:,mask]\n",
    "\n",
    "pts1 = np.transpose(pair[(0,1)]['pts1'])\n",
    "pts2 = np.transpose(pair[(0,1)]['pts2'])\n",
    "# 过滤匹配点\n",
    "pts1_filtered, pts2_filtered = filter_matches_by_epipolar(pts1, pts2, F)\n",
    "print(\"过滤前的匹配点数量为: \", pts1.shape[1])\n",
    "print(\"过滤后的匹配点数量为: \", pts1_filtered.shape[1])\n",
    "\n",
    "# 三角测量获得空间点坐标\n",
    "points_3d = cv2.triangulatePoints(P_0, P_1, pts1_filtered, pts2_filtered)\n",
    "# 齐次坐标的最后一行为1，需要除以最后一行\n",
    "points_3d /= points_3d[3]\n",
    "points_3d = points_3d[:3, :].T\n",
    "print(\"得到的三维点为:\\n\", points_3d)\n",
    "\n",
    "# 计算有效点占像素比例\n",
    "print(\"检测到的点占图像像素的比例为: {:02f}%\".format(\n",
    "    points_3d.shape[0]/len(imgs[0][...,0].flatten())*100))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b1d8059f",
   "metadata": {},
   "source": [
    "可以发现，存在部分匹配点不严格遵循对极约束。接下来我们可视化极线和匹配点。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "6dda5c72",
   "metadata": {
    "execution": {
     "iopub.execute_input": "2023-08-15T11:35:57.841447Z",
     "iopub.status.busy": "2023-08-15T11:35:57.839940Z",
     "iopub.status.idle": "2023-08-15T11:35:58.865600Z",
     "shell.execute_reply": "2023-08-15T11:35:58.864334Z"
    },
    "papermill": {
     "duration": 1.045955,
     "end_time": "2023-08-15T11:35:58.868807",
     "exception": false,
     "start_time": "2023-08-15T11:35:57.822852",
     "status": "completed"
    },
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 435884 characters]",
      "text/plain": [
       "<Figure size 1000x1000 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def draw_epipolar_lines(pts1, pts2, img1, img2, F):\n",
    "    \"\"\"\n",
    "    绘制极线。\n",
    "\n",
    "    参数:\n",
    "    - pts1: (np.ndarray): 第一幅图像中的匹配点。\n",
    "    - pts2: (np.ndarray): 第二幅图像中的匹配点。\n",
    "    - img1: (np.ndarray): 第一幅图像。\n",
    "    - img2: (np.ndarray): 第二幅图像。\n",
    "    - F: (np.ndarray): 基础矩阵。\n",
    "    \"\"\"\n",
    "    # 将点坐标转为整数\n",
    "    pts1 = np.int32(pts1)\n",
    "    pts2 = np.int32(pts2)\n",
    "\n",
    "    # 选取前8个点进行可视化\n",
    "    idx = np.arange(8)\n",
    "    pts1 = pts1[idx]\n",
    "    pts2 = pts2[idx]\n",
    "\n",
    "    # 计算对应的极线\n",
    "    lines1 = cv2.computeCorrespondEpilines(\n",
    "                        pts2.reshape(-1,1,2), 2, F)\n",
    "    lines1 = lines1.reshape(-1,3)\n",
    "\n",
    "    # 绘制极线\n",
    "    imgl, imgr = drawlines(img1, img2, lines1, pts1, pts2)\n",
    "\n",
    "    # 将两幅图拼接在一起\n",
    "    vis = np.concatenate((imgl, imgr), axis=1)\n",
    "\n",
    "    # 展示图像\n",
    "    plt.figure(figsize=(10, 10))\n",
    "    plt.imshow(vis)\n",
    "    plt.axis('off')\n",
    "    plt.show()\n",
    "\n",
    "def drawlines(img1, img2, lines, pts1, pts2):\n",
    "    \"\"\"\n",
    "    绘制极线。\n",
    "\n",
    "    参数:\n",
    "    - img1: (np.ndarray): 左图。\n",
    "    - img2: (np.ndarray): 右图。\n",
    "    - lines: (np.ndarray): 极线。\n",
    "    - pts1: (np.ndarray): 左图对应的匹配点。\n",
    "    - pts2: (np.ndarray): 右图对应的匹配点。\n",
    "    \"\"\"\n",
    "    # 获取图像尺寸\n",
    "    r, c, _ = img1.shape\n",
    "\n",
    "    for r, pt1, pt2 in zip(lines, pts1, pts2):\n",
    "        color = tuple(np.random.randint(50, 255, 3).tolist())\n",
    "        # 计算直线上的两个点\n",
    "        x0, y0 = map(int, [0, -r[2]/r[1]]) \n",
    "        x1, y1 = map(int, [c, -(r[2]+r[0]*c)/r[1]])\n",
    "        # 在img1中绘制直线\n",
    "        img1 = cv2.line(img1,  (x0, y0),  (x1, y1),  color, 1) \n",
    "        # 在img1中绘制对应的特征点\n",
    "        img1 = cv2.circle(img1, tuple(pt1), 5, color, -1) \n",
    "        # 在img2中绘制对应的特征点\n",
    "        img2 = cv2.circle(img2, tuple(pt2), 5, color, -1) \n",
    "    return img1, img2\n",
    "\n",
    "# 在原图上可视化特征点对应的极线\n",
    "draw_epipolar_lines(pts1_filtered.T, pts2_filtered.T, \n",
    "                    imgs[0], imgs[1], F)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "vis",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.16"
  },
  "papermill": {
   "default_parameters": {},
   "duration": 21.383055,
   "end_time": "2023-08-15T11:36:00.035210",
   "environment_variables": {},
   "exception": null,
   "input_path": "__notebook__.ipynb",
   "output_path": "__notebook__.ipynb",
   "parameters": {},
   "start_time": "2023-08-15T11:35:38.652155",
   "version": "2.4.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
