diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0b1bc70
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,93 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*/__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+# Usually these files are written by a python script from a template
+# before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+.hypothesis/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# IPython Notebook
+.ipynb_checkpoints
+
+# pyenv
+.python-version
+
+# celery beat schedule file
+celerybeat-schedule
+
+# dotenv
+.env
+
+# virtualenv
+venv/
+ENV/
+
+# Spyder project settings
+.spyderproject
+
+# Rope project settings
+.ropeproject
+
+*-data.inc
+src/**/*.py.bak
diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..10b731c
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,5 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
+# 基于编辑器的 HTTP 客户端请求
+/httpRequests/
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..5060d8a
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..5f6aad3
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/surround-view-system-introduction-master.iml b/.idea/surround-view-system-introduction-master.iml
new file mode 100644
index 0000000..0fc9e4c
--- /dev/null
+++ b/.idea/surround-view-system-introduction-master.iml
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..317c729
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 Zhao Liang
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/README.md b/README.md
index 3b99e9e..e9571e7 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,9 @@
-# ADAS360
+一份简单的环视系统制作实现，包含完整的标定、投影、拼接和实时运行流程，详细文档见 `doc` 目录。环视系统的开源代码很少见，希望大家积极提 issue 和 pull request，让这个项目能更好地有益于入门者学习。
+
+This is a simple implementation of a surround view system, including calibration, projection, stitching, and real-time running processes. The [English documentation](https://github.com/hynpu/surround-view-system-introduction/blob/master/doc/en.md) can be found in the `doc` folder.
+
+
+# Use cases
+
+
diff --git a/doc/doc.md b/doc/doc.md
new file mode 100644
index 0000000..e00eb4c
--- /dev/null
+++ b/doc/doc.md
@@ -0,0 +1,279 @@
+关于车辆的全景环视系统网上已经有很多的资料，然而几乎没有可供参考的代码，这一点对入门的新人来说非常不友好。这个项目的目的就是介绍全景系统的原理，并给出一份基本要素齐全的、可以实际运行的 Python 实现供大家参考。环视全景系统涉及的知识并不复杂，只需要读者了解相机的标定、透视变换，并懂得如何使用 OpenCV。
+
+这个程序最初是在一辆搭载了一台 AGX Xavier 的无人小车上开发的，运行效果如下：(见 `img/smallcar.mp4`)
+
+
+
+小车上搭载了四个 USB 环视鱼眼摄像头，相机传回的画面分辨率为 640x480，图像首先经过畸变校正，然后在射影变换下转换为对地面的鸟瞰图，最后拼接起来经过平滑处理后得到了上面的效果。全部过程在 CPU 中进行处理，整体运行流畅。
+
+后来我把代码重构以后移植到一辆乘用车上 (处理器是同型号的 AGX)，得到了差不多的效果：(见 `img/car.mp4`)
+
+
+
+这个版本使用的是四个 960x640 的 csi 摄像头，输出的全景图分辨率为 1200x1600，在不进行亮度均衡处理时全景图处理线程运行速度大约为 17 fps，加入亮度均衡处理后骤降到只有 7 fps。我认为适当缩小分辨率的话 (比如采用 480x640 的输出可以将像素个数降低到原来的 1/6) 应该也可以获得流畅的视觉效果。
+
+
+> **注**：画面中黑色的部分是相机投影后出现的盲区，这是因为前面的相机为了避开车标的部位安装在了车头左侧且角度倾斜，所以视野受到了限制。想象一个人歪着脖子还斜着眼走路的样子 ...
+
+这个项目的实现比较粗糙，仅作为演示项目展示生成环视全景图的基本要素，大家领会精神即可。我开发这个项目的目的是为了在自动泊车时实时显示车辆的轨迹，同时也用来作为我指导的实习生的实习项目。由于之前没有经验和参照，大多数算法和流程都是琢磨着写的，不见得高明，请大家多指教。代码是 Python 写成的，效率上肯定不如 C++，所以仅适合作学习和验证想法使用。
+
+下面就来一一介绍我的实现步骤。
+
+
+# 硬件和软件配置
+
+
+我想首先强调的是，硬件配置是这个项目中最不需要费心的事情，在第一个小车项目中使用的硬件如下：
+
+1. 四个 USB 鱼眼相机，支持的分辨率为 640x480|800x600|1920x1080 三种。我这里因为是需要在 Python 下面实时运行，为了效率考虑设置的分辨率是 640x480。
+2. 一台 AGX Xavier。实际上在普通笔记本上跑一样溜得飞起。
+
+第二个乘用车项目使用的硬件如下：
+
+1. 四个 csi 摄像头，设置的分辨率是 960x640。型号是 Sekonix 的 [SF3326-100-RCCB](http://sekolab.com/products/camera/)。
+2. 一台 AGX Xavier，型号同上面的小车项目一样，不过外加了一个工控机接收 csi 摄像头画面。
+
+我认为你只要有四个视野足够覆盖车周围的摄像头，再加一个普通笔记本电脑就足够进行全部的离线开发了。需要注意的是，由于传输速率的限制，笔记本的一个 USB 口可能无法同时带动四个摄像头，所以你可能需要用 hub 将摄像头分配在两个 USB 端口上。
+
+软件配置如下：
+
+1. 操作系统 Ubuntu 16.04/18.04.
+2. Python>=3.
+3. OpenCV>=3.
+4. PyQt5.
+
+其中 PyQt5 主要用来实现多线程，方便将来移植到 Qt 环境。
+
+
+# 项目采用的若干约定
+
+
+为了方便起见，在本项目中四个环视相机分别用 `front`、`back`、`left`、`right` 来指代，并假定其对应的设备号是整数，例如 0, 1, 2, 3。实际开发中请针对具体情况进行修改。
+
+相机的内参矩阵记作 `camera_matrix`，这是一个 3x3 的矩阵。畸变系数记作 `dist_coeffs`，这是一个 1x4 的向量。相机的投影矩阵记作 `project_matrix`，这是一个 3x3 的射影矩阵。
+
+
+# 准备工作：获得原始图像与相机内参
+
+
+首先我们需要获取每个相机的内参矩阵与畸变系数。我在项目中附上了一个脚本 [run_calibrate_camera.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_calibrate_camera.py)，你只需要运行这个脚本，通过命令行参数告诉它相机设备号，是否是鱼眼相机，以及标定板的网格大小，然后手举标定板在相机面前摆几个姿势即可。
+
+以下是视频中四个相机分别拍摄的原始画面，顺序依次为前、后、左、右，并命名为 `front.png`、`back.png`、`left.png`、`right.png` 保存在项目的 `images/` 目录下。
+
+| | | | |
+|:-:|:-:|:-:|:-:|
+|front|back|left|right|
+|
|
|
|
|
+
+四个相机的内参文件分别为 `front.yaml`、`back.yaml`、`left.yaml`、`right.yaml`，这些内参文件都存放在项目的 [yaml](https://github.com/neozhaoliang/surround-view-system-introduction/tree/master/yaml) 子目录下。
+
+你可以看到图中地面上铺了一张标定布，这个布的尺寸是 `6mx10m`，每个黑白方格的尺寸为 `40cmx40cm`，每个圆形图案所在的方格是 `80cmx80cm`。我们将利用这个标定物来手动选择对应点获得投影矩阵。
+
+
+# 设置投影范围和参数
+
+
+接下来我们需要获取每个相机到地面的投影矩阵，这个投影矩阵会把相机校正后的画面转换为对地面上某个矩形区域的鸟瞰图。这四个相机的投影矩阵不是独立的，它们必须保证投影后的区域能够正好拼起来。
+
+这一步是通过联合标定实现的，即在车的四周地面上摆放标定物，拍摄图像，手动选取对应点，然后获取投影矩阵。
+
+请看下图：
+
+
+
+首先在车身的四角摆放四个标定板，标定板的图案大小并无特殊要求，只要尺寸一致，能在图像中清晰看到即可。每个标定板应当恰好位于相邻的两个相机视野的重合区域中。
+
+在上面拍摄的相机画面中车的四周铺了一张标定布，这个具体是标定板还是标定布不重要，只要能清楚的看到特征点就可以了。
+
+然后我们需要设置几个参数：(以下所有参数均以厘米为单位)
+
++ `innerShiftWidth`, `innerShiftHeight`：标定板内侧边缘与车辆左右两侧的距离，标定板内侧边缘与车辆前后方的距离。
++ `shiftWidth`, `shiftHeight`：这两个参数决定了在鸟瞰图中向标定板的外侧看多远。这两个值越大，鸟瞰图看的范围就越大，相应地远处的物体被投影后的形变也越严重，所以应酌情选择。
++ `totalWidth`, `totalHeight`：这两个参数代表鸟瞰图的总宽高，在我们这个项目中标定布宽 6m 高 10m，于是鸟瞰图中地面的范围为 `(600 + 2 * shiftWidth, 1000 + 2 * shiftHeight)`。为方便计我们让每个像素对应 1 厘米，于是鸟瞰图的总宽高为
+
+ ```
+ totalWidth = 600 + 2 * shiftWidth
+ totalHeight = 1000 + 2 * shiftHeight
+ ```
+
++ 车辆所在矩形区域的四角 (图中标注的红色圆点)，这四个角点的坐标分别为 `(xl, yt)`, `(xr, yt)`, `(xl, yb)`, `(xr, yb)` (`l` 表示 left, `r` 表示 right，`t` 表示 top，`b` 表示 bottom)。这个矩形区域相机是看不到的，我们会用一张车辆的图标来覆盖此处。
+
+注意这个车辆区域四边的延长线将整个鸟瞰图分为前左 (FL)、前中 (F)、前右 (FR)、左 (L)、右 (R)、后左 (BL)、后中 (B)、后右 (BR) 八个部分，其中 FL (区域 I)、FR (区域 II)、BL (区域 III)、BR (区域 IV) 是相邻相机视野的重合区域，也是我们重点需要进行融合处理的部分。F、R、L、R 四个区域属于每个相机单独的视野，不需要进行融合处理。
+
+以上参数存放在 [param_settings.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/param_settings.py) 中。
+
+设置好参数以后，每个相机的投影区域也就确定了，比如前方相机对应的投影区域如下：
+
+
+
+接下来我们需要通过手动选取标志点来获取到地面的投影矩阵。
+
+
+# 手动标定获取投影矩阵
+
+
+首先运行项目中 [run_get_projection_maps.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_get_projection_maps.py) 这个脚本，这个脚本需要你输入如下的参数：
+
++ `-camera`: 指定是哪个相机。
++ `-scale`: 校正后画面的横向和纵向放缩比。
++ `-shift`: 校正后画面中心的横向和纵向平移距离。
+
+为什么需要 `scale` 和 `shift` 这两个参数呢？这是因为默认的 OpenCV 的校正方式是在鱼眼相机校正后的图像中裁剪出一个 OpenCV "认为" 合适的区域并将其返回，这必然会丢失一部分像素，特别地可能会把我们希望选择的特征点给裁掉。幸运的是 [cv2.fisheye.initUndistortRectifyMap](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#ga0d37b45f780b32f63ed19c21aa9fd333) 这个函数允许我们再传入一个新的内参矩阵，对校正后但是裁剪前的画面作一次放缩和平移。你可以尝试调整并选择合适的横向、纵向压缩比和图像中心的位置使得地面上的标志点出现在画面中舒服的位置上，以方便进行标定。
+
+运行
+
+```bash
+python run_get_projection_maps.py -camera front -scale 0.7 0.8 -shift -150 -100
+```
+
+后显示前方相机校正后的画面如下：
+
+
+
+然后依次点击事先确定好的四个标志点 (顺序不能错！)，得到的效果如下：
+
+
+
+注意标志点的设置代码在[这里](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/param_settings.py#L40)。
+
+这四个点是可以自由设置的，但是你需要在程序中手动修改它们在鸟瞰图中的像素坐标。当你在校正图中点击这四个点时，OpenCV 会根据它们在校正图中的像素坐标和在鸟瞰图中的像素坐标的对应关系计算一个射影矩阵。这里用到的原理就是四点对应确定一个射影变换 (四点对应可以给出八个方程，从而求解出射影矩阵的八个未知量。注意射影矩阵的最后一个分量总是固定为 1)。
+
+如果你不小心点歪了的话可以按 `d` 键删除上一个错误的点。选择好以后点回车，就会显示投影后的效果图:
+
+
+
+觉得效果可以的话敲回车，就会将投影矩阵写入 `front.yaml` 中，这个矩阵的名字为 `project_matrix`。失败的话就按 `q` 退出再来一次。
+
+再比如后面相机的标定如下图所示：
+
+
+
+对应的投影图为
+
+
+
+对四个相机分别采用此操作，我们就得到了四个相机的鸟瞰图，以及对应的四个投影矩阵。下一步我们的任务是把这四个鸟瞰图拼起来。
+
+> **重要注意事项**：有不少读者反映在按照上面的方法进行标定时，得到的拼接效果不够好。经过询问，发现原因是他们选择的四个标定点都集中在图像的中心部分。你应该让四个标定点构成的矩形区域覆盖图像尽可能大的范围。原因是鱼眼图像在矫正后也是有误差的，边缘的误差更大。所以要尽可能让 OpenCV 在更大的范围内计算一个全局最优的射影矩阵。
+
+
+# 鸟瞰图的拼接与平滑
+
+
+如果你前面操作一切正常的话，运行 [run_get_weight_matrices.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_get_weight_matrices.py) 后应该会显示如下的拼接图：
+
+
+
+我来逐步介绍它是怎么做到的：
+
+1. 由于相邻相机之间有重叠的区域，所以这部分的融合是关键。如果直接采取两幅图像加权平均 (权重各自为 1/2) 的方式融合的话你会得到类似下面的结果：
+
+ 
+
+ 你可以看到由于校正和投影的误差，相邻相机在重合区域的投影结果并不能完全吻合，导致拼接的结果出现乱码和重影。这里的关键在于权重系数应该是随像素变化而变化的，并且是随着像素连续变化。
+
+2. 以左上角区域为例，这个区域是 `front`, `left` 两个相机视野的重叠区域。我们首先将投影图中的重叠部分取出来：
+
+ 
+
+ 灰度化并二值化：
+
+ 
+
+ 注意这里面有噪点，可以用形态学操作去掉 (不必特别精细，大致去掉即可)：
+
+ 
+
+ 至此我们就得到了重叠区域的一个完整 mask。
+
+3. 将 `front`, `left` 图像各自位于重叠区域外部的边界检测出来，这一步是通过先调用 `cv2.findContours` 求出最外围的边界，再用 `cv2.approxPolyDP` 获得逼近的多边形轮廓：
+
+ |||
+ |:-:|:-:|
+ |
|
|
+
+ 我们把 `front` 相机减去重叠区域后的轮廓记作 `polyA` (左上图中蓝色边界)，`left` 相机减去重叠区域后的轮廓记作 `polyB` (右上图中绿色边界)。
+
+4. 对重叠区域中的每个像素，利用 `cv2.pointPolygonTest` 计算其到这两个多边形 `polyA` 和 `polyB` 的距离 $d_A,d_B$，则该像素对应的权值为 $w=d_B^2/(d_A^2+d_B^2)$，即如果这个像素落在 `front` 画面内，则它与 `polyB` 的距离就更远，从而权值更大。
+
+5. 对不在重叠区域内的像素，若其属于 `front` 相机的范围则其权值为 1，否则权值为 0。于是我们得到了一个连续变化的，取值范围在 0~1 之间的矩阵 $G$，其灰度图如下：
+
+ 
+
+ 将 $G$ 作为权值可得融合后的图像为 `front * G + (1- G) * left`。
+
+6. 注意由于重叠区域中的像素值是来自两幅图像的加权平均，所以出现在这个区域内的物体会不可避免出现虚影的现象，所以我们需要尽量压缩重叠区域的范围，尽可能只对拼接缝周围的像素计算权值，拼接缝上方的像素尽量使用来自 `front` 的原像素，拼接缝下方的像素尽量使用来自 `back` 的原像素。这一步可以通过控制 $d_B$ 的值得到。
+
+7. 我们还漏掉了重要的一步：由于不同相机的曝光度不同，导致不同的区域会出现明暗的亮度差，影响美观。我们需要调整每个区域的亮度，使得整个拼接图像的亮度趋于一致。这一步做法不唯一，自由发挥的空间很大。我查阅了一下网上提到的方法，发现它们要么过于复杂，几乎不可能是实时的；要么过于简单，无法达到理想的效果。特别在上面第二个视频的例子中，由于前方相机的视野被车标遮挡导致感光范围不足，导致其与其它三个相机的画面亮度差异很大，调整起来很费劲。
+
+ 一个基本的想法是这样的：每个相机传回的画面有 `BGR` 三个通道，四个相机传回的画面总共有 12 个通道。我们要计算 12 个系数，将这 12 个系数分别乘到这 12 个通道上，然后再合并起来形成调整后的画面。过亮的通道要调暗一些所以乘的系数小于 1，过暗的通道要调亮一些所以乘的系数大于 1。这些系数可以通过四个画面在四个重合区域内的亮度比值得出，你可以自由设计计算系数的方式，只要满足这个基本原理即可。
+
+ 我的实现见[这里](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/birdview.py#L210)。感觉就像一段 shader 代码。
+
+ 还有一种偷懒的办法是事先计算一个 tone mapping 函数 (比如逐段线性的，或者 AES tone mapping function)，然后强制把所有像素进行转换，这个方法最省力，但是得到的画面色调会与真实场景有较大差距。似乎有的市面产品就是采用的这种方法。
+
+8. 最后由于有些情况下摄像头不同通道的强度不同，还需要进行一次色彩平衡，见下图:
+
+ | | | |
+ |:-:|:-:|:-:|
+ |拼接后原始画面| 亮度平衡画面 | 亮度平衡+色彩平衡画面|
+ |
|
|
|
+
+ 在第二个视频的例子中，画面的颜色偏红，加入色彩平衡后画面恢复了正常。
+
+
+# 具体实现的注意事项
+
+
+1. 多线程与线程同步。本文的两个例子中四个摄像头都不是硬件触发保证同步的，而且即便是硬件同步的，四个画面的处理线程也未必同步，所以需要有一个线程同步机制。这个项目的实现采用的是比较原始的一种，其核心代码如下：
+```python
+class MultiBufferManager(object):
+
+ ...
+
+ def sync(self, device_id):
+ # only perform sync if enabled for specified device/stream
+ self.mutex.lock()
+ if device_id in self.sync_devices:
+ # increment arrived count
+ self.arrived += 1
+ # we are the last to arrive: wake all waiting threads
+ if self.do_sync and self.arrived == len(self.sync_devices):
+ self.wc.wakeAll()
+ # still waiting for other streams to arrive: wait
+ else:
+ self.wc.wait(self.mutex)
+ # decrement arrived count
+ self.arrived -= 1
+ self.mutex.unlock()
+```
+这里使用了一个 `MultiBufferManager` 对象来管理所有的线程，每个摄像头的线程在每次循环时会调用它的 `sync` 方法，并通过将计数器加 1 的方法来通知这个对象 "报告，我已做完上次的任务，请将我加入休眠池等待下次任务"。一旦计数器达到 4 就会触发唤醒所有线程进入下一轮的任务循环。
+
+2. 建立查找表 (lookup table) 以加快运算速度。鱼眼镜头的画面需要经过校正、投影、翻转以后才能用于拼接，这三步涉及频繁的图像内存分配和销毁，非常费时间。在我的测试中抓取线程始终稳定在 30fps 多一点左右，但是每个画面的处理线程只有 20 fps 左右。这一步最好是通过预计算一个查找表来加速。你还记得 `cv2.fisheye.initUndistortRectifyMap` 这个函数吗？它返回的 `mapx, mapy` 就是两个查找表。比如当你指定它返回的矩阵类型为 `cv2.CV_16SC2` 的时候，它返回的 `mapx` 就是一个逐像素的查找表，`mapy` 是一个用于插值平滑的一维数组 (可以扔掉不要)。同理对于 `project_matrix` 也不难获得一个查找表，两个合起来就可以得到一个直接从原始画面到投影画面的查找表 (当然损失了用于插值的信息)。
+ 在这个项目中由于采用的是 Python 实现，而 Python 的 `for` 循环效率不高，所以没有采用这种查找表的方式。
+
+3. 四个权重矩阵可以作为 `RGBA` 四个通道压缩到一张图片中，这样存储和读取都很方便。四个重叠区域对应的 mask 矩阵也是如此：
+
+ 
+
+ 
+
+
+# 实车运行
+
+
+你可以在实车上运行 [run_live_demo.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_live_demo.py) 来验证最终的效果。
+
+你需要注意修改相机设备号，以及 OpenCV 打开摄像头的方式。usb 相机可以直接用 `cv2.VideoCapture(i)` (`i` 是 usb 设备号) 的方式打开，csi 相机则需要调用 `gstreamer` 打开，对应的代码在[这里](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/utils.py#L5)和[这里](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/capture_thread.py#L75)。
+
+
+# 附录：项目各脚本一览
+
+
+项目中目前的脚本根据执行顺序排列如下:
+
+1. `run_calibrate_camera.py`：用于相机内参标定。
+2. `param_settings.py`：用于设置投影区域的各参数。
+3. `run_get_projection_maps.py`：用于手动标定获取到地面的投影矩阵。
+4. `run_get_weight_matrices.py`：用于计算四个重叠区域对应的权重矩阵以及 mask 矩阵，并显示拼接效果。
+6. `run_live_demo.py`：用于在实车上运行的最终版本。
diff --git a/doc/en.md b/doc/en.md
new file mode 100644
index 0000000..6d283a8
--- /dev/null
+++ b/doc/en.md
@@ -0,0 +1,285 @@
+This project is a simple, runnable, and reproducible demo to show how to develop a surround-view system in Python.
+
+It contains all the key steps: camera calibration, image stitching, and real-time processing.
+
+This project was originally developed on a small car with an AGX Xavier and four USB fisheye cameras: (see `img/smallcar.mp4`)
+
+
+
+The camera resolution was set to 640x480, everything was done in Python.
+
+Later I improved the project and migrated it to a [EU5 car](https://en.wikipedia.org/wiki/Beijing_U5), still processing in a Xavier AGX, and got a better result: (see `img/car.mp4`)
+
+
+
+This EU5 car version used the four CSI cameras of resolution 960x640. The full review image has a resolution 1200x1600, the fps is about 17/7 without/with post-precessing, respectively.
+
+
+> **Remark**：The black area in front of the car is the blind area after projection, it's because the front camera wasn't installed correctly.
+
+The project is not very complex, but it does involve some careful computations. Now we explain the whole process step by step.
+
+
+# Hardware and software
+
+The hardware I used in the small car project is:
+
+1. Four USB fisheye cameras, supporting three different modes of resolution: 640x480|800x600|1920x1080. I used 640x480 because it suffices for a car of this size.
+2. AGX Xavier.
+
+Indeed you can do all the development on your laptop, an AGX is not a strict prerequisite to reproduce this project.
+
+The hardware I used in the EU5 car project is:
+
+1. Four CSI cameras of resolution 960x640。I used Sekonix's [SF3326-100-RCCB camera](http://sekolab.com/products/camera/).
+2. Also, AGX Xavier is the same as in the small car.
+
+The software:
+
+1. Ubuntu 16.04/18.04.
+2. Python>=3.
+3. OpenCV>=3.
+4. PyQt5.
+
+`PyQt5` is used mainly for multi-threading.
+
+
+# Conventions
+
+The four cameras will be named `front`、`back`、`left`、`right`，and with device numbers 0, 1, 2, and 3, respectively. Please modify this according to your actual device numbers.
+
+The camera intrinsic matrix is denoted as `camera_matrix`， this is a 3x3 matrix.
+The distorted coefficients are stored in `dist_coeffs`, this is a 1x4 vector.
+The projection matrix is denoted as `project_matrix`， this is a 3x3 projective matrix.
+
+
+# Prepare work: camera calibration
+
+
+There is a script [run_calibrate_camera.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_calibrate_camera.py) in this project to help
+ you calibrate the camera. I'm not going to discuss how to calibrate a camera here, as there are lots of resources on the web.
+
+Below are the images taken by the four cameras, in the order `front.png`、`back.png`、`left.png`、`right.png`, they are in the `images/` directory.
+
+| | | | |
+|:-:|:-:|:-:|:-:|
+|front|back|left|right|
+|||||
+
+The parameters of these cameras are stored in the yaml files `front.yaml`、`back.yaml`、`left.yaml`、`right.yaml`, these files can be found in the [yaml](https://github.com/neozhaoliang/surround-view-system-introduction/tree/master/yaml) directory.
+
+You can see there is a black-white calibration pattern on the ground, the size of the pattern is `6mx10m`, the size of each black/white square is `40cmx40cm`, the size of each square with a circle in it is `80cmx80cm`.
+
+
+# Setting projection parameters
+
+
+Now we compute the projection matrix for each camera. This matrix will transform the undistorted image into a bird's view of the ground. All four projection matrices must fit together to make sure the four projected images can be stitched together.
+
+This is done by putting calibration patterns on the ground, taking the camera images, manually choosing the feature points, and then computing the matrix.
+
+See the illustration below:
+
+
+
+Firstly you put four calibration boards at the four corners around the car (the blue squares). There are no particular restrictions on how large the board must be, only make sure you can see it clearly in the image.
+
+Of course, each board must be seen by the two adjacent cameras.
+
+Now we need to set a few parameters: (in `cm` units)
+
++ `innerShiftWidth`, `innerShiftHeight`: distance between the inner edges of the left/right calibration boards and the car; the distance between the inner edges of the front/back calibration boards and the car。
++ `shiftWidth`, `shiftHeight`: How far you will want to look at out of the boards. The bigger these values, the larger the area the birdview image will cover.
++ `totalWidth`, `totalHeight`: Size of the area that the birdview image covers. In this project, the calibration pattern is of width `600cm` and height `1000cm`, hence the bird view image will cover an area of size `(600 + 2 * shiftWidth, 1000 + 2 * shiftHeight)`. For simplicity, we let each pixel correspond to 1cm, so the final bird-view image also has a resolution
+
+ ```
+ totalWidth = 600 + 2 * shiftWidth
+ totalHeight = 1000 + 2 * shiftHeight
+ ```
+
++ The four corners of the rectangular area where the vehicle is located (marked with red dots in the image) are denoted by the coordinates (xl, yt), (xr, yt), (xl, yb), and (xr, yb), where "l" stands for left, "r" stands for right, "t" stands for top, and "b" stands for bottom. The camera cannot see this rectangular area, and we will use an icon of the vehicle to cover it.
+
+Note that the extension lines of the four sides of the vehicle area divide the entire bird's-eye view into eight parts: front-left (FL), front-center (F), front-right (FR), left (L), right (R), back-left (BL), back-center (B), and back-right (BR). Among them, FL (area I), FR (area II), BL (area III), and BR (area IV) are the overlapping areas of adjacent camera views, and they are the parts that we need to focus on for fusion processing. The areas F, R, L, and R belong to the individual views of each camera and do not require fusion processing.
+
+The above parameters are saved in [param_settings.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/param_settings.py)
+
+
+Once the parameters are set, the projection area for each camera is determined. For example, the projection area for the front camera is as follows:
+
+
+
+Next, we need to manually select the feature points to obtain the projection matrix for the ground plane.
+
+
+# Manually select feature points for the projection matrix
+
+
+Firstly you need to run this script, [run_get_projection_maps.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_get_projection_maps.py), with the following parameters：
+
++ `-camera`: specify the camera (left, right, front, rear)
++ `-scale`: The horizontal and vertical scaling ratios of the corrected image after undistortion
++ `-shift`: The horizontal and vertical distances of the corrected image center after undistortion。
+
+
+The scale and shift parameters are needed because the default OpenCV calibration method for fisheye cameras involves cropping the corrected image to a region that OpenCV "thinks" is appropriate. This inevitably results in the loss of some pixels, especially the feature points that we may want to select.
+
+Fortunately, the function [`cv2.fisheye.initUndistortRectifyMap`](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#ga0d37b45f780b32f63ed19c21aa9fd333) allows us to provide a new intrinsic matrix, which can be used to perform a scaling and translation of the un-cropped corrected image. By adjusting the horizontal and vertical scaling ratios and the position of the image center, we can ensure that the feature points on the ground plane appear in comfortable places in the image, making it easier to perform calibration.
+
+
+```bash
+python run_get_projection_maps.py -camera front -scale 0.7 0.8 -shift -150 -100
+```
+
+The undistorted image of the front camera：
+
+
+
+Then, click on the four predetermined feature points in order (the order cannot be wrong!), and the result will look like this:
+
+
+
+The script for setting up the points is [here](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/param_settings.py#L40)。
+
+These four points can be freely set, but you need to manually modify their pixel coordinates in the bird's-eye view in the program. When you click on these four points in the corrected image, OpenCV will calculate a perspective transformation matrix based on the correspondence between their pixel coordinates in the corrected image and their corresponding coordinates in the bird view. The principle used here is that a perspective transformation can be uniquely determined by four corresponding points (four points can give eight equations, from which the eight unknowns in the perspective matrix can be solved. Note that the last component of the perspective matrix is always fixed to 1).
+
+If you accidentally click the wrong point, you can press the d key to delete the last selected point. After selecting the four points, press Enter, and the program will display the resulting bird's-eye view image:
+
+
+
+If you are satisfied with the result, press the Enter key to write the projection matrix to the front.yaml file. The name of the matrix is project_matrix. If you are not satisfied, press 'q' to exit and start over.
+
+The four points of the rear camera image：
+
+
+
+The corresponding undistorted image:
+
+
+
+We will stitch the four bird's-eye view images together using the same procedure and get their projection matrix respectively.
+
+> **Important**: It is crucial to select four points that cover as largest possible area in the image to ensure seamless stitching. Failure to do so may result in poor stitching. Despite being called as undistorted, the image may still contain distortions due to various errors in the undistortion process, particularly noticeable towards the image periphery. Therefore, we should ask OpenCV to find a globally optimized projective matrix by leveraging information from a broader image area, rather than relying solely on local and limited regions.
+
+
+# Stitching and smoothing of the birdseye view image
+
+
+If everything goes well from the previous section, and after executing this script: [run_get_weight_matrices.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_get_weight_matrices.py), you will notice the stitched birdseye view image：
+
+
+
+The logic behind this script is as follows：
+
+1. Due to the overlapping areas between adjacent cameras, the fusion of these overlapping parts is crucial for this task. If we directly use a simple weighted averaging approach with weights of 0.5 for each image, we would observe the output like the following image:
+
+
+
+You can see that due to the errors in calibration and projection, the projected results of adjacent cameras in the overlapping area do not match perfectly, resulting in garbled and ghosting effects in the stitched image. The key point is the weighting coefficients which should vary with the pixels and change continuously with them.
+
+2. Let's take the upper-left corner as an example, which is the overlapping area of the front and left cameras. We first extract the overlapping area from the projected images:
+
+
+
+ grayscale and thresholding：
+
+
+
+ We can use morphological operations to remove the noise (it doesn't need to be very precise, rough removal is enough):
+
+
+
+ Then we have the mask for this overlapping area。
+
+3. To obtain the outer boundaries of the front and left images that lie outside the overlapping region, we first use cv2.findContours to find the outermost contour and then use cv2.approxPolyDP to obtain the approximated polygonal contour：
+
+ |||
+ |:-:|:-:|
+ |||
+
+ We denote the contour obtained by subtracting the overlapping region from the front camera as polyA (blue boundary in the top left image), and the contour obtained by subtracting the overlapping region from the left camera as polyB (green boundary in the top right image).
+
+4. For each pixel in the overlapping area, we can calculate its distance to the two polygons polyA and polyB using cv2.pointPolygonTest, denoting the distances as $d_A$ and $d_B$, respectively. The weight of the pixel is then given by $w=d_B^2/(d_A^2+d_B^2)$. If the pixel falls inside the front image, then its distance to polyB will be greater, giving it a larger weight.
+
+5. For each pixel in the overlapping region, we can use cv2.pointPolygonTest to calculate its distance to the two polygons polyA and polyB. Let $d_A$ and $d_B$ be the distances from the pixel to polyA and polyB, respectively. Then the weight of the pixel is calculated as $w=d_B^2/(d_A^2+d_B^2)$. This means that if the pixel is in the front camera view, it will have a larger weight if it is farther away from polyB. For pixels outside the overlapping region, their weight is 1 if they belong to the front camera's view, and 0 otherwise. Thus, we obtain a continuous matrix $G$ with values ranging between 0 and 1. Here is the grayscale image of $G$:
+
+
+
+ By using $G$ as the weight matrix, we can get the fused image: `front * G + (1- G) * left`。
+
+6. Please note that since the pixel values in the overlapping region are the weighted average of two images, there will inevitably be ghosting artifacts for objects in this region. Therefore, we need to minimize the size of the overlapping region as much as possible and only calculate the weight values for pixels around the stitching seam. We should use the original pixels from the front image as much as possible for the pixels above the seam and the original pixels from the back image for the pixels below the seam. This step can be achieved by controlling the value of $d_B$.
+
+7. Due to the different exposure levels of different cameras, there will be brightness differences in different areas, which will affect the performance of the final stitched image. We need to adjust the brightness of each area to make the overall brightness of the stitched image tend to be consistent. And there is no unique method. After doing several searches online and then realized that the methods mentioned are either too complicated and computationally expensive or too simple and unable to achieve the ideal performance. In particular, in the example of the second video above, the field of view of the front camera is insufficient due to the obstruction of the car logo, resulting in a large difference in brightness between its image and the other three cameras, which is very difficult to adjust.
+
+ One basic idea is as follows: Each camera returns an image with three channels in BGR format, and the four cameras together provide a total of 12 channels. We need to calculate 12 coefficients, which are then multiplied with each of the 12 channels, and then combined to form the adjusted image. Channels that are too bright need to be darkened, so the coefficients are less than 1, and channels that are too dark need to be brightened, so the coefficients are greater than 1. These coefficients can be obtained from the brightness ratio of the four images in their overlapping regions. You can design the method for calculating these coefficients as you wish as long as it satisfies this basic principle.
+
+ Here is my [implementation](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/birdview.py#L210).
+
+ There is also another simple way to adjust the brightness, which is to pre-calculate a tone mapping function (such as piecewise linear or AES tone mapping function) and then force all pixels to be converted using this function. This method is the most simple one, but the color tone in the output frame may differ significantly from the actual environment.
+
+8. The final step is color balance, which is needed in some cases where the intensity of different channels may vary between cameras. Please refer to the image below:
+
+ | | | |
+ |:-:|:-:|:-:|
+ |Raw frame after stitching| After white balance | After brightness and color balance|
+ ||||
+
+ In the example of the second video, the image turns to be more red than normal. After applying color balancing, the image is back to normal.
+
+
+# Attentions
+
+
+1. Multi-threading and thread synchronization. In the two examples in this article, none of the four cameras are hardware-triggered for synchronization, and even if hardware synchronization is used, the processing threads of the four images may not be synchronized, so a thread synchronization mechanism is needed. The implementation of this project uses a relatively primitive method, and its core code is as follows:
+
+```python
+class MultiBufferManager(object):
+
+ ...
+
+ def sync(self, device_id):
+ # only perform sync if enabled for specified device/stream
+ self.mutex.lock()
+ if device_id in self.sync_devices:
+ # increment arrived count
+ self.arrived += 1
+ # we are the last to arrive: wake all waiting threads
+ if self.do_sync and self.arrived == len(self.sync_devices):
+ self.wc.wakeAll()
+ # still waiting for other streams to arrive: wait
+ else:
+ self.wc.wait(self.mutex)
+ # decrement arrived count
+ self.arrived -= 1
+ self.mutex.unlock()
+```
+Here, a MultiBufferManager object is used to manage all the threads. Each camera thread calls its sync method at each iteration and notifies the object by incrementing a counter, saying "report, I have completed the previous task, please put me in the sleep pool and wait for the next task." Once the counter reaches 4, all threads are awakened to enter the next round of task iteration.
+
+2. Creating a lookup table can speed up the processing. To stitch images from fisheye lenses, the captured images need to go through calibration, projection, and flipping before they can be used for stitching. These three steps involve frequent memory allocation and deallocation, which is time-consuming. In our experiment, the capturing threads were stable at around 30 fps, but each processing thread is only about 20 fps. To speed up this processing, it's best to precompute a lookup table. For example, the cv2.fisheye.initUndistortRectifyMap returns two lookup tables, mapx and mapy, and when you specify the matrix type cv2.CV_16SC2, mapx returned is a per-pixel lookup table, and mapy is a one-dimensional array for interpolation smoothing (which can be discarded). Similarly, a lookup table can also be obtained for the project_matrix, and combining the two will give you a lookup table that directly maps the original image to the projected image (although losing information for interpolation).
+
+In this project, Python was used for implementation; however, the for loops in Python are not very efficient, this lookup table method was not used.
+
+3. The four weight matrices can be compressed into a single image with four channels (RGBA), which is convenient for storage and retrieval. The same applied for the four overlapping mask matrices:
+
+
+
+
+
+
+# On vehicle demo
+
+
+You can run this algo on vehicle to test performance: [run_live_demo.py](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/run_live_demo.py)
+
+But you need to specify the correct porf of the cameras. For USB camera, you can directly call `cv2.VideoCapture(i)` (`i` indicates the USB port for this camera)，for CSI camera, you need to use `gstreamer` , and here is the script [CSI camera scripts](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/utils.py#L5) and [Capture thred](https://github.com/neozhaoliang/surround-view-system-introduction/blob/master/surround_view/capture_thread.py#L75)。
+
+
+# Appendix: scripts in this repo and their description
+
+
+The script squence of running this repo:
+
+1. `run_calibrate_camera.py`：intrinsic matrix calibration
+2. `param_settings.py`：set up projection matrix and related parameters
+3. `run_get_projection_maps.py`：manually select projection points and area
+4. `run_get_weight_matrices.py`：calculate the weight matrix and mask matrix for the four overlapping regions, and to display the stitching results。
+6. `run_live_demo.py`：on vehicle demo。
diff --git a/doc/img/back_proj.png b/doc/img/back_proj.png
new file mode 100644
index 0000000..2fe91b8
Binary files /dev/null and b/doc/img/back_proj.png differ
diff --git a/doc/img/bad.png b/doc/img/bad.png
new file mode 100644
index 0000000..0247671
Binary files /dev/null and b/doc/img/bad.png differ
diff --git a/doc/img/car.mp4 b/doc/img/car.mp4
new file mode 100644
index 0000000..38eebb9
Binary files /dev/null and b/doc/img/car.mp4 differ
diff --git a/doc/img/choose_back.png b/doc/img/choose_back.png
new file mode 100644
index 0000000..972b8ee
Binary files /dev/null and b/doc/img/choose_back.png differ
diff --git a/doc/img/choose_front.png b/doc/img/choose_front.png
new file mode 100644
index 0000000..aacbee7
Binary files /dev/null and b/doc/img/choose_front.png differ
diff --git a/doc/img/example1.png b/doc/img/example1.png
new file mode 100644
index 0000000..85cfeac
Binary files /dev/null and b/doc/img/example1.png differ
diff --git a/doc/img/example2.png b/doc/img/example2.png
new file mode 100644
index 0000000..306f5ab
Binary files /dev/null and b/doc/img/example2.png differ
diff --git a/doc/img/example3.png b/doc/img/example3.png
new file mode 100644
index 0000000..c7b26eb
Binary files /dev/null and b/doc/img/example3.png differ
diff --git a/doc/img/front_proj.png b/doc/img/front_proj.png
new file mode 100644
index 0000000..045ef65
Binary files /dev/null and b/doc/img/front_proj.png differ
diff --git a/doc/img/images/back.png b/doc/img/images/back.png
new file mode 100644
index 0000000..1d40bd5
Binary files /dev/null and b/doc/img/images/back.png differ
diff --git a/doc/img/images/car.png b/doc/img/images/car.png
new file mode 100644
index 0000000..d76c6c4
Binary files /dev/null and b/doc/img/images/car.png differ
diff --git a/doc/img/images/front.png b/doc/img/images/front.png
new file mode 100644
index 0000000..527895a
Binary files /dev/null and b/doc/img/images/front.png differ
diff --git a/doc/img/images/left.png b/doc/img/images/left.png
new file mode 100644
index 0000000..66e34fb
Binary files /dev/null and b/doc/img/images/left.png differ
diff --git a/doc/img/images/right.png b/doc/img/images/right.png
new file mode 100644
index 0000000..6efcefa
Binary files /dev/null and b/doc/img/images/right.png differ
diff --git a/doc/img/mask.png b/doc/img/mask.png
new file mode 100644
index 0000000..f7cf44a
Binary files /dev/null and b/doc/img/mask.png differ
diff --git a/doc/img/mask_dilate.png b/doc/img/mask_dilate.png
new file mode 100644
index 0000000..52ed102
Binary files /dev/null and b/doc/img/mask_dilate.png differ
diff --git a/doc/img/masks.png b/doc/img/masks.png
new file mode 100644
index 0000000..aafc61d
Binary files /dev/null and b/doc/img/masks.png differ
diff --git a/doc/img/original.png b/doc/img/original.png
new file mode 100644
index 0000000..6b4ec66
Binary files /dev/null and b/doc/img/original.png differ
diff --git a/doc/img/overlap.png b/doc/img/overlap.png
new file mode 100644
index 0000000..4c05262
Binary files /dev/null and b/doc/img/overlap.png differ
diff --git a/doc/img/overlap_gray.png b/doc/img/overlap_gray.png
new file mode 100644
index 0000000..28e4411
Binary files /dev/null and b/doc/img/overlap_gray.png differ
diff --git a/doc/img/paramsettings.png b/doc/img/paramsettings.png
new file mode 100644
index 0000000..758bf4a
Binary files /dev/null and b/doc/img/paramsettings.png differ
diff --git a/doc/img/polyA.png b/doc/img/polyA.png
new file mode 100644
index 0000000..35a06c5
Binary files /dev/null and b/doc/img/polyA.png differ
diff --git a/doc/img/polyB.png b/doc/img/polyB.png
new file mode 100644
index 0000000..51f6870
Binary files /dev/null and b/doc/img/polyB.png differ
diff --git a/doc/img/result.mp4 b/doc/img/result.mp4
new file mode 100644
index 0000000..7d0d0ed
Binary files /dev/null and b/doc/img/result.mp4 differ
diff --git a/doc/img/result.png b/doc/img/result.png
new file mode 100644
index 0000000..92f1eee
Binary files /dev/null and b/doc/img/result.png differ
diff --git a/doc/img/smallcar.mp4 b/doc/img/smallcar.mp4
new file mode 100644
index 0000000..f8bec8b
Binary files /dev/null and b/doc/img/smallcar.mp4 differ
diff --git a/doc/img/weight_for_FL.png b/doc/img/weight_for_FL.png
new file mode 100644
index 0000000..dcfeee8
Binary files /dev/null and b/doc/img/weight_for_FL.png differ
diff --git a/doc/img/weights.png b/doc/img/weights.png
new file mode 100644
index 0000000..11c1107
Binary files /dev/null and b/doc/img/weights.png differ
diff --git a/images/1/back.png b/images/1/back.png
new file mode 100644
index 0000000..8a116dd
Binary files /dev/null and b/images/1/back.png differ
diff --git a/images/1/front.png b/images/1/front.png
new file mode 100644
index 0000000..928baea
Binary files /dev/null and b/images/1/front.png differ
diff --git a/images/1/left.png b/images/1/left.png
new file mode 100644
index 0000000..c06ecc2
Binary files /dev/null and b/images/1/left.png differ
diff --git a/images/1/right.png b/images/1/right.png
new file mode 100644
index 0000000..19d4c40
Binary files /dev/null and b/images/1/right.png differ
diff --git a/images/2/back.png b/images/2/back.png
new file mode 100644
index 0000000..41b14de
Binary files /dev/null and b/images/2/back.png differ
diff --git a/images/2/front.png b/images/2/front.png
new file mode 100644
index 0000000..7f6df08
Binary files /dev/null and b/images/2/front.png differ
diff --git a/images/2/left.png b/images/2/left.png
new file mode 100644
index 0000000..c4cf37b
Binary files /dev/null and b/images/2/left.png differ
diff --git a/images/2/right.png b/images/2/right.png
new file mode 100644
index 0000000..322eecc
Binary files /dev/null and b/images/2/right.png differ
diff --git a/images/3/back.png b/images/3/back.png
new file mode 100644
index 0000000..79c1535
Binary files /dev/null and b/images/3/back.png differ
diff --git a/images/3/front.png b/images/3/front.png
new file mode 100644
index 0000000..1ffe8a3
Binary files /dev/null and b/images/3/front.png differ
diff --git a/images/3/left.png b/images/3/left.png
new file mode 100644
index 0000000..3eff90b
Binary files /dev/null and b/images/3/left.png differ
diff --git a/images/3/right.png b/images/3/right.png
new file mode 100644
index 0000000..d78dc1e
Binary files /dev/null and b/images/3/right.png differ
diff --git a/images/back.png b/images/back.png
new file mode 100644
index 0000000..826ec4a
Binary files /dev/null and b/images/back.png differ
diff --git a/images/car.png b/images/car.png
new file mode 100644
index 0000000..d76c6c4
Binary files /dev/null and b/images/car.png differ
diff --git a/images/front.png b/images/front.png
new file mode 100644
index 0000000..d0ce919
Binary files /dev/null and b/images/front.png differ
diff --git a/images/left.png b/images/left.png
new file mode 100644
index 0000000..7989af2
Binary files /dev/null and b/images/left.png differ
diff --git a/images/right.png b/images/right.png
new file mode 100644
index 0000000..8bcce32
Binary files /dev/null and b/images/right.png differ
diff --git a/masks.png b/masks.png
new file mode 100644
index 0000000..4b88e20
Binary files /dev/null and b/masks.png differ
diff --git a/run_calibrate_camera.py b/run_calibrate_camera.py
new file mode 100644
index 0000000..1a2a055
--- /dev/null
+++ b/run_calibrate_camera.py
@@ -0,0 +1,355 @@
+import argparse
+import os
+import numpy as np
+import cv2
+from surround_view import CaptureThread, MultiBufferManager
+import surround_view.utils as utils
+import re
+
+# 保存相机参数文件的目录
+TARGET_DIR = os.path.join(os.getcwd(), "yaml")
+
+# 默认参数文件路径
+DEFAULT_PARAM_FILE = os.path.join(TARGET_DIR, "camera_params.yaml")
+
+
+def is_rtsp_url(input_str):
+ """检查输入是否为RTSP URL"""
+ return input_str.startswith('rtsp://')
+
+
+def main():
+ parser = argparse.ArgumentParser()
+
+ # 输入视频流（可以是相机设备索引或RTSP URL）
+ parser.add_argument("-i", "--input", type=str, default="0",
+ help="输入相机设备索引或RTSP URL")
+
+ # 棋盘格图案尺寸
+ parser.add_argument("-grid", "--grid", default="9x6",
+ help="标定棋盘格的尺寸")
+
+ parser.add_argument("-r", "--resolution", default="640x480",
+ help="相机图像的分辨率")
+
+ parser.add_argument("-framestep", type=int, default=20,
+ help="视频中每隔n帧使用一次")
+
+ parser.add_argument("-o", "--output", default=DEFAULT_PARAM_FILE,
+ help="输出yaml文件的路径")
+
+ parser.add_argument("-fisheye", "--fisheye", action="store_true",
+ help="如果是鱼眼相机则设置为true")
+
+ parser.add_argument("-flip", "--flip", default=0, type=int,
+ help="相机的翻转方式")
+
+ parser.add_argument("--no_gst", action="store_true",
+ help="如果不使用gstreamer捕获相机则设置为true")
+
+ args = parser.parse_args()
+
+ if not os.path.exists(TARGET_DIR):
+ os.mkdir(TARGET_DIR)
+
+ text1 = "按c键进行标定"
+ text2 = "按q键退出"
+ text3 = "设备: {}".format(args.input)
+ font = cv2.FONT_HERSHEY_SIMPLEX
+ fontscale = 0.6
+
+ resolution_str = args.resolution.split("x")
+ target_W = int(resolution_str[0])
+ target_H = int(resolution_str[1])
+ grid_size = tuple(int(x) for x in args.grid.split("x"))
+ grid_points = np.zeros((1, np.prod(grid_size), 3), np.float32)
+ grid_points[0, :, :2] = np.indices(grid_size).T.reshape(-1, 2)
+
+ objpoints = [] # 真实世界中的3D点
+ imgpoints = [] # 图像平面中的2D点
+
+ # 检查输入是RTSP URL还是设备索引
+ if is_rtsp_url(args.input):
+ rtsp_url = args.input
+ # 初始化VideoCapture（强制使用FFmpeg后端）
+ cap = cv2.VideoCapture(rtsp_url, cv2.CAP_FFMPEG)
+
+ # 配置H.265解码参数（关键修改）
+ try:
+ # 基础参数：使用TCP传输避免丢包
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "rtsp_transport", "tcp")
+ # 明确指定H.265编码（hevc是H.265的标准编码名）
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "vcodec", "hevc")
+ # 低延迟配置
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "flags", "low_delay")
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "fflags", "nobuffer")
+ # 设置缓冲区大小为1，减少延迟
+ cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+ # 设置超时参数，避免无限等待
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "stimeout", "5000000") # 5秒超时
+ except Exception as e:
+ print(f"警告: 设置FFmpeg参数时出错: {e}")
+
+ # 第一次连接失败：尝试简化参数（仅保留必要参数）
+ if not cap.isOpened():
+ print("首次RTSP连接失败，尝试简化参数...")
+ cap.release()
+ cap = cv2.VideoCapture(rtsp_url, cv2.CAP_FFMPEG)
+ try:
+ # 仅保留最关键的TCP传输参数
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "rtsp_transport", "tcp")
+ cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "stimeout", "5000000")
+ except Exception as e:
+ print(f"简化参数设置失败: {e}")
+
+ # 最终检查连接状态
+ if not cap.isOpened():
+ print(f"无法打开RTSP流: {rtsp_url}")
+ print("可能的原因: 1. FFmpeg不支持H.265; 2. 相机认证失败; 3. 网络连接问题")
+ return
+
+ # 获取流的实际分辨率
+ actual_W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+ actual_H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+ W, H = actual_W, actual_H
+ print(f"检测到RTSP流分辨率: {W}x{H}")
+
+ # 尝试设置目标分辨率（RTSP流可能不支持修改分辨率）
+ if actual_W != target_W or actual_H != target_H:
+ print(f"尝试将分辨率设置为 {target_W}x{target_H}...")
+ cap.set(cv2.CAP_PROP_FRAME_WIDTH, target_W)
+ cap.set(cv2.CAP_PROP_FRAME_HEIGHT, target_H)
+
+ # 验证是否设置成功
+ new_W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+ new_H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+ if new_W != target_W or new_H != target_H:
+ print(f"无法设置分辨率（RTSP流可能为固定分辨率），使用 {new_W}x{new_H}")
+ W, H = new_W, new_H
+
+ use_rtsp = True
+ else:
+ # 处理本地相机设备
+ device = int(args.input)
+ cap_thread = CaptureThread(device_id=device,
+ flip_method=args.flip,
+ resolution=(target_W, target_H),
+ use_gst=not args.no_gst,
+ )
+ buffer_manager = MultiBufferManager()
+ buffer_manager.bind_thread(cap_thread, buffer_size=8)
+ if cap_thread.connect_camera():
+ cap_thread.start()
+ use_rtsp = False
+ W, H = target_W, target_H
+ else:
+ print("无法打开本地相机设备")
+ return
+
+ quit = False
+ do_calib = False
+ i = -1
+
+ print("\n开始标定过程...")
+ print("按'c'键开始标定（需要≥12个有效的棋盘格帧）")
+ print("按'q'键退出")
+
+ # 创建可调整大小的窗口
+ cv2.namedWindow("corners", cv2.WINDOW_NORMAL)
+
+ while True:
+ i += 1
+
+ # 根据输入类型读取帧
+ if use_rtsp:
+ ret, img = cap.read()
+ if not ret:
+ print("读取RTSP帧失败，重试...")
+ # 帧读取失败时重试连接
+ cap.release()
+ cap = cv2.VideoCapture(rtsp_url, cv2.CAP_FFMPEG)
+ # 重新设置关键参数
+ try:
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "rtsp_transport", "tcp")
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "vcodec", "hevc")
+ cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+ except:
+ pass
+ continue
+ else:
+ img = buffer_manager.get_device(device).get().image
+ if img is None:
+ print("读取本地相机帧失败")
+ break
+
+ # 每隔N帧处理一次
+ if i % args.framestep != 0:
+ continue
+
+ print(f"在第{i}帧中搜索棋盘格角点...")
+ gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ # 检测棋盘格角点（增强鲁棒性参数）
+ found, corners = cv2.findChessboardCorners(
+ gray,
+ grid_size,
+ cv2.CALIB_CB_ADAPTIVE_THRESH +
+ cv2.CALIB_CB_NORMALIZE_IMAGE +
+ cv2.CALIB_CB_FILTER_QUADS +
+ cv2.CALIB_CB_FAST_CHECK # 快速校验，减少误检
+ )
+ if found:
+ # 亚像素级角点优化
+ term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.01)
+ cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), term)
+ print("✓ 检测到角点")
+ imgpoints.append(corners)
+ objpoints.append(grid_points)
+ # 绘制角点到图像
+ cv2.drawChessboardCorners(img, grid_size, corners, found)
+ else:
+ print("✗ 未找到角点")
+
+ # 绘制状态文本
+ cv2.putText(img, text1, (20, 70), font, fontscale, (255, 200, 0), 2)
+ cv2.putText(img, text2, (20, 110), font, fontscale, (255, 200, 0), 2)
+ cv2.putText(img, text3, (20, 30), font, fontscale, (255, 200, 0), 2)
+ cv2.putText(img, f"分辨率: {img.shape[1]}x{img.shape[0]} | 有效帧数: {len(objpoints)}",
+ (20, 150), font, fontscale, (255, 200, 0), 2)
+
+ # 自动缩放过大图像
+ max_display_width = 1280
+ max_display_height = 720
+ scale = min(max_display_width / img.shape[1], max_display_height / img.shape[0])
+ if scale < 1:
+ img = cv2.resize(img, (int(img.shape[1] * scale), int(img.shape[0] * scale)))
+
+ cv2.imshow("corners", img)
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord("c"):
+ print("\n=== 开始标定 ===")
+ N_OK = len(objpoints)
+ if N_OK < 12:
+ print(f"错误: 只有{N_OK}个有效帧（需要≥12个）。继续采集...")
+ else:
+ do_calib = True
+ break
+
+ elif key == ord("q"):
+ quit = True
+ break
+
+ # 资源清理
+ if use_rtsp:
+ cap.release()
+ else:
+ cap_thread.stop()
+ cap_thread.disconnect_camera()
+ cv2.destroyAllWindows()
+
+ if quit:
+ print("用户退出程序")
+ return
+
+ if do_calib:
+ N_OK = len(objpoints)
+ K = np.zeros((3, 3)) # 相机内参矩阵
+ D = np.zeros((4, 1)) # 畸变系数
+ rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for _ in range(N_OK)] # 旋转向量
+ tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for _ in range(N_OK)] # 平移向量
+ # 鱼眼标定 flags
+ calibration_flags = (cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC +
+ # cv2.fisheye.CALIB_CHECK_COND +
+ cv2.fisheye.CALIB_FIX_SKEW)
+
+ if args.fisheye:
+ # 鱼眼相机标定
+ ret, mtx, dist, rvecs, tvecs = cv2.fisheye.calibrate(
+ objpoints,
+ imgpoints,
+ (W, H),
+ K,
+ D,
+ rvecs,
+ tvecs,
+ calibration_flags,
+ (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
+ )
+ else:
+ # 普通相机标定
+ ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
+ objpoints,
+ imgpoints,
+ (W, H),
+ None,
+ None)
+
+ if ret:
+ # 保存标定结果到YAML文件
+ fs = cv2.FileStorage(args.output, cv2.FILE_STORAGE_WRITE)
+ fs.write("resolution", np.int32([W, H]))
+ fs.write("camera_matrix", K)
+ fs.write("dist_coeffs", D)
+ fs.release()
+ print(f"✓ 标定成功!")
+ print(f" 保存至: {args.output}")
+ print(f" 使用的有效帧数: {N_OK}")
+
+ # 显示标定结果
+ print("\n=== 标定结果 ===")
+ print("相机内参矩阵 (K):")
+ print(np.round(K, 4))
+ print("\n畸变系数 (D):")
+ print(np.round(D.T, 6))
+
+ # 展示畸变校正前后对比
+ print("\n显示原始图像与校正后图像（5秒）...")
+ if use_rtsp:
+ # 重新打开RTSP流获取测试帧
+ cap = cv2.VideoCapture(rtsp_url, cv2.CAP_FFMPEG)
+ try:
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "rtsp_transport", "tcp")
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "vcodec", "hevc")
+ cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+ except:
+ pass
+ ret, test_img = cap.read()
+ cap.release()
+ else:
+ # 从本地相机获取测试帧
+ test_img = buffer_manager.get_device(device).get().image
+
+ if test_img is not None:
+ if args.fisheye:
+ # 鱼眼图像校正
+ map1, map2 = cv2.fisheye.initUndistortRectifyMap(
+ K, D, np.eye(3), K, (W, H), cv2.CV_16SC2)
+ undistorted = cv2.remap(test_img, map1, map2,
+ interpolation=cv2.INTER_LINEAR,
+ borderMode=cv2.BORDER_CONSTANT)
+ else:
+ # 普通图像校正
+ undistorted = cv2.undistort(test_img, K, D)
+
+ # 拼接对比图并显示
+ comparison = np.hstack((test_img, undistorted))
+ cv2.putText(comparison, "原始图像(左) vs 校正后图像(右)",
+ (10, 30), font, 0.7, (0, 255, 0), 2)
+
+ # 缩放对比图适配屏幕
+ scale = min(max_display_width / comparison.shape[1], max_display_height / comparison.shape[0])
+ if scale < 1:
+ comparison = cv2.resize(comparison,
+ (int(comparison.shape[1] * scale),
+ int(comparison.shape[0] * scale)))
+
+ cv2.imshow("标定结果: 原始图像 vs 校正后图像", comparison)
+ cv2.waitKey(5000) # 显示5秒
+ cv2.destroyAllWindows()
+
+ else:
+ print("✗ 标定失败! (检查棋盘格检测或帧质量)")
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/run_get_projection_maps.py b/run_get_projection_maps.py
new file mode 100644
index 0000000..8d30b26
--- /dev/null
+++ b/run_get_projection_maps.py
@@ -0,0 +1,71 @@
+"""
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Manually select points to get the projection map
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+"""
+import argparse
+import os
+import numpy as np
+import cv2
+from surround_view import FisheyeCameraModel, PointSelector, display_image
+import surround_view.param_settings as settings
+
+
+def get_projection_map(camera_model, image):
+ und_image = camera_model.undistort(image)
+ name = camera_model.camera_name
+ gui = PointSelector(und_image, title=name)
+ dst_points = settings.project_keypoints[name]
+ choice = gui.loop()
+ if choice > 0:
+ src = np.float32(gui.keypoints)
+ dst = np.float32(dst_points)
+ camera_model.project_matrix = cv2.getPerspectiveTransform(src, dst)
+ proj_image = camera_model.project(und_image)
+
+ ret = display_image("Bird's View", proj_image)
+ if ret > 0:
+ return True
+ if ret < 0:
+ cv2.destroyAllWindows()
+
+ return False
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("-camera", required=True,
+ choices=["front", "back", "left", "right"],
+ help="The camera view to be projected")
+ parser.add_argument("-scale", nargs="+", default=None,
+ help="scale the undistorted image")
+ parser.add_argument("-shift", nargs="+", default=None,
+ help="shift the undistorted image")
+ args = parser.parse_args()
+
+ if args.scale is not None:
+ scale = [float(x) for x in args.scale]
+ else:
+ scale = (1.0, 1.0)
+
+ if args.shift is not None:
+ shift = [float(x) for x in args.shift]
+ else:
+ shift = (0, 0)
+
+ camera_name = args.camera
+ camera_file = os.path.join(os.getcwd(), "yaml", camera_name + ".yaml")
+ image_file = os.path.join(os.getcwd(), "images", camera_name + ".png")
+ image = cv2.imread(image_file)
+ camera = FisheyeCameraModel(camera_file, camera_name)
+ camera.set_scale_and_shift(scale, shift)
+ success = get_projection_map(camera, image)
+ if success:
+ print("saving projection matrix to yaml")
+ camera.save_data()
+ else:
+ print("failed to compute the projection map")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/run_get_weight_matrices.py b/run_get_weight_matrices.py
new file mode 100644
index 0000000..a38dd30
--- /dev/null
+++ b/run_get_weight_matrices.py
@@ -0,0 +1,36 @@
+import os
+import numpy as np
+import cv2
+from PIL import Image
+from surround_view import FisheyeCameraModel, display_image, BirdView
+import surround_view.param_settings as settings
+
+
+def main():
+ names = settings.camera_names
+ images = [os.path.join(os.getcwd(), "images", name + ".png") for name in names]
+ yamls = [os.path.join(os.getcwd(), "yaml", name + ".yaml") for name in names]
+ camera_models = [FisheyeCameraModel(camera_file, camera_name) for camera_file, camera_name in zip (yamls, names)]
+
+ projected = []
+ for image_file, camera in zip(images, camera_models):
+ img = cv2.imread(image_file)
+ img = camera.undistort(img)
+ img = camera.project(img)
+ img = camera.flip(img)
+ projected.append(img)
+
+ birdview = BirdView()
+ Gmat, Mmat = birdview.get_weights_and_masks(projected)
+ birdview.update_frames(projected)
+ birdview.make_luminance_balance().stitch_all_parts()
+ birdview.make_white_balance()
+ birdview.copy_car_image()
+ ret = display_image("BirdView Result", birdview.image)
+ if ret > 0:
+ Image.fromarray((Gmat * 255).astype(np.uint8)).save("weights.png")
+ Image.fromarray(Mmat.astype(np.uint8)).save("masks.png")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/run_live_demo.py b/run_live_demo.py
new file mode 100644
index 0000000..35c05d5
--- /dev/null
+++ b/run_live_demo.py
@@ -0,0 +1,140 @@
+import os
+import cv2
+import re
+from surround_view import CaptureThread, CameraProcessingThread
+from surround_view import FisheyeCameraModel, BirdView
+from surround_view import MultiBufferManager, ProjectedImageBuffer
+import surround_view.param_settings as settings
+
+
+def is_rtsp_url(input_str):
+ """检查输入是否为RTSP URL"""
+ return input_str.startswith('rtsp://')
+
+
+def main():
+ # 配置四个RTSP摄像头地址（前、后、左、右）
+ camera_ids = [
+ "rtsp://admin:@192.168.112.153:554/video", # 前
+ "rtsp://admin:@192.168.112.152:554/video", # 后
+ "rtsp://admin:@192.168.112.150:554/video", # 左
+ "rtsp://admin:@192.168.112.151:554/video" # 右
+ ]
+
+ # 摄像头ID映射（使用索引作为设备ID）
+ camera_id_mapping = {i: cam_id for i, cam_id in enumerate(camera_ids)}
+
+ # 摄像头翻转参数（根据实际安装方向调整）
+ flip_methods = [0, 0, 0, 0] # 0:不翻转, 1:水平翻转, 2:垂直翻转, 3:水平+垂直
+
+ # 加载相机内参模型
+ yamls_dir = os.path.join(os.getcwd(), "yaml")
+ camera_files = [os.path.join(yamls_dir, name + ".yaml") for name in settings.camera_names]
+ camera_models = [FisheyeCameraModel(camera_file, name)
+ for camera_file, name in zip(camera_files, settings.camera_names)]
+
+ # 初始化捕获线程（针对RTSP特殊处理）
+ capture_tds = []
+ for idx, (cam_id, flip) in enumerate(zip(camera_ids, flip_methods)):
+ # 使用索引作为设备ID，解决KeyError问题
+ if is_rtsp_url(cam_id):
+ # RTSP流配置（使用FFmpeg后端）
+ capture_tds.append(CaptureThread(
+ device_id=idx, # 使用索引作为设备ID
+ flip_method=flip,
+ use_gst=False, # 关闭GStreamer，使用FFmpeg
+ api_preference=cv2.CAP_FFMPEG, # 强制使用FFmpeg
+ resolution=(960, 640)
+ ))
+ # 手动打开RTSP流
+ if not capture_tds[-1].cap.open(cam_id, cv2.CAP_FFMPEG):
+ print(f"无法打开RTSP流: {cam_id}")
+ return
+ # 设置RTSP参数（兼容不同OpenCV版本）
+ try:
+ # 尝试通用参数设置方式
+ capture_tds[-1].cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) # 减少缓冲
+ capture_tds[-1].cap.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
+ capture_tds[-1].cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 640)
+ # 处理FFMPEG选项（兼容旧版本）
+ if hasattr(cv2, 'CAP_PROP_FFMPEG_OPTION'):
+ capture_tds[-1].cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "rtsp_transport", "tcp")
+ capture_tds[-1].cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "stimeout", "5000000")
+ else:
+ print(f"RTSP流 {cam_id} 使用兼容模式，部分参数可能无法设置")
+ except Exception as e:
+ print(f"设置RTSP参数警告: {e}")
+ else:
+ # 普通USB/CSI相机配置
+ capture_tds.append(CaptureThread(
+ device_id=idx, # 使用索引作为设备ID
+ flip_method=flip,
+ use_gst=True,
+ resolution=(960, 640)
+ ))
+ if not capture_tds[-1].connect_camera():
+ print(f"启动摄像头 {cam_id} 失败，退出程序")
+ return
+
+ # 绑定捕获缓冲区管理器
+ capture_buffer_manager = MultiBufferManager()
+ for td in capture_tds:
+ capture_buffer_manager.bind_thread(td, buffer_size=8)
+ td.start()
+
+ # 初始化处理线程（使用索引作为device_id）
+ proc_buffer_manager = ProjectedImageBuffer()
+ process_tds = [
+ CameraProcessingThread(
+ capture_buffer_manager,
+ device_id=idx, # 使用索引作为设备ID
+ camera_model=model
+ )
+ for idx, model in enumerate(camera_models)
+ ]
+
+ # 启动处理线程
+ for td in process_tds:
+ proc_buffer_manager.bind_thread(td)
+ td.start()
+
+ # 初始化环视拼接
+ birdview = BirdView(proc_buffer_manager)
+ birdview.load_weights_and_masks("./weights.png", "./masks.png")
+ birdview.start()
+
+ # 主循环显示
+ try:
+ while True:
+ birdview_img = birdview.get()
+ if birdview_img is not None:
+ display_img = cv2.resize(birdview_img, (800, 600))
+ cv2.imshow("Surround View", display_img)
+
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord("q"):
+ break
+
+ # 打印帧率信息
+ status = []
+ for td in capture_tds:
+ cam_id = camera_id_mapping[td.device_id]
+ status.append(f"捕获 {cam_id.split('@')[-1].split(':')[0]} FPS: {td.stat_data.average_fps:.1f}")
+ for td in process_tds:
+ cam_name = settings.camera_names[td.device_id]
+ status.append(f"处理 {cam_name} FPS: {td.stat_data.average_fps:.1f}")
+ status.append(f"全景 FPS: {birdview.stat_data.average_fps:.1f}")
+ print(" | ".join(status), end="\r")
+
+ finally:
+ for td in process_tds:
+ td.stop()
+ for td in capture_tds:
+ td.stop()
+ td.disconnect_camera()
+ birdview.stop()
+ cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/surround_view/__init__.py b/surround_view/__init__.py
new file mode 100644
index 0000000..c76a305
--- /dev/null
+++ b/surround_view/__init__.py
@@ -0,0 +1,6 @@
+from .fisheye_camera import FisheyeCameraModel
+from .imagebuffer import MultiBufferManager
+from .capture_thread import CaptureThread
+from .process_thread import CameraProcessingThread
+from .simple_gui import display_image, PointSelector
+from .birdview import BirdView, ProjectedImageBuffer
diff --git a/surround_view/base_thread.py b/surround_view/base_thread.py
new file mode 100644
index 0000000..37752eb
--- /dev/null
+++ b/surround_view/base_thread.py
@@ -0,0 +1,52 @@
+from queue import Queue
+import cv2
+from PyQt5.QtCore import (QThread, QTime, QMutex, pyqtSignal, QMutexLocker)
+
+from .structures import ThreadStatisticsData
+
+
+class BaseThread(QThread):
+
+ """
+ Base class for all types of threads (capture, processing, stitching, ...,
+ etc). Mainly for collecting statistics of the threads.
+ """
+
+ FPS_STAT_QUEUE_LENGTH = 32
+
+ update_statistics_gui = pyqtSignal(ThreadStatisticsData)
+
+ def __init__(self, parent=None):
+ super(BaseThread, self).__init__(parent)
+ self.init_commons()
+
+ def init_commons(self):
+ self.stopped = False
+ self.stop_mutex = QMutex()
+ self.clock = QTime()
+ self.fps = Queue()
+ self.processing_time = 0
+ self.processing_mutex = QMutex()
+ self.fps_sum = 0
+ self.stat_data = ThreadStatisticsData()
+
+ def stop(self):
+ with QMutexLocker(self.stop_mutex):
+ self.stopped = True
+
+ def update_fps(self, dt):
+ # add instantaneous fps value to queue
+ if dt > 0:
+ self.fps.put(1000 / dt)
+
+ # discard redundant items in the fps queue
+ if self.fps.qsize() > self.FPS_STAT_QUEUE_LENGTH:
+ self.fps.get()
+
+ # update statistics
+ if self.fps.qsize() == self.FPS_STAT_QUEUE_LENGTH:
+ while not self.fps.empty():
+ self.fps_sum += self.fps.get()
+
+ self.stat_data.average_fps = round(self.fps_sum / self.FPS_STAT_QUEUE_LENGTH, 2)
+ self.fps_sum = 0
diff --git a/surround_view/birdview.py b/surround_view/birdview.py
new file mode 100644
index 0000000..514b02f
--- /dev/null
+++ b/surround_view/birdview.py
@@ -0,0 +1,339 @@
+import os
+import numpy as np
+import cv2
+from PIL import Image
+from PyQt5.QtCore import QMutex, QWaitCondition, QMutexLocker
+from .base_thread import BaseThread
+from .imagebuffer import Buffer
+from . import param_settings as settings
+from .param_settings import xl, xr, yt, yb
+from . import utils
+
+
+class ProjectedImageBuffer(object):
+
+ """
+ Class for synchronizing processing threads from different cameras.
+ """
+
+ def __init__(self, drop_if_full=True, buffer_size=8):
+ self.drop_if_full = drop_if_full
+ self.buffer = Buffer(buffer_size)
+ self.sync_devices = set()
+ self.wc = QWaitCondition()
+ self.mutex = QMutex()
+ self.arrived = 0
+ self.current_frames = dict()
+
+ def bind_thread(self, thread):
+ with QMutexLocker(self.mutex):
+ self.sync_devices.add(thread.device_id)
+
+ name = thread.camera_model.camera_name
+ shape = settings.project_shapes[name]
+ self.current_frames[thread.device_id] = np.zeros(shape[::-1] + (3,), np.uint8)
+ thread.proc_buffer_manager = self
+
+ def get(self):
+ return self.buffer.get()
+
+ def set_frame_for_device(self, device_id, frame):
+ if device_id not in self.sync_devices:
+ raise ValueError("Device not held by the buffer: {}".format(device_id))
+ self.current_frames[device_id] = frame
+
+ def sync(self, device_id):
+ # only perform sync if enabled for specified device/stream
+ self.mutex.lock()
+ if device_id in self.sync_devices:
+ # increment arrived count
+ self.arrived += 1
+ # we are the last to arrive: wake all waiting threads
+ if self.arrived == len(self.sync_devices):
+ self.buffer.add(self.current_frames, self.drop_if_full)
+ self.wc.wakeAll()
+ # still waiting for other streams to arrive: wait
+ else:
+ self.wc.wait(self.mutex)
+ # decrement arrived count
+ self.arrived -= 1
+ self.mutex.unlock()
+
+ def wake_all(self):
+ with QMutexLocker(self.mutex):
+ self.wc.wakeAll()
+
+ def __contains__(self, device_id):
+ return device_id in self.sync_devices
+
+ def __str__(self):
+ return (self.__class__.__name__ + ":\n" + \
+ "devices: {}\n".format(self.sync_devices))
+
+
+def FI(front_image):
+ return front_image[:, :xl]
+
+
+def FII(front_image):
+ return front_image[:, xr:]
+
+
+def FM(front_image):
+ return front_image[:, xl:xr]
+
+
+def BIII(back_image):
+ return back_image[:, :xl]
+
+
+def BIV(back_image):
+ return back_image[:, xr:]
+
+
+def BM(back_image):
+ return back_image[:, xl:xr]
+
+
+def LI(left_image):
+ return left_image[:yt, :]
+
+
+def LIII(left_image):
+ return left_image[yb:, :]
+
+
+def LM(left_image):
+ return left_image[yt:yb, :]
+
+
+def RII(right_image):
+ return right_image[:yt, :]
+
+
+def RIV(right_image):
+ return right_image[yb:, :]
+
+
+def RM(right_image):
+ return right_image[yt:yb, :]
+
+
+class BirdView(BaseThread):
+
+ def __init__(self,
+ proc_buffer_manager=None,
+ drop_if_full=True,
+ buffer_size=8,
+ parent=None):
+ super(BirdView, self).__init__(parent)
+ self.proc_buffer_manager = proc_buffer_manager
+ self.drop_if_full = drop_if_full
+ self.buffer = Buffer(buffer_size)
+ self.image = np.zeros((settings.total_h, settings.total_w, 3), np.uint8)
+ self.weights = None
+ self.masks = None
+ self.car_image = settings.car_image
+ self.frames = None
+
+ def get(self):
+ return self.buffer.get()
+
+ def update_frames(self, images):
+ self.frames = images
+
+ def load_weights_and_masks(self, weights_image, masks_image):
+ GMat = np.asarray(Image.open(weights_image).convert("RGBA"), dtype=np.float64) / 255.0
+ self.weights = [np.stack((GMat[:, :, k],
+ GMat[:, :, k],
+ GMat[:, :, k]), axis=2)
+ for k in range(4)]
+
+ Mmat = np.asarray(Image.open(masks_image).convert("RGBA"), dtype=np.float64)
+ Mmat = utils.convert_binary_to_bool(Mmat)
+ self.masks = [Mmat[:, :, k] for k in range(4)]
+
+ def merge(self, imA, imB, k):
+ G = self.weights[k]
+ return (imA * G + imB * (1 - G)).astype(np.uint8)
+
+ @property
+ def FL(self):
+ return self.image[:yt, :xl]
+
+ @property
+ def F(self):
+ return self.image[:yt, xl:xr]
+
+ @property
+ def FR(self):
+ return self.image[:yt, xr:]
+
+ @property
+ def BL(self):
+ return self.image[yb:, :xl]
+
+ @property
+ def B(self):
+ return self.image[yb:, xl:xr]
+
+ @property
+ def BR(self):
+ return self.image[yb:, xr:]
+
+ @property
+ def L(self):
+ return self.image[yt:yb, :xl]
+
+ @property
+ def R(self):
+ return self.image[yt:yb, xr:]
+
+ @property
+ def C(self):
+ return self.image[yt:yb, xl:xr]
+
+ def stitch_all_parts(self):
+ front, back, left, right = self.frames
+ np.copyto(self.F, FM(front))
+ np.copyto(self.B, BM(back))
+ np.copyto(self.L, LM(left))
+ np.copyto(self.R, RM(right))
+ np.copyto(self.FL, self.merge(FI(front), LI(left), 0))
+ np.copyto(self.FR, self.merge(FII(front), RII(right), 1))
+ np.copyto(self.BL, self.merge(BIII(back), LIII(left), 2))
+ np.copyto(self.BR, self.merge(BIV(back), RIV(right), 3))
+
+ def copy_car_image(self):
+ np.copyto(self.C, self.car_image)
+
+ def make_luminance_balance(self):
+
+ def tune(x):
+ if x >= 1:
+ return x * np.exp((1 - x) * 0.5)
+ else:
+ return x * np.exp((1 - x) * 0.8)
+
+ front, back, left, right = self.frames
+ m1, m2, m3, m4 = self.masks
+ Fb, Fg, Fr = cv2.split(front)
+ Bb, Bg, Br = cv2.split(back)
+ Lb, Lg, Lr = cv2.split(left)
+ Rb, Rg, Rr = cv2.split(right)
+
+ a1 = utils.mean_luminance_ratio(RII(Rb), FII(Fb), m2)
+ a2 = utils.mean_luminance_ratio(RII(Rg), FII(Fg), m2)
+ a3 = utils.mean_luminance_ratio(RII(Rr), FII(Fr), m2)
+
+ b1 = utils.mean_luminance_ratio(BIV(Bb), RIV(Rb), m4)
+ b2 = utils.mean_luminance_ratio(BIV(Bg), RIV(Rg), m4)
+ b3 = utils.mean_luminance_ratio(BIV(Br), RIV(Rr), m4)
+
+ c1 = utils.mean_luminance_ratio(LIII(Lb), BIII(Bb), m3)
+ c2 = utils.mean_luminance_ratio(LIII(Lg), BIII(Bg), m3)
+ c3 = utils.mean_luminance_ratio(LIII(Lr), BIII(Br), m3)
+
+ d1 = utils.mean_luminance_ratio(FI(Fb), LI(Lb), m1)
+ d2 = utils.mean_luminance_ratio(FI(Fg), LI(Lg), m1)
+ d3 = utils.mean_luminance_ratio(FI(Fr), LI(Lr), m1)
+
+ t1 = (a1 * b1 * c1 * d1)**0.25
+ t2 = (a2 * b2 * c2 * d2)**0.25
+ t3 = (a3 * b3 * c3 * d3)**0.25
+
+ x1 = t1 / (d1 / a1)**0.5
+ x2 = t2 / (d2 / a2)**0.5
+ x3 = t3 / (d3 / a3)**0.5
+
+ x1 = tune(x1)
+ x2 = tune(x2)
+ x3 = tune(x3)
+
+ Fb = utils.adjust_luminance(Fb, x1)
+ Fg = utils.adjust_luminance(Fg, x2)
+ Fr = utils.adjust_luminance(Fr, x3)
+
+ y1 = t1 / (b1 / c1)**0.5
+ y2 = t2 / (b2 / c2)**0.5
+ y3 = t3 / (b3 / c3)**0.5
+
+ y1 = tune(y1)
+ y2 = tune(y2)
+ y3 = tune(y3)
+
+ Bb = utils.adjust_luminance(Bb, y1)
+ Bg = utils.adjust_luminance(Bg, y2)
+ Br = utils.adjust_luminance(Br, y3)
+
+ z1 = t1 / (c1 / d1)**0.5
+ z2 = t2 / (c2 / d2)**0.5
+ z3 = t3 / (c3 / d3)**0.5
+
+ z1 = tune(z1)
+ z2 = tune(z2)
+ z3 = tune(z3)
+
+ Lb = utils.adjust_luminance(Lb, z1)
+ Lg = utils.adjust_luminance(Lg, z2)
+ Lr = utils.adjust_luminance(Lr, z3)
+
+ w1 = t1 / (a1 / b1)**0.5
+ w2 = t2 / (a2 / b2)**0.5
+ w3 = t3 / (a3 / b3)**0.5
+
+ w1 = tune(w1)
+ w2 = tune(w2)
+ w3 = tune(w3)
+
+ Rb = utils.adjust_luminance(Rb, w1)
+ Rg = utils.adjust_luminance(Rg, w2)
+ Rr = utils.adjust_luminance(Rr, w3)
+
+ self.frames = [cv2.merge((Fb, Fg, Fr)),
+ cv2.merge((Bb, Bg, Br)),
+ cv2.merge((Lb, Lg, Lr)),
+ cv2.merge((Rb, Rg, Rr))]
+ return self
+
+ def get_weights_and_masks(self, images):
+ front, back, left, right = images
+ G0, M0 = utils.get_weight_mask_matrix(FI(front), LI(left))
+ G1, M1 = utils.get_weight_mask_matrix(FII(front), RII(right))
+ G2, M2 = utils.get_weight_mask_matrix(BIII(back), LIII(left))
+ G3, M3 = utils.get_weight_mask_matrix(BIV(back), RIV(right))
+ self.weights = [np.stack((G, G, G), axis=2) for G in (G0, G1, G2, G3)]
+ self.masks = [(M / 255.0).astype(int) for M in (M0, M1, M2, M3)]
+ return np.stack((G0, G1, G2, G3), axis=2), np.stack((M0, M1, M2, M3), axis=2)
+
+ def make_white_balance(self):
+ self.image = utils.make_white_balance(self.image)
+
+ def run(self):
+ if self.proc_buffer_manager is None:
+ raise ValueError("This thread requires a buffer of projected images to run")
+
+ while True:
+ self.stop_mutex.lock()
+ if self.stopped:
+ self.stopped = False
+ self.stop_mutex.unlock()
+ break
+ self.stop_mutex.unlock()
+ self.processing_time = self.clock.elapsed()
+ self.clock.start()
+
+ self.processing_mutex.lock()
+
+ self.update_frames(self.proc_buffer_manager.get().values())
+ self.make_luminance_balance().stitch_all_parts()
+ self.make_white_balance()
+ self.copy_car_image()
+ self.buffer.add(self.image.copy(), self.drop_if_full)
+ self.processing_mutex.unlock()
+
+ # update statistics
+ self.update_fps(self.processing_time)
+ self.stat_data.frames_processed_count += 1
+ # inform GUI of updated statistics
+ self.update_statistics_gui.emit(self.stat_data)
diff --git a/surround_view/capture_thread.py b/surround_view/capture_thread.py
new file mode 100644
index 0000000..3170f0d
--- /dev/null
+++ b/surround_view/capture_thread.py
@@ -0,0 +1,114 @@
+import cv2
+from PyQt5.QtCore import qDebug
+
+from .base_thread import BaseThread
+from .structures import ImageFrame
+from .utils import gstreamer_pipeline
+
+
+class CaptureThread(BaseThread):
+
+ def __init__(self,
+ device_id,
+ flip_method=2,
+ drop_if_full=True,
+ api_preference=cv2.CAP_GSTREAMER,
+ resolution=None,
+ use_gst=True,
+ parent=None):
+ """
+ device_id: device number of the camera.
+ flip_method: 0 for identity, 2 for 180 degree rotation (if the camera is installed
+ up-side-down).
+ drop_if_full: drop the frame if buffer is full.
+ api_preference: cv2.CAP_GSTREAMER for csi cameras, usually cv2.CAP_ANY would suffice.
+ resolution: camera resolution (width, height).
+ """
+ super(CaptureThread, self).__init__(parent)
+ self.device_id = device_id
+ self.flip_method = flip_method
+ self.use_gst = use_gst
+ self.drop_if_full = drop_if_full
+ self.api_preference = api_preference
+ self.resolution = resolution
+ self.cap = cv2.VideoCapture()
+ # an instance of the MultiBufferManager object,
+ # for synchronizing this thread with other cameras.
+ self.buffer_manager = None
+
+ def run(self):
+ if self.buffer_manager is None:
+ raise ValueError("This thread has not been binded to any buffer manager yet")
+
+ while True:
+ self.stop_mutex.lock()
+ if self.stopped:
+ self.stopped = False
+ self.stop_mutex.unlock()
+ break
+ self.stop_mutex.unlock()
+
+ # save capture time
+ self.processing_time = self.clock.elapsed()
+ # start timer (used to calculate capture rate)
+ self.clock.start()
+
+ # synchronize with other streams (if enabled for this stream)
+ self.buffer_manager.sync(self.device_id)
+
+ if not self.cap.grab():
+ continue
+
+ # retrieve frame and add it to buffer
+ _, frame = self.cap.retrieve()
+ img_frame = ImageFrame(self.clock.msecsSinceStartOfDay(), frame)
+ self.buffer_manager.get_device(self.device_id).add(img_frame, self.drop_if_full)
+
+ # update statistics
+ self.update_fps(self.processing_time)
+ self.stat_data.frames_processed_count += 1
+ # inform GUI of updated statistics
+ self.update_statistics_gui.emit(self.stat_data)
+
+ qDebug("Stopping capture thread...")
+
+# 在surround-view-system-introduction/surround_view/capture_thread.py中修改connect_camera方法
+ def connect_camera(self):
+ # 如果设备ID是字符串且以rtsp开头，则视为RTSP地址
+ if isinstance(self.device_id, str) and self.device_id.startswith('rtsp://'):
+ # 使用FFmpeg的H265解码方式打开RTSP流
+ # 降低延迟参数：-fflags nobuffer -flags low_delay -rtsp_transport tcp
+ ffmpeg_cmd = (
+ f"rtspsrc location={self.device_id} latency=0 ! "
+ "rtph265depay ! h265parse ! avdec_h265 ! "
+ "videoconvert ! video/x-raw,format=BGR ! appsink"
+ )
+ self.cap.open(ffmpeg_cmd, cv2.CAP_GSTREAMER)
+ elif self.use_gst:
+ options = gstreamer_pipeline(cam_id=self.device_id, flip_method=self.flip_method)
+ self.cap.open(options, self.api_preference)
+ else:
+ self.cap.open(self.device_id)
+
+ if not self.cap.isOpened():
+ qDebug(f"Cannot open camera {self.device_id}")
+ return False
+
+ # 设置分辨率（RTSP流通常固定分辨率，可根据实际情况调整）
+ if self.resolution is not None:
+ width, height = self.resolution
+ self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
+ self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
+
+ return True
+ def disconnect_camera(self):
+ # disconnect camera if it's already opened.
+ if self.cap.isOpened():
+ self.cap.release()
+ return True
+ # else do nothing and return
+ else:
+ return False
+
+ def is_camera_connected(self):
+ return self.cap.isOpened()
diff --git a/surround_view/fisheye_camera.py b/surround_view/fisheye_camera.py
new file mode 100644
index 0000000..b8cb0f2
--- /dev/null
+++ b/surround_view/fisheye_camera.py
@@ -0,0 +1,105 @@
+import os
+import numpy as np
+import cv2
+
+from . import param_settings as settings
+
+
+class FisheyeCameraModel(object):
+
+ """
+ Fisheye camera model, for undistorting, projecting and flipping camera frames.
+ """
+
+ def __init__(self, camera_param_file, camera_name):
+ if not os.path.isfile(camera_param_file):
+ raise ValueError("Cannot find camera param file")
+
+ if camera_name not in settings.camera_names:
+ raise ValueError("Unknown camera name: {}".format(camera_name))
+
+ self.camera_file = camera_param_file
+ self.camera_name = camera_name
+ self.scale_xy = (1.0, 1.0)
+ self.shift_xy = (0, 0)
+ self.undistort_maps = None
+ self.project_matrix = None
+ self.project_shape = settings.project_shapes[self.camera_name]
+ self.load_camera_params()
+
+ def load_camera_params(self):
+ fs = cv2.FileStorage(self.camera_file, cv2.FILE_STORAGE_READ)
+ self.camera_matrix = fs.getNode("camera_matrix").mat()
+ self.dist_coeffs = fs.getNode("dist_coeffs").mat()
+ self.resolution = fs.getNode("resolution").mat().flatten()
+
+ scale_xy = fs.getNode("scale_xy").mat()
+ if scale_xy is not None:
+ self.scale_xy = scale_xy
+
+ shift_xy = fs.getNode("shift_xy").mat()
+ if shift_xy is not None:
+ self.shift_xy = shift_xy
+
+ project_matrix = fs.getNode("project_matrix").mat()
+ if project_matrix is not None:
+ self.project_matrix = project_matrix
+
+ fs.release()
+ self.update_undistort_maps()
+
+ def update_undistort_maps(self):
+ new_matrix = self.camera_matrix.copy()
+ new_matrix[0, 0] *= self.scale_xy[0]
+ new_matrix[1, 1] *= self.scale_xy[1]
+ new_matrix[0, 2] += self.shift_xy[0]
+ new_matrix[1, 2] += self.shift_xy[1]
+ width, height = self.resolution
+
+ self.undistort_maps = cv2.fisheye.initUndistortRectifyMap(
+ self.camera_matrix,
+ self.dist_coeffs,
+ np.eye(3),
+ new_matrix,
+ (width, height),
+ cv2.CV_16SC2
+ )
+ return self
+
+ def set_scale_and_shift(self, scale_xy=(1.0, 1.0), shift_xy=(0, 0)):
+ self.scale_xy = scale_xy
+ self.shift_xy = shift_xy
+ self.update_undistort_maps()
+ return self
+
+ def undistort(self, image):
+ result = cv2.remap(image, *self.undistort_maps, interpolation=cv2.INTER_LINEAR,
+ borderMode=cv2.BORDER_CONSTANT)
+ return result
+
+ def project(self, image):
+ result = cv2.warpPerspective(image, self.project_matrix, self.project_shape)
+ return result
+
+ def flip(self, image):
+ if self.camera_name == "front":
+ return image.copy()
+
+ elif self.camera_name == "back":
+ return image.copy()[::-1, ::-1, :]
+
+ elif self.camera_name == "left":
+ return cv2.transpose(image)[::-1]
+
+ else:
+ return np.flip(cv2.transpose(image), 1)
+
+ def save_data(self):
+ fs = cv2.FileStorage(self.camera_file, cv2.FILE_STORAGE_WRITE)
+ fs.write("camera_matrix", self.camera_matrix)
+ fs.write("dist_coeffs", self.dist_coeffs)
+ fs.write("resolution", self.resolution)
+ fs.write("project_matrix", self.project_matrix)
+ fs.write("scale_xy", np.float32(self.scale_xy))
+ fs.write("shift_xy", np.float32(self.shift_xy))
+ fs.release()
diff --git a/surround_view/imagebuffer.py b/surround_view/imagebuffer.py
new file mode 100644
index 0000000..2afadb4
--- /dev/null
+++ b/surround_view/imagebuffer.py
@@ -0,0 +1,161 @@
+from PyQt5.QtCore import QSemaphore, QMutex
+from PyQt5.QtCore import QMutexLocker, QWaitCondition
+from queue import Queue
+
+
+class Buffer(object):
+
+ def __init__(self, buffer_size=5):
+ self.buffer_size = buffer_size
+ self.free_slots = QSemaphore(self.buffer_size)
+ self.used_slots = QSemaphore(0)
+ self.clear_buffer_add = QSemaphore(1)
+ self.clear_buffer_get = QSemaphore(1)
+ self.queue_mutex = QMutex()
+ self.queue = Queue(self.buffer_size)
+
+ def add(self, data, drop_if_full=False):
+ self.clear_buffer_add.acquire()
+ if drop_if_full:
+ if self.free_slots.tryAcquire():
+ self.queue_mutex.lock()
+ self.queue.put(data)
+ self.queue_mutex.unlock()
+ self.used_slots.release()
+ else:
+ self.free_slots.acquire()
+ self.queue_mutex.lock()
+ self.queue.put(data)
+ self.queue_mutex.unlock()
+ self.used_slots.release()
+
+ self.clear_buffer_add.release()
+
+ def get(self):
+ # acquire semaphores
+ self.clear_buffer_get.acquire()
+ self.used_slots.acquire()
+ self.queue_mutex.lock()
+ data = self.queue.get()
+ self.queue_mutex.unlock()
+ # release semaphores
+ self.free_slots.release()
+ self.clear_buffer_get.release()
+ # return item to caller
+ return data
+
+ def clear(self):
+ # check if buffer contains items
+ if self.queue.qsize() > 0:
+ # stop adding items to buffer (will return false if an item is currently being added to the buffer)
+ if self.clear_buffer_add.tryAcquire():
+ # stop taking items from buffer (will return false if an item is currently being taken from the buffer)
+ if self.clear_buffer_get.tryAcquire():
+ # release all remaining slots in queue
+ self.free_slots.release(self.queue.qsize())
+ # acquire all queue slots
+ self.free_slots.acquire(self.buffer_size)
+ # reset used_slots to zero
+ self.used_slots.acquire(self.queue.qsize())
+ # clear buffer
+ for _ in range(self.queue.qsize()):
+ self.queue.get()
+ # release all slots
+ self.free_slots.release(self.buffer_size)
+ # allow get method to resume
+ self.clear_buffer_get.release()
+ else:
+ return False
+ # allow add method to resume
+ self.clear_buffer_add.release()
+ return True
+ else:
+ return False
+ else:
+ return False
+
+ def size(self):
+ return self.queue.qsize()
+
+ def maxsize(self):
+ return self.buffer_size
+
+ def isfull(self):
+ return self.queue.qsize() == self.buffer_size
+
+ def isempty(self):
+ return self.queue.qsize() == 0
+
+
+class MultiBufferManager(object):
+
+ """
+ Class for synchronizing capture threads from different cameras.
+ """
+
+ def __init__(self, do_sync=True):
+ self.sync_devices = set()
+ self.do_sync = do_sync
+ self.wc = QWaitCondition()
+ self.mutex = QMutex()
+ self.arrived = 0
+ self.buffer_maps = dict()
+
+ def bind_thread(self, thread, buffer_size, sync=True):
+ self.create_buffer_for_device(thread.device_id, buffer_size, sync)
+ thread.buffer_manager = self
+
+ def create_buffer_for_device(self, device_id, buffer_size, sync=True):
+ if sync:
+ with QMutexLocker(self.mutex):
+ self.sync_devices.add(device_id)
+
+ self.buffer_maps[device_id] = Buffer(buffer_size)
+
+ def get_device(self, device_id):
+ return self.buffer_maps[device_id]
+
+ def remove_device(self, device_id):
+ self.buffer_maps.pop(device_id)
+ with QMutexLocker(self.mutex):
+ if device_id in self.sync_devices:
+ self.sync_devices.remove(device_id)
+ self.wc.wakeAll()
+
+ def sync(self, device_id):
+ # only perform sync if enabled for specified device/stream
+ self.mutex.lock()
+ if device_id in self.sync_devices:
+ # increment arrived count
+ self.arrived += 1
+ # we are the last to arrive: wake all waiting threads
+ if self.do_sync and self.arrived == len(self.sync_devices):
+ self.wc.wakeAll()
+ # still waiting for other streams to arrive: wait
+ else:
+ self.wc.wait(self.mutex)
+ # decrement arrived count
+ self.arrived -= 1
+ self.mutex.unlock()
+
+ def wake_all(self):
+ with QMutexLocker(self.mutex):
+ self.wc.wakeAll()
+
+ def set_sync(self, enable):
+ self.do_sync = enable
+
+ def sync_enabled(self):
+ return self.do_sync
+
+ def sync_enabled_for_device(self, device_id):
+ return device_id in self.sync_devices
+
+ def __contains__(self, device_id):
+ return device_id in self.buffer_maps
+
+ def __str__(self):
+ return (self.__class__.__name__ + ":\n" + \
+ "sync: {}\n".format(self.do_sync) + \
+ "devices: {}\n".format(tuple(self.buffer_maps.keys())) + \
+ "sync enabled devices: {}".format(self.sync_devices))
diff --git a/surround_view/param_settings copy.py b/surround_view/param_settings copy.py
new file mode 100644
index 0000000..e6eb63b
--- /dev/null
+++ b/surround_view/param_settings copy.py
@@ -0,0 +1,66 @@
+import os
+import cv2
+
+
+camera_names = ["front", "back", "left", "right"]
+
+# --------------------------------------------------------------------
+# (shift_width, shift_height): 鸟瞰图超出标定布的横向和纵向范围
+# 适配桌面环境，确保总范围不超过桌面尺寸
+shift_w = 50 # 左右方向各扩展10cm
+shift_h = 50 # 前后方向各扩展10cm
+
+# 标定布与车身之间的间隙（横向和纵向）
+inn_shift_w = 5 # 左右方向间隙
+inn_shift_h = 8 # 前后方向间隙
+
+# 标定布主体尺寸（宽×高）
+calib_width = 60 # 标定布宽度60cm
+calib_height = 100 # 标定布高度100cm
+
+# 拼接后图像的总尺寸
+total_w = calib_width + 2 * shift_w # 60 + 2×10 = 80cm（宽）
+total_h = calib_height + 2 * shift_h # 100 + 2×10 = 120cm（高）
+
+# 车身占据的矩形区域四角坐标
+# 确保与小车实际尺寸匹配：宽12cm，长20cm
+xl = shift_w + (calib_width - 12) // 2 + inn_shift_w # 10 + 24 + 5 = 39
+xr = xl + 12 # 右侧坐标，确保宽度12cm
+yt = shift_h + (calib_height - 20) // 2 + inn_shift_h # 10 + 40 + 8 = 58
+yb = yt + 20 # 底部坐标，确保长度20cm
+# --------------------------------------------------------------------
+
+project_shapes = {
+ "front": (total_w, yt), # 前相机投影区域：宽80cm × 高58cm
+ "back": (total_w, total_h - yb), # 后相机投影区域：宽80cm × 高42cm
+ "left": (total_h, xl), # 左相机投影区域：高120cm × 宽39cm
+ "right": (total_h, total_w - xr) # 右相机投影区域：高120cm × 宽29cm
+}
+
+# 四个标记点的像素位置，运行get_projection_map.py时需按相同顺序点击
+project_keypoints = {
+ "front": [(shift_w + 12, shift_h), # 前标定板左上
+ (shift_w + 48, shift_h), # 前标定板右上
+ (shift_w + 12, shift_h + 16), # 前标定板左下
+ (shift_w + 48, shift_h + 16)], # 前标定板右下
+
+ "back": [(shift_w + 12, shift_h), # 后标定板左上
+ (shift_w + 48, shift_h), # 后标定板右上
+ (shift_w + 12, shift_h + 16), # 后标定板左下
+ (shift_w + 48, shift_h + 16)], # 后标定板右下
+
+ "left": [(shift_h + 28, shift_w), # 左标定板左上
+ (shift_h + 64, shift_w), # 左标定板右上
+ (shift_h + 28, shift_w + 12), # 左标定板左下
+ (shift_h + 68, shift_w + 12)], # 左标定板右下
+
+ "right": [(shift_h + 28, shift_w), # 右标定板左上
+ (shift_h + 64, shift_w), # 右标定板右上
+ (shift_h + 28, shift_w + 12), # 右标定板左下
+ (shift_h + 68, shift_w + 12)] # 右标定板右下
+}
+
+# 加载并调整车身图标大小以匹配车身区域
+car_image = cv2.imread(os.path.join(os.getcwd(), "images", "car.png"))
+car_image = cv2.resize(car_image, (xr - xl, yb - yt)) # 调整为12×20cm的车身大小
+
\ No newline at end of file
diff --git a/surround_view/param_settings.py b/surround_view/param_settings.py
new file mode 100644
index 0000000..9fe9cac
--- /dev/null
+++ b/surround_view/param_settings.py
@@ -0,0 +1,63 @@
+import os
+import cv2
+
+
+camera_names = ["front", "back", "left", "right"]
+
+# --------------------------------------------------------------------
+# (shift_width, shift_height): how far away the birdview looks outside
+# of the calibration pattern in horizontal and vertical directions
+shift_w = 300
+shift_h = 300
+
+# size of the gap between the calibration pattern and the car
+# in horizontal and vertical directions
+inn_shift_w = 20
+inn_shift_h = 50
+
+# total width/height of the stitched image
+total_w = 800 + 2 * shift_w
+total_h = 960 + 2 * shift_h
+
+# four corners of the rectangular region occupied by the car
+# top-left (x_left, y_top), bottom-right (x_right, y_bottom)
+xl = shift_w + 180 + inn_shift_w
+xr = total_w - xl
+yt = shift_h + 200 + inn_shift_h
+yb = total_h - yt
+# --------------------------------------------------------------------
+
+project_shapes = {
+ "front": (total_w, yt),
+ "back": (total_w, yt),
+ "left": (total_h, xl),
+ "right": (total_h, xl)
+}
+
+# pixel locations of the four points to be chosen.
+# you must click these pixels in the same order when running
+# the get_projection_map.py script
+project_keypoints = {
+ "front": [(shift_w + 200, shift_h),
+ (shift_w + 2800, shift_h),
+ (shift_w + 200, shift_h + 800),
+ (shift_w + 2800, shift_h + 800)],
+
+ "back": [(shift_w + 80, shift_h),
+ (shift_w + 320, shift_h),
+ (shift_w + 80, shift_h + 200),
+ (shift_w + 320, shift_h + 200)],
+
+ "left": [(shift_w + 80, shift_h),
+ (shift_w + 320, shift_h),
+ (shift_w + 80, shift_h + 200),
+ (shift_w + 320, shift_h + 200)],
+
+ "right": [(shift_h + 240, shift_w),
+ (shift_h + 560, shift_w),
+ (shift_h + 240, shift_w + 120),
+ (shift_h + 560, shift_w + 120)],
+}
+
+car_image = cv2.imread(os.path.join(os.getcwd(), "images", "car.png"))
+car_image = cv2.resize(car_image, (xr - xl, yb - yt))
diff --git a/surround_view/process_thread.py b/surround_view/process_thread.py
new file mode 100644
index 0000000..17a0295
--- /dev/null
+++ b/surround_view/process_thread.py
@@ -0,0 +1,62 @@
+import cv2
+from PyQt5.QtCore import qDebug, QMutex
+
+from .base_thread import BaseThread
+
+
+class CameraProcessingThread(BaseThread):
+
+ """
+ Thread for processing individual camera images, i.e. undistort, project and flip.
+ """
+
+ def __init__(self,
+ capture_buffer_manager,
+ device_id,
+ camera_model,
+ drop_if_full=True,
+ parent=None):
+ """
+ capture_buffer_manager: an instance of the `MultiBufferManager` object.
+ device_id: device number of the camera to be processed.
+ camera_model: an instance of the `FisheyeCameraModel` object.
+ drop_if_full: drop if the buffer is full.
+ """
+ super(CameraProcessingThread, self).__init__(parent)
+ self.capture_buffer_manager = capture_buffer_manager
+ self.device_id = device_id
+ self.camera_model = camera_model
+ self.drop_if_full = drop_if_full
+ # an instance of the `ProjectedImageBuffer` object
+ self.proc_buffer_manager = None
+
+ def run(self):
+ if self.proc_buffer_manager is None:
+ raise ValueError("This thread has not been binded to any processing thread yet")
+
+ while True:
+ self.stop_mutex.lock()
+ if self.stopped:
+ self.stopped = False
+ self.stop_mutex.unlock()
+ break
+ self.stop_mutex.unlock()
+
+ self.processing_time = self.clock.elapsed()
+ self.clock.start()
+
+ self.processing_mutex.lock()
+ raw_frame = self.capture_buffer_manager.get_device(self.device_id).get()
+ und_frame = self.camera_model.undistort(raw_frame.image)
+ pro_frame = self.camera_model.project(und_frame)
+ flip_frame = self.camera_model.flip(pro_frame)
+ self.processing_mutex.unlock()
+
+ self.proc_buffer_manager.sync(self.device_id)
+ self.proc_buffer_manager.set_frame_for_device(self.device_id, flip_frame)
+
+ # update statistics
+ self.update_fps(self.processing_time)
+ self.stat_data.frames_processed_count += 1
+ # inform GUI of updated statistics
+ self.update_statistics_gui.emit(self.stat_data)
diff --git a/surround_view/simple_gui.py b/surround_view/simple_gui.py
new file mode 100644
index 0000000..e6e6a64
--- /dev/null
+++ b/surround_view/simple_gui.py
@@ -0,0 +1,158 @@
+import cv2
+import numpy as np
+
+# return -1 if user press 'q'. return 1 if user press 'Enter'.
+def display_image(window_title, image):
+ # 创建可调整大小的窗口
+ cv2.namedWindow(window_title, cv2.WINDOW_NORMAL)
+ # 显示图像
+ cv2.imshow(window_title, image)
+
+ while True:
+ # 检查窗口是否被关闭
+ if cv2.getWindowProperty(window_title, cv2.WND_PROP_VISIBLE) < 1:
+ return -1
+
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord("q"):
+ return -1
+ # 'Enter' key is detected!
+ if key == 13:
+ return 1
+
+
+class PointSelector(object):
+ """
+ ---------------------------------------------------
+ | A simple gui point selector. |
+ | Usage: |
+ | |
+ | 1. call the `loop` method to show the image. |
+ | 2. click on the image to select key points, |
+ | press `d` to delete the last points. |
+ | 3. press `q` to quit, press `Enter` to confirm. |
+ ---------------------------------------------------
+ """
+
+ POINT_COLOR = (0, 0, 255)
+ FILL_COLOR = (0, 255, 255)
+
+ def __init__(self, image, title="PointSelector"):
+ self.original_image = image.copy() # 保存原始图像
+ self.image = image
+ self.title = title
+ self.keypoints = []
+ self.window_width = image.shape[1]
+ self.window_height = image.shape[0]
+ self.scale = 1.0 # 缩放比例
+
+ def draw_image(self):
+ """
+ Display the selected keypoints and draw the convex hull.
+ """
+ # 基于当前缩放比例调整点坐标
+ scaled_keypoints = [
+ (int(x * self.scale), int(y * self.scale))
+ for x, y in self.keypoints
+ ]
+
+ # 创建缩放后的图像副本
+ scaled_image = cv2.resize(self.original_image,
+ (self.window_width, self.window_height))
+
+ # 绘制选中的关键点
+ for i, pt in enumerate(scaled_keypoints):
+ cv2.circle(scaled_image, pt, 6, self.POINT_COLOR, -1)
+ cv2.putText(scaled_image, str(i), (pt[0], pt[1] - 15),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.6, self.POINT_COLOR, 2)
+
+ # 如果有两个点，绘制连接线
+ if len(scaled_keypoints) == 2:
+ p1, p2 = scaled_keypoints
+ cv2.line(scaled_image, p1, p2, self.POINT_COLOR, 2)
+
+ # 如果有两个以上的点，绘制凸包
+ if len(scaled_keypoints) > 2:
+ mask = self.create_mask_from_pixels(scaled_keypoints,
+ (self.window_height, self.window_width))
+ scaled_image = self.draw_mask_on_image(scaled_image, mask)
+
+ cv2.imshow(self.title, scaled_image)
+
+ def onclick(self, event, x, y, flags, param):
+ """
+ 点击点(x, y)会将该点添加到列表并重新绘制图像。
+ 考虑窗口缩放，将点击坐标转换回原始图像坐标。
+ """
+ if event == cv2.EVENT_LBUTTONDOWN:
+ # 将点击坐标转换回原始图像坐标
+ orig_x = int(x / self.scale)
+ orig_y = int(y / self.scale)
+ print(f"click ({orig_x}, {orig_y}) (scaled: ({x}, {y}))")
+ self.keypoints.append((orig_x, orig_y))
+ self.draw_image()
+ # 窗口大小改变事件
+ elif event == cv2.EVENT_RESIZE:
+ self.window_width = x
+ self.window_height = y
+ # 计算新的缩放比例
+ self.scale = min(x / self.original_image.shape[1],
+ y / self.original_image.shape[0])
+ self.draw_image()
+
+ def loop(self):
+ """
+ Press "q" will exit the gui and return False
+ press "d" will delete the last selected point.
+ Press "Enter" will exit the gui and return True.
+ """
+ # 创建可调整大小的窗口
+ cv2.namedWindow(self.title, cv2.WINDOW_NORMAL)
+ # 设置窗口初始大小为图像大小
+ cv2.resizeWindow(self.title, self.image.shape[1], self.image.shape[0])
+ cv2.setMouseCallback(self.title, self.onclick, param=())
+ self.draw_image()
+
+ while True:
+ # 检查窗口是否被关闭
+ if cv2.getWindowProperty(self.title, cv2.WND_PROP_VISIBLE) < 1:
+ return False
+
+ key = cv2.waitKey(1) & 0xFF
+
+ # 按q键返回False
+ if key == ord("q"):
+ return False
+
+ # 按d键删除最后一个点
+ if key == ord("d"):
+ if len(self.keypoints) > 0:
+ x, y = self.keypoints.pop()
+ print(f"Delete ({x}, {y})")
+ self.draw_image()
+
+ # 按Enter键确认
+ if key == 13:
+ return True
+
+ def create_mask_from_pixels(self, pixels, image_shape):
+ """
+ Create mask from the convex hull of a list of pixels.
+ """
+ pixels = np.int32(pixels).reshape(-1, 2)
+ hull = cv2.convexHull(pixels)
+ mask = np.zeros(image_shape[:2], np.int8)
+ cv2.fillConvexPoly(mask, hull, 1, lineType=8, shift=0)
+ mask = mask.astype(bool)
+ return mask
+
+ def draw_mask_on_image(self, image, mask):
+ """
+ Paint the region defined by a given mask on an image.
+ """
+ new_image = np.zeros_like(image)
+ new_image[:, :] = self.FILL_COLOR
+ mask = np.array(mask, dtype=np.uint8)
+ new_mask = cv2.bitwise_and(new_image, new_image, mask=mask)
+ cv2.addWeighted(image, 1.0, new_mask, 0.5, 0.0, image)
+ return image
diff --git a/surround_view/structures.py b/surround_view/structures.py
new file mode 100644
index 0000000..85d346c
--- /dev/null
+++ b/surround_view/structures.py
@@ -0,0 +1,12 @@
+class ImageFrame(object):
+
+ def __init__(self, timestamp, image):
+ self.timestamp = timestamp
+ self.image = image
+
+
+class ThreadStatisticsData(object):
+
+ def __init__(self):
+ self.average_fps = 0
+ self.frames_processed_count = 0
diff --git a/surround_view/utils.py b/surround_view/utils.py
new file mode 100644
index 0000000..c2a2248
--- /dev/null
+++ b/surround_view/utils.py
@@ -0,0 +1,148 @@
+import cv2
+import numpy as np
+
+
+def gstreamer_pipeline(cam_id=0,
+ capture_width=960,
+ capture_height=640,
+ framerate=60,
+ flip_method=2):
+ """
+ Use libgstreamer to open csi-cameras.
+ """
+ return ("nvarguscamerasrc sensor-id={} ! ".format(cam_id) + \
+ "video/x-raw(memory:NVMM), "
+ "width=(int)%d, height=(int)%d, "
+ "format=(string)NV12, framerate=(fraction)%d/1 ! "
+ "nvvidconv flip-method=%d ! "
+ "video/x-raw, format=(string)BGRx ! "
+ "videoconvert ! "
+ "video/x-raw, format=(string)BGR ! appsink"
+ % (capture_width,
+ capture_height,
+ framerate,
+ flip_method
+ )
+ )
+
+
+def convert_binary_to_bool(mask):
+ """
+ Convert a binary image (only one channel and pixels are 0 or 255) to
+ a bool one (all pixels are 0 or 1).
+ """
+ return (mask.astype(np.float64) / 255.0).astype(int)
+
+
+def adjust_luminance(gray, factor):
+ """
+ Adjust the luminance of a grayscale image by a factor.
+ """
+ return np.minimum((gray * factor), 255).astype(np.uint8)
+
+
+def get_mean_statistisc(gray, mask):
+ """
+ Get the total values of a gray image in a region defined by a mask matrix.
+ The mask matrix must have values either 0 or 1.
+ """
+ return np.sum(gray * mask)
+
+
+def mean_luminance_ratio(grayA, grayB, mask):
+ return get_mean_statistisc(grayA, mask) / get_mean_statistisc(grayB, mask)
+
+
+def get_mask(img):
+ """
+ Convert an image to a mask array.
+ """
+ gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ ret, mask = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY)
+ return mask
+
+
+def get_overlap_region_mask(imA, imB):
+ """
+ Given two images of the save size, get their overlapping region and
+ convert this region to a mask array.
+ """
+ overlap = cv2.bitwise_and(imA, imB)
+ mask = get_mask(overlap)
+ mask = cv2.dilate(mask, np.ones((2, 2), np.uint8), iterations=2)
+ return mask
+
+
+def get_outmost_polygon_boundary(img):
+ """
+ Given a mask image with the mask describes the overlapping region of
+ two images, get the outmost contour of this region.
+ """
+ mask = get_mask(img)
+ mask = cv2.dilate(mask, np.ones((2, 2), np.uint8), iterations=2)
+ cnts, hierarchy = cv2.findContours(
+ mask,
+ cv2.RETR_EXTERNAL,
+ cv2.CHAIN_APPROX_SIMPLE)[-2:]
+
+ # get the contour with largest aera
+ C = sorted(cnts, key=lambda x: cv2.contourArea(x), reverse=True)[0]
+
+ # polygon approximation
+ polygon = cv2.approxPolyDP(C, 0.009 * cv2.arcLength(C, True), True)
+
+ return polygon
+
+
+def get_weight_mask_matrix(imA, imB, dist_threshold=5):
+ """
+ Get the weight matrix G that combines two images imA, imB smoothly.
+ """
+ overlapMask = get_overlap_region_mask(imA, imB)
+ overlapMaskInv = cv2.bitwise_not(overlapMask)
+ indices = np.where(overlapMask == 255)
+
+ imA_diff = cv2.bitwise_and(imA, imA, mask=overlapMaskInv)
+ imB_diff = cv2.bitwise_and(imB, imB, mask=overlapMaskInv)
+
+ G = get_mask(imA).astype(np.float32) / 255.0
+
+ polyA = get_outmost_polygon_boundary(imA_diff)
+ polyB = get_outmost_polygon_boundary(imB_diff)
+
+ # 添加微小值防止除零
+ epsilon = 1e-8
+ for y, x in zip(*indices):
+ xy_tuple = tuple([int(x), int(y)])
+ distToB = cv2.pointPolygonTest(polyB, xy_tuple, True)
+
+ if distToB < dist_threshold:
+ distToA = cv2.pointPolygonTest(polyA, xy_tuple, True)
+
+ # 计算平方距离
+ distToB_sq = distToB **2
+ distToA_sq = distToA** 2
+
+ # 检查距离和是否为零（添加epsilon避免除零）
+ total = distToA_sq + distToB_sq + epsilon
+ G[y, x] = distToB_sq / total
+
+ return G, overlapMask
+
+
+def make_white_balance(image):
+ """
+ Adjust white balance of an image base on the means of its channels.
+ """
+ B, G, R = cv2.split(image)
+ m1 = np.mean(B)
+ m2 = np.mean(G)
+ m3 = np.mean(R)
+ K = (m1 + m2 + m3) / 3
+ c1 = K / m1
+ c2 = K / m2
+ c3 = K / m3
+ B = adjust_luminance(B, c1)
+ G = adjust_luminance(G, c2)
+ R = adjust_luminance(R, c3)
+ return cv2.merge((B, G, R))
diff --git a/test.py b/test.py
new file mode 100644
index 0000000..8d6e632
--- /dev/null
+++ b/test.py
@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+"""
+test.py
+摄像头畸变校正效果测试 - 仅显示校正前后对比画面
+"""
+
+import cv2
+import numpy as np
+import argparse
+import os
+
+def is_rtsp_url(input_str):
+ """检查输入是否为RTSP URL"""
+ return input_str.startswith('rtsp://')
+
+def load_calibration_params(yaml_file):
+ """从YAML文件加载标定参数"""
+ if not os.path.exists(yaml_file):
+ raise FileNotFoundError(f"标定文件 {yaml_file} 不存在")
+
+ fs = cv2.FileStorage(yaml_file, cv2.FILE_STORAGE_READ)
+ if not fs.isOpened():
+ raise IOError(f"无法打开标定文件 {yaml_file}")
+
+ # 读取参数
+ camera_matrix = fs.getNode("camera_matrix").mat()
+ dist_coeffs = fs.getNode("dist_coeffs").mat()
+
+ fs.release()
+
+ print(f"✓ 成功加载标定参数")
+ print(f" 相机矩阵:\n{camera_matrix}")
+ print(f" 畸变系数: {dist_coeffs.flatten()}")
+
+ return camera_matrix, dist_coeffs
+
+def main():
+ parser = argparse.ArgumentParser(description='摄像头畸变校正效果测试')
+ parser.add_argument("-i", "--input", type=str, required=True,
+ help="输入源（摄像头设备索引、视频文件路径或RTSP URL）")
+ parser.add_argument("-c", "--calibration", type=str, required=True,
+ help="标定参数YAML文件路径")
+ parser.add_argument("-f", "--fisheye", action="store_true",
+ help="是否为鱼眼相机")
+
+ args = parser.parse_args()
+
+ # 加载标定参数
+ try:
+ camera_matrix, dist_coeffs = load_calibration_params(args.calibration)
+ except Exception as e:
+ print(f"❌ 加载标定参数失败: {e}")
+ return
+
+ # 初始化视频源
+ if is_rtsp_url(args.input):
+ # RTSP流
+ cap = cv2.VideoCapture(args.input, cv2.CAP_FFMPEG)
+ try:
+ cap.set(cv2.CAP_PROP_FFMPEG_OPTION, "rtsp_transport", "tcp")
+ cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+ except:
+ pass
+ elif args.input.isdigit():
+ # 摄像头设备索引
+ cap = cv2.VideoCapture(int(args.input))
+ else:
+ # 视频文件
+ cap = cv2.VideoCapture(args.input)
+
+ if not cap.isOpened():
+ print(f"❌ 无法打开视频源: {args.input}")
+ return
+
+ # 获取实际分辨率
+ actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+ actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+ print(f"视频源分辨率: {actual_w}x{actual_h}")
+
+ print("✓ 准备开始校正")
+ print("按 'q' 键退出")
+ print("按 's' 键切换校正方法")
+
+ window_name = "畸变校正对比 - 原始(左) vs 校正后(右)"
+ cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
+
+ # 校正方法选择
+ use_remap = True # 默认使用remap方法
+
+ frame_count = 0
+ while True:
+ ret, frame = cap.read()
+ if not ret:
+ print("读取帧失败")
+ # 对于RTSP流，尝试重新连接
+ if is_rtsp_url(args.input):
+ cap.release()
+ cap = cv2.VideoCapture(args.input, cv2.CAP_FFMPEG)
+ continue
+ break
+
+ frame_count += 1
+
+ # 应用畸变校正
+ try:
+ if args.fisheye:
+ if use_remap:
+ # 使用映射方法
+ new_camera_matrix = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(
+ camera_matrix, dist_coeffs, (actual_w, actual_h), np.eye(3), balance=0.8)
+ map1, map2 = cv2.fisheye.initUndistortRectifyMap(
+ camera_matrix, dist_coeffs, np.eye(3), new_camera_matrix,
+ (actual_w, actual_h), cv2.CV_16SC2)
+ undistorted = cv2.remap(frame, map1, map2,
+ interpolation=cv2.INTER_LINEAR,
+ borderMode=cv2.BORDER_CONSTANT)
+ else:
+ # 使用直接校正方法
+ undistorted = cv2.fisheye.undistortImage(frame, camera_matrix, dist_coeffs)
+ else:
+ # 普通相机校正
+ undistorted = cv2.undistort(frame, camera_matrix, dist_coeffs)
+
+ except Exception as e:
+ print(f"校正失败: {e}")
+ undistorted = frame.copy() # 失败时使用原图
+
+ # 检查校正后的图像是否有效
+ if undistorted is None or undistorted.size == 0:
+ print(f"警告: 第 {frame_count} 帧校正失败")
+ undistorted = frame.copy()
+
+ # 调整大小以便显示
+ max_height = 600
+ scale_orig = max_height / frame.shape[0]
+ orig_resized = cv2.resize(frame,
+ (int(frame.shape[1] * scale_orig),
+ int(frame.shape[0] * scale_orig)))
+
+ scale_undist = max_height / undistorted.shape[0]
+ undistorted_resized = cv2.resize(undistorted,
+ (int(undistorted.shape[1] * scale_undist),
+ int(undistorted.shape[0] * scale_undist)))
+
+ # 确保两个图像高度相同
+ min_height = min(orig_resized.shape[0], undistorted_resized.shape[0])
+ orig_resized = orig_resized[:min_height, :]
+ undistorted_resized = undistorted_resized[:min_height, :]
+
+ # 水平拼接显示对比
+ comparison = np.hstack((orig_resized, undistorted_resized))
+
+ # 添加文本说明
+ cv2.putText(comparison, "原始图像 (有畸变)",
+ (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+ cv2.putText(comparison, "校正后图像",
+ (orig_resized.shape[1] + 10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+
+ # 显示相机类型和校正方法
+ camera_type = "鱼眼相机" if args.fisheye else "普通相机"
+ method_type = "映射方法" if use_remap else "直接方法"
+ cv2.putText(comparison, f"类型: {camera_type} | 方法: {method_type}",
+ (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
+ cv2.putText(comparison, "按 's' 切换方法 | 按 'q' 退出",
+ (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
+
+ cv2.imshow(window_name, comparison)
+
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord('q'):
+ break
+ elif key == ord('s'):
+ use_remap = not use_remap
+ method_name = "映射方法" if use_remap else "直接方法"
+ print(f"切换到 {method_name}")
+
+ cap.release()
+ cv2.destroyAllWindows()
+ print("测试完成")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/test_cameras.py b/test_cameras.py
new file mode 100644
index 0000000..18e7493
--- /dev/null
+++ b/test_cameras.py
@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+
+import cv2
+import time
+
+# get the installed camera list for initialization.
+def get_cam_lst(cam_lst=range(0, 24)):
+ arr = []
+ for iCam in cam_lst:
+ cap = cv2.VideoCapture(iCam)
+ if not cap.read()[0]:
+ continue
+ else:
+ arr.append(iCam)
+
+ cap.release()
+ return arr
+
+def show_cam_img(caps, cam_list):
+ print("INFO: Press 'q' to quit! Press 's' to save a picture, 'n' to change to next camera device!")
+ idx = 0
+ while True:
+ cap_device = caps[idx]
+ ret, frame = cap_device.read()
+ if ret:
+ cv2.imshow('video', frame)
+ else:
+ print("ERROR: failed read frame!")
+
+ # quit the test
+ c = cv2.waitKey(1)
+ if c == ord('q'):
+ break
+
+ # change to next camera device
+ if c == ord('n'):
+ idx += 1
+ if idx >= len(caps):
+ idx = 0
+ continue
+
+ # save the picture
+ if c == ord('s'):
+ if ret:
+ name = 'video{0}_{1}.png'.format(cam_list[idx],
+ time.strftime("%Y-%m-%d_%H:%M:%S", time.localtime()))
+ cv2.imwrite(name, frame)
+ print("saved file: %s!" %name)
+
+ cv2.destroyAllWindows()
+
+def init_caps(cam_list, resolution=(1280,720)):
+ caps = []
+ for iCam in cam_list:
+ cap = cv2.VideoCapture(iCam)
+ cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
+ cap.set(3, resolution[0])
+ cap.set(4, resolution[1])
+ caps.append(cap)
+
+ return caps
+
+def deinit_caps(cap_list):
+ for cap in cap_list:
+ cap.release()
+
+def show_cameras(video_list=None):
+ if video_list == None:
+ print("Start to search all available camera devices, please wait... ")
+ cam_list = get_cam_lst()
+ err_msg = "cannot find any video device!"
+ else:
+ cam_list = get_cam_lst(video_list)
+ err_msg = "cannot find available video device in list: {0}!".format(video_list) +\
+ "\nPlease check the video devices in /dev/v4l/by-path/ folder!"
+
+ if len(cam_list) < 1:
+ print("ERROR: " + err_msg)
+ return
+
+ print("Available video device list is {}".format(cam_list))
+ caps = init_caps(cam_list)
+ show_cam_img(caps, cam_list)
+ deinit_caps(caps)
+
+if __name__ == "__main__":
+ # User can specify the video list here.
+ show_cameras([2, 6, 10, 14])
+
+ # Or search all available video devices automatically.
+ # show_cameras()
diff --git a/videos/back.h264 b/videos/back.h264
new file mode 100644
index 0000000..9a993c7
Binary files /dev/null and b/videos/back.h264 differ
diff --git a/videos/front.h264 b/videos/front.h264
new file mode 100644
index 0000000..ad9f7b5
Binary files /dev/null and b/videos/front.h264 differ
diff --git a/videos/left.h264 b/videos/left.h264
new file mode 100644
index 0000000..c5e7c81
Binary files /dev/null and b/videos/left.h264 differ
diff --git a/videos/right.h264 b/videos/right.h264
new file mode 100644
index 0000000..713fbce
Binary files /dev/null and b/videos/right.h264 differ
diff --git a/weights.png b/weights.png
new file mode 100644
index 0000000..251cda7
Binary files /dev/null and b/weights.png differ
diff --git a/yaml/1/back.yaml b/yaml/1/back.yaml
new file mode 100644
index 0000000..b2d334f
--- /dev/null
+++ b/yaml/1/back.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 660.65198906031776, 0., 918.64495995045081, 0.,
+ 660.20878035295686, 452.79606951218005, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.24434175408363587, 0.24719464745338432,
+ -0.42793222699954014, 0.26776763692583722 ]
diff --git a/yaml/1/front.yaml b/yaml/1/front.yaml
new file mode 100644
index 0000000..b2d334f
--- /dev/null
+++ b/yaml/1/front.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 660.65198906031776, 0., 918.64495995045081, 0.,
+ 660.20878035295686, 452.79606951218005, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.24434175408363587, 0.24719464745338432,
+ -0.42793222699954014, 0.26776763692583722 ]
diff --git a/yaml/1/left.yaml b/yaml/1/left.yaml
new file mode 100644
index 0000000..b2d334f
--- /dev/null
+++ b/yaml/1/left.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 660.65198906031776, 0., 918.64495995045081, 0.,
+ 660.20878035295686, 452.79606951218005, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.24434175408363587, 0.24719464745338432,
+ -0.42793222699954014, 0.26776763692583722 ]
diff --git a/yaml/1/right.yaml b/yaml/1/right.yaml
new file mode 100644
index 0000000..b2d334f
--- /dev/null
+++ b/yaml/1/right.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 660.65198906031776, 0., 918.64495995045081, 0.,
+ 660.20878035295686, 452.79606951218005, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.24434175408363587, 0.24719464745338432,
+ -0.42793222699954014, 0.26776763692583722 ]
diff --git a/yaml/2/back.yaml b/yaml/2/back.yaml
new file mode 100644
index 0000000..983e887
--- /dev/null
+++ b/yaml/2/back.yaml
@@ -0,0 +1,36 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 651.87961525368564, 0., 920.48558826717237, 0.,
+ 652.82835006081007, 454.17340218030813, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.18013707979325458, -0.017645349592443155,
+ 0.041108541483470908, -0.017486755673543321 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -0.70651991509303735, -1.8025582621608418, 1295.7490058655571,
+ -0.01329797566893791, -2.5188223244302055, 1074.1131579051068,
+ -3.1661846830804434e-05, -0.003165537318318209, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 0.5, 0.5 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 200., -100. ]
diff --git a/yaml/2/front.yaml b/yaml/2/front.yaml
new file mode 100644
index 0000000..23489e1
--- /dev/null
+++ b/yaml/2/front.yaml
@@ -0,0 +1,36 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 651.87961525368564, 0., 920.48558826717237, 0.,
+ 652.82835006081007, 454.17340218030813, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.18013707979325458, -0.017645349592443155,
+ 0.041108541483470908, -0.017486755673543321 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -0.70195159738658375, -1.6098600396732468, 1033.2128385022481,
+ -0.0043878140297416244, -1.6915573577485288, 742.41080377410708,
+ -0.00029382004132222784, -0.0024012036841861021, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 0.5, 0.5 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ -200., -100. ]
diff --git a/yaml/2/left.yaml b/yaml/2/left.yaml
new file mode 100644
index 0000000..3ca072d
--- /dev/null
+++ b/yaml/2/left.yaml
@@ -0,0 +1,36 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 651.87961525368564, 0., 920.48558826717237, 0.,
+ 652.82835006081007, 454.17340218030813, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.18013707979325458, -0.017645349592443155,
+ 0.041108541483470908, -0.017486755673543321 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -0.69031151103290689, -1.9530656261659638, 1517.6349813143574,
+ -0.061343353534429643, -1.2906982851206221, 705.89604046372699,
+ -0.00017940779371332523, -0.0021525707093952733, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 0.5, 0.5 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 200., -100. ]
diff --git a/yaml/2/right.yaml b/yaml/2/right.yaml
new file mode 100644
index 0000000..d869a46
--- /dev/null
+++ b/yaml/2/right.yaml
@@ -0,0 +1,36 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 651.87961525368564, 0., 920.48558826717237, 0.,
+ 652.82835006081007, 454.17340218030813, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -0.18013707979325458, -0.017645349592443155,
+ 0.041108541483470908, -0.017486755673543321 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 1920, 1080 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -0.48242684206128689, -2.3345074199146203, 1322.288518428865,
+ 0.035002427874919924, -1.9237190166433751, 776.22487491439711,
+ 2.1961727429861459e-05, -0.0030802200454818456, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 0.5, 0.5 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 200., -100. ]
diff --git a/yaml/3/back.yaml b/yaml/3/back.yaml
new file mode 100644
index 0000000..4a35cbb
--- /dev/null
+++ b/yaml/3/back.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.1359191618490445e+02, 0., 1.2635952283523022e+03, 0.,
+ 9.3632218470618568e+02, 7.3904470142576406e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.5688204048403651e-01, 1.5654636330056526e-01,
+ -1.7971288175677119e-01, 8.7103625346845376e-02 ]
diff --git a/yaml/3/front.yaml b/yaml/3/front.yaml
new file mode 100644
index 0000000..8b9963f
--- /dev/null
+++ b/yaml/3/front.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.1297883483233306e+02, 0., 1.1530623178818100e+03, 0.,
+ 9.3430332152657456e+02, 7.0322316308984716e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.2262094222158474e-01, 9.1910107741035543e-02,
+ -1.1125663233319319e-01, 5.2563645197652560e-02 ]
diff --git a/yaml/3/left.yaml b/yaml/3/left.yaml
new file mode 100644
index 0000000..82866e8
--- /dev/null
+++ b/yaml/3/left.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.0580569362933545e+02, 0., 1.2526625521235414e+03, 0.,
+ 9.0650948172469225e+02, 6.5033182889464206e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.2164953330895090e-01, 4.4177658648078696e-02,
+ -1.9393499331826249e-02, 6.6005226176948407e-03 ]
diff --git a/yaml/3/right.yaml b/yaml/3/right.yaml
new file mode 100644
index 0000000..be4bb4d
--- /dev/null
+++ b/yaml/3/right.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.0277823073247305e+02, 0., 1.2536850522335756e+03, 0.,
+ 8.9630980881437813e+02, 6.5471986586202775e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.1511756060462653e-01, 2.0035463501247644e-02,
+ 9.5960604184571630e-03, -4.4510596773296362e-03 ]
diff --git a/yaml/back.yaml b/yaml/back.yaml
new file mode 100644
index 0000000..b47a239
--- /dev/null
+++ b/yaml/back.yaml
@@ -0,0 +1,37 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.1359191618490445e+02, 0., 1.2635952283523022e+03, 0.,
+ 9.3632218470618568e+02, 7.3904470142576406e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.5688204048403651e-01, 1.5654636330056526e-01,
+ -1.7971288175677119e-01, 8.7103625346845376e-02 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -2.9179778642686394e-01, -7.4241907296046106e-01,
+ 7.9539195878902456e+02, -1.6965639676084780e-02,
+ -1.1036320216549194e+00, 8.9754354771952148e+02,
+ -2.1693275673820841e-05, -1.5407635352890023e-03, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 6.99999988e-01, 8.00000012e-01 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ -150., -100. ]
diff --git a/yaml/front.yaml b/yaml/front.yaml
new file mode 100644
index 0000000..5d8b8a4
--- /dev/null
+++ b/yaml/front.yaml
@@ -0,0 +1,37 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.1297883483233306e+02, 0., 1.1530623178818100e+03, 0.,
+ 9.3430332152657456e+02, 7.0322316308984716e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.2262094222158474e-01, 9.1910107741035543e-02,
+ -1.1125663233319319e-01, 5.2563645197652560e-02 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -2.8283572994274103e-01, -1.2971399339352716e+00,
+ 1.0824292174928530e+03, 2.5824309209632131e-02,
+ -1.3252387496909435e+00, 1.0023621246915676e+03,
+ 3.7653658154558861e-05, -1.6884376226546168e-03, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 6.99999988e-01, 8.00000012e-01 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ -150., -100. ]
diff --git a/yaml/left.yaml b/yaml/left.yaml
new file mode 100644
index 0000000..978056a
--- /dev/null
+++ b/yaml/left.yaml
@@ -0,0 +1,37 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.0580569362933545e+02, 0., 1.2526625521235414e+03, 0.,
+ 9.0650948172469225e+02, 6.5033182889464206e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.2164953330895090e-01, 4.4177658648078696e-02,
+ -1.9393499331826249e-02, 6.6005226176948407e-03 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -5.0581752299581340e-01, -1.3713554452799930e+00,
+ 1.3568685515624798e+03, 1.3945277219985447e-02,
+ -9.3798987988477867e-01, 6.5516461320394853e+02,
+ -4.0658856622034665e-07, -1.7343355065690702e-03, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 6.99999988e-01, 8.00000012e-01 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ -150., -100. ]
diff --git a/yaml/right.yaml b/yaml/right.yaml
new file mode 100644
index 0000000..032f62f
--- /dev/null
+++ b/yaml/right.yaml
@@ -0,0 +1,37 @@
+%YAML:1.0
+---
+camera_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ 9.0277823073247305e+02, 0., 1.2536850522335756e+03, 0.,
+ 8.9630980881437813e+02, 6.5471986586202775e+02, 0., 0., 1. ]
+dist_coeffs: !!opencv-matrix
+ rows: 4
+ cols: 1
+ dt: d
+ data: [ -2.1511756060462653e-01, 2.0035463501247644e-02,
+ 9.5960604184571630e-03, -4.4510596773296362e-03 ]
+resolution: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: i
+ data: [ 2560, 1440 ]
+project_matrix: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [ -4.4842873174953890e-01, -1.2747192546497945e+00,
+ 1.1617447668040295e+03, 6.6242519223062351e-03,
+ -1.0531016668903570e+00, 6.7139498427549870e+02,
+ 1.7957623585927813e-05, -1.9558863991643110e-03, 1. ]
+scale_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ 6.99999988e-01, 8.00000012e-01 ]
+shift_xy: !!opencv-matrix
+ rows: 2
+ cols: 1
+ dt: f
+ data: [ -150., -100. ]