Merge pull request 'jhy：vision代码笔记更新' (#15) from jhy/RobotKernal-UESTC:main into main

Reviewed-on: http://logzhan.ticp.io:30000/logzhan/RobotKernal-UESTC/pulls/15

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/Note/upbot_vision阅读笔记.md

@@ -0,0 +1,628 @@
+### 一.目录结构
+
+include：包含头文件
+
+model：存放用于目标检测的权重文件
+
+src：源文件
+
+### 二.主要源文件
+
+#### 2.1 前提知识
+
+**1.相机成像**
+
+**四个坐标系：**
+
+图像处理、立体视觉等等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系。
+
+- 世界坐标系：代表物体在三维世界里的真实坐标，坐标用(Xw, Yw, Zw)表示，其中的w可以认为是world的缩写
+
+- 相机坐标系：代表以相机光学中心为原点的坐标，Z轴与光轴重合，坐标用(Xc, Yc, Zc)表示，其中的c可以认为是camera的缩写
+
+- 图像坐标系：代表相机拍摄时，在成像平面上使用的坐标系，成像平面和相机光轴的交点为原点，坐标用(X, Y)表示
+
+- 像素坐标系：在相机成像平面上的图像，通常情况下我们不能直接使用，我们定义了一套新的坐标系来表示在电子设备上被显示的图像，图像的左上角是原点，坐标系用(u, v)表示
+
+**<font color = "red">坐标系之间的转换：</font>**
+
+- 世界坐标系转相机坐标系
+
+  ![image-20240528142049084](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528142049084.png)
+
+  这里的transform函数代表具体的转换过程，很好理解，从世界坐标系转成相机坐标系时，需要进行两种操作：旋转和平移：![image-20240528142259790](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528142259790.png)
+
+  还可以继续用齐次坐标来简化，即把三维坐标增加一个维度，变成四维，第四维的值设为1：
+
+  ![image-20240528142325700](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\世界转相机.png)
+  
+  上面的第二个矩阵就是我们常说的**外参矩阵**。
+
+- 相机坐标系转图像坐标系
+
+  有了相机坐标系的坐标，再根据相机成像的原理，即可推断出对应的成像平面的图像坐标：
+
+  ![                                                                ](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\成像原理.png)
+
+  根据相似三角形的性质，很容易得出下面关系：
+
+  ![image-20240528142626714](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528142626714.png)
+
+  写成矩阵乘的形式如下：![image-20240528142644836](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\相机转图像.png)
+
+- 图像坐标转像素坐标
+  
+  像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述个像素点都是几行几列。所以这二者之间的转换如下:其中dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm。
+  
+  ![image-20240528143447600](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\图像转像素.png)
+  
+  ![image-20240528143532858](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528143532858.png)
+  
+  上式中就包含了我们常说的**相机的内参矩阵（intrinsics）**，单独提出来如下所示：
+  
+  ![image-20240528143035233](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528143035233.png)
+
+![image-20240528172144383](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\四大坐标系.png)
+
+![image-20240528172159999](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\整体转换.png)
+
+**3.双目测距原理及步骤**
+
+（1）去畸变以及立体校正：消除相机镜头的畸变效应，使得图像中的物体保持准确的几何形状，提高图像处理的精度和准确性；校正立体摄像机的图像，使得左右摄像机的图像在几何上保持一致，以便后续的立体视觉处理。校正过程包括旋转和平移操作，以及重投影矩阵Q的计算，以确保左右摄像机的图像具有相同的视图和几何结构。
+
+（2）立体匹配：找出两个相机中相互匹配的点以便后续计算视差以及深度。
+
+（3）识别得到左右目中，待测物体的中心像素点坐标。
+
+（4）计算距离（深度信息）
+
+**4.测距过程中使用到的矩阵**
+
+测距过程使用到的矩阵除了上面1中提到的相机内外参两个矩阵，还包括有：
+
+- 畸变系数矩阵：用于相机标定过程中的去畸变。
+
+- 重投影矩阵Q：用于将视差图转换为3D点云。举例如下：
+  $$
+  \left(
+  \begin{matrix}
+  1 & 0 & 0 & -c_x \\ 0 & 1 & 0 & -c_y \\ 0 & 0 & 0 & f \\ 0 & 0 & -1/T_x & (c_x - c^{'}_x)/T_x 
+  \end{matrix}
+  \right)
+  $$
+  其中cx与cy代表相机主点（相机主光轴与像平面的交点即是主点）的图像坐标；f为重投影后的焦距；Tx相反数等于基线距离；$(c_x - c^{'}_x) $ 代表了左右摄像头图像中心的差距。
+
+- Z矩阵：从 3D 点云中提取的深度图，表示每个像素到相机的距离。
+
+#### 2.2 主要内容
+
+- detection.cc
+
+  **关联头文件**：dection.h
+
+  **主要功能**：对输入的视频帧进行目标检测。
+
+  **主要函数**：
+
+  - ``` Dection()``` ：整个构造函数的作用是在创建`Detection`对象时加载模型，并初始化相关的属性和参数，以备后续的使用；
+  - `detect_result_group_t Detection::outputParse(cv::Mat netInputImg)` ：接收一个图像作为输入，对其进行预处理，推理和后处理，最终的到检测结果保存到detect_result_group_t结构体中并返回。  
+  
+- <font color = "red">**ranging.cc**</font>
+
+  **关联头文件**：ranging.h
+
+  **头文件重要参数**：目标检测模型、相机内外参及畸变系数、图像校正矩阵、重投影矩阵、深度图等。
+
+  **主要功能**：利用两个摄像头拍摄的左右图像，通过图像校正、模板匹配和三维重建等技术来检测目标并计算其距离、宽度、高度和角度信息。系统的主要功能包括图像校正、像素坐标转相机坐标、模板匹配、水平估计以及从双目图像中提取测距信息。
+
+  **主要函数**：
+
+  ##### 1. `Ranging::rectifyImage`
+
+  ```cpp
+  void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR)
+  ```
+
+  - **作用**: 对输入的左图像 `oriImgL` 和右图像 `oriImgR` 进行重映射校正，去除图像畸变。
+
+  - **参数:**
+    
+    - `oriImgL`: 左图像的引用。
+    
+    - `oriImgR`: 右图像的引用。
+    
+  - **步骤：**
+
+    使用OpenCV的remap函数对左右图像去畸变：`remap` 函数是 OpenCV 中用于图像重映射的函数，它可以根据给定的映射关系对图像进行变换：
+
+    ```cpp
+     remap(oriImgL, oriImgL, mapX1, mapX2, cv::INTER_LINEAR);//左照片重映射
+     remap(oriImgR, oriImgR, mapY1, mapY2, cv::INTER_LINEAR);//右照片重映射
+    ```
+
+    - `oriImgL`：这是原始的左侧图像，需要进行畸变校正和旋转校正。
+    - `mapX1` 和 `mapX2`：这两个参数是 `initUndistortRectifyMap` 函数计算得到的映射数组，用于指定图像的重映射关系。`mapX` 存储了水平方向的映射关系，`mapY` 则存储了垂直方向的映射关系。
+    - `cv::INTER_LINEAR`：这是插值方法的选项之一。在这里使用的是双线性插值方法，它会根据相邻像素的值进行加权平均，以便在重映射过程中获得更平滑的结果。
+  ##### 2. `Ranging::pic2cam`
+
+  ```c++
+  std::vector<float> Ranging::pic2cam(int u, int v)
+  ```
+
+  - **作用**: 将像素坐标 `(u, v)` 转换为相机坐标 `(x, y)`。
+  - **参数:**
+    - `u`: 像素坐标的横坐标。
+    - `v`: 像素坐标的纵坐标。
+  - **返回值**: 相机坐标 `(x, y)` 的向量。
+  - **依据公式：**相机坐标与像素坐标之间的转换：(u-u0)*f/fx；(v-v0)*f/fy
+
+  ##### 3. `Ranging::muban`
+
+  ```c++
+  std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates)
+  ```
+
+  - **作用**: 使用模板匹配的方法根据右图像的指定区域在左图像中找到匹配的位置。
+
+  - **参数:**
+    
+    - `left_image`: 左图像的引用。
+    - `right_image`: 右图像的引用。
+    - `coordinates`: 感兴趣区域的坐标数组 `[x1, y1, x2, y2]`。
+    
+  - **返回值**: 匹配点的坐标向量 `[maxLoc.x, maxLoc.y]`。
+
+  - **重要库函数：**muban函数核心部分使用了 OpenCV 中的 `matchTemplate` 函数，用于在目标图像中寻找模板图像的匹配位置，并计算匹配程度。
+
+    ```cpp
+    matchTemplate(target, tpl, result, TM_CCOEFF_NORMED);
+    ```
+
+    - `target`：这是目标图像，即需要在其中寻找模板匹配的图像。
+    - `tpl`：这是模板图像，即用于匹配的参考图像。
+    - `result`：这是输出的匹配结果图像，用于存储匹配程度。它的大小通常会比目标图像小，因为匹配操作会在目标图像上滑动模板，并在每个位置计算匹配程度，因此结果图像会比目标图像的尺寸小。
+    - `TM_CCOEFF_NORMED`：这是匹配方法的选择参数。在这里使用的是归一化的相关系数匹配方法。该方法会在目标图像上滑动模板，并计算每个位置的相关系数（匹配程度），最终将相关系数归一化到 [0, 1] 的范围内。归一化的相关系数越接近于 1，表示匹配程度越高。
+
+    `matchTemplate` 函数的作用是在目标图像中寻找模板图像的匹配位置，并计算匹配程度。在这个例子中，使用的是归一化的相关系数匹配方法，可以有效地检测出目标图像中与模板图像相似的区域，并提供匹配程度的评估。
+
+  ##### 4. `Ranging::horizon_estimate`
+
+  ```c++
+  void Ranging::horizon_estimate(Mat& img, Mat& bboxs, int k)
+  ```
+
+  - **作用**: （单目系统）计算目标框中的每个点的距离和角度，并存储在 `Z` 矩阵中。
+
+  - **参数:**
+    - `img`: 输入图像的引用。
+    
+    - `bboxs`: 包含目标框坐标的矩阵。
+    
+    - `k`: 目标框的索引。
+    
+  - **重要函数：**要进行深度估计，重要的一步需要使用到OpenCV的canny（）函数进行边缘检测：
+
+    canny()边缘检测过程：
+    1).Canny边缘检测只算子是John F. Canny于 1986 年开发出来的一个多级边缘检测算法。2.)Canny 的目标是找到一个最优的边缘检测算法，最优边缘检测的含义是:好的检测-算法能够尽可能多地标识出图像中的实际边缘。好的定位-标识出的边缘要尽可能与实际图像中的实际边缘尽可能接近。最小响应- 图像中的边缘只能标识一次，并且可能存在的图像噪声不应标识为边缘。
+
+    3).算法步骤:①灰度转换-cvtColor②高斯模糊-GaussianBlur。以平滑图像，滤除噪声。③计算梯度-Sobel/Scharr
+    ④非最大信号抑制，以消除边缘检测带来的杂散响应，
+    高低阈值输出二值图像，检测来确定真实的和潜在的边缘，最后，通过抑制孤立的弱边缘最终完成边缘检测。
+
+    ```cpp
+    cvtColor(tpl, grayImage, COLOR_BGR2GRAY); 
+    GaussianBlur(grayImage,grayImage,Size(5,5),0);
+    Canny(grayImage, edge, 120, 180, 3);  //提取边缘，获取与地面接触点
+    ```
+
+    - `grayImage`：这是输入的灰度图像，即需要进行边缘检测的图像。
+    - `edge`：这是输出的边缘图像，用于存储检测到的边缘信息。
+    - `120` 和 `180`：这两个参数分别是 Canny 边缘检测算法中的低阈值和高阈值。在检测边缘时，像素的梯度值高于高阈值被认为是强边缘，低于低阈值但高于低阈值的像素被认为是弱边缘，低于低阈值的像素被认为是非边缘。
+    - `3`：这是 Sobel 算子的大小，用于计算图像的梯度。在这里，大小为 3 表示使用 3x3 的 Sobel 算子进行梯度计算。
+
+  ##### 5. `Ranging::getInfo`
+
+  ```c++
+  void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
+  ```
+
+  - **作用**: （双目系统）从右图像的所有目标框中提取测距信息，包括目标中心距离、物体宽度、高度、与相机光轴之间的水平夹角。
+
+  - **参数:**
+    
+    - `imgL`: 左图像的引用。
+    
+    - `imgR`: 右图像的引用。
+    
+    - `detBoxes`: 包含目标框坐标的矩阵。
+    
+    - `info`: 存储测距信息的矩阵。
+    
+   - **函数过程：**
+
+     针对右图像中每个目标框的边界进行模板匹配得到水平视差-->定义视差矩阵并根据重投影矩阵q得到每个像素点的深度即立体重建过程-->得到每个像素点距离相机的距离-->根据相似三角形等原理计算物体与相机光轴之间的水平夹角、物体宽度、高度。
+
+  - **重要函数：**
+
+    ```cpp
+    reprojectImageTo3D(disp_matrix, threedImage, q, false);
+    ```
+
+    `reprojectImageTo3D` 函数利用视差图和 Q 矩阵，通过对每个像素点进行上述的齐次坐标变换，最终将二维的视差图转化为三维的坐标点云。
+
+    对于一个在图像中的像素点 (𝑢,𝑣)和其视差值 𝑑，该点在相机坐标系下的三维坐标 (𝑋,𝑌,𝑍)可以通过以下过程计算：
+
+    1. **基本关系**：在立体相机系统中，两个相机之间有一个基线距离 𝐵。两个相机的投影平面上的点之间的视差 𝑑 与深度 𝑍 的关系为：
+
+       ![image-20240603151211728](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\深度计算公式.png)
+
+       其中，𝑓*f* 是相机的焦距，𝐵*B* 是两个相机的基线距离。
+
+    2. **像素坐标到相机坐标**： 假设图像的中心点坐标是 (𝑐𝑥,𝑐𝑦)，然后像素点 (𝑢,𝑣)在图像坐标系中表示，相应的相机坐标 (𝑋,𝑌,𝑍) 可以通过以下公式得到：
+
+       ![image-20240603151302765](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151302765.png)
+
+       ### 三维坐标计算
+
+       利用视差图和 Q 矩阵，可以得到像素点的三维坐标。
+
+       ```cpp
+       X = (u * q11 + v * q12 + d * q13 + q14) / (u * q41 + v * q42 + d * q43 + q44)
+       Y = (u * q21 + v * q22 + d * q23 + q24) / (u * q41 + v * q42 + d * q43 + q44)
+       Z = (u * q31 + v * q32 + d * q33 + q34) / (u * q41 + v * q42 + d * q43 + q44)	
+       ```
+
+       ### 公式推导过程
+
+       - 对于 X 坐标：
+
+         ![image-20240603151559322](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151559322.png)
+
+       - 对于 Y 坐标：
+
+         ![image-20240603151609412](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151609412.png)
+
+       - 对于 Z 坐标：
+
+         ![image-20240603151617966](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151617966.png)
+
+  ##### 6. `Ranging::get_range`
+
+  ```c++
+  std::vector<Mat> Ranging::get_range()
+  ```
+
+  - **作用**: 获取双目相机的图像帧，对图像进行处理并返回测距信息。
+  - **返回值**: 包含右图像、目标框和测距信息的向量。
+
+  ##### 7. `Ranging::Ranging`
+
+  ```c++
+  Ranging::Ranging(int index, int imgw, int imgh)
+  ```
+
+  - **作用**: 类的构造函数，初始化各类矩阵和相机参数。
+
+  - **参数:**
+    
+    - `index`: 摄像头的索引。
+    
+    - `imgw`: 图像宽度。
+    
+    - `imgh`: 图像高度。    
+    
+   - **重要库函数：**在初始化相机参数的过程中需要使用到OenCV的以下库函数：
+
+     `initUndistortRectifyMap` 函数的作用是根据相机的内部参数和畸变系数，以及校正变换的参数，计算出一个映射关系。这个映射关系可以将畸变的图像映射为经过校正后的图像，以消除镜头畸变和校正图像的旋转，使得校正后的图像中的物体保持准确的几何形状。
+
+     ```c++
+     initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);
+     ```
+
+     - `cam_matrix_left.t()`：这是左摄像机的相机矩阵的转置。相机矩阵是一个 3x3 的矩阵，它包含了相机的内部参数，如焦距、主点位置等信息。`.t()` 是转置操作符，用于将相机矩阵进行转置操作。
+
+     - `distortion_l`：这是左摄像机的畸变系数，它包含了径向畸变和切向畸变的参数，用于描述相机镜头的非理想特性。
+
+     - `r1` 和 `p1`：这两个参数是矫正变换的输出矩阵。`r1` 是旋转矩阵，用于将图像校正为水平方向；`p1` 是投影矩阵，用于将校正后的图像映射到一个新的视图中。
+
+     - `imgSize`：这是原始图像的大小，用于指定待校正图像的尺寸。
+
+     - `CV_32F`：这是映射数据的数据类型，指定为 `CV_32F` 表示映射数据将使用 32 位浮点数进行存储。
+
+     - `mapX1` 和 `mapX2`：这两个参数是输出的映射数组，它们用来存储 `initUndistortRectifyMap` 计算出的水平方向的映射关系。
+
+     `stereoRectify` 函数，用于校正立体摄像机的图像，以便在后续的立体视觉处理中获得准确的深度信息。
+     
+     ```cpp
+     stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,imgSize, rotate.t(), trans, r1, r2, p1, p2, q);
+     ```
+     
+     - `cam_matrix_left.t()` 和 `cam_matrix_right.t()`：分别是左侧和右侧摄像机的相机矩阵的转置。相机矩阵包含了相机的内部参数，如焦距、主点位置等信息。
+     - `distortion_l` 和 `distortion_r`：分别是左侧和右侧摄像机的畸变系数，用于描述相机镜头的非理想特性。
+     - `imgSize`：原始图像的大小，用于指定待校正图像的尺寸。
+     - `rotate`：旋转矩阵，用于将左右摄像机的图像进行校正，使得它们的光学轴平行。
+     - `trans`：平移矩阵，用于将左右摄像机的图像进行校正，使得它们的光学中心在同一水平线上。
+     - `r1` 和 `r2`：分别是左侧和右侧摄像机的旋转矩阵，用于将左右摄像机的图像进行校正，使得它们的光学轴平行。
+     - `p1` 和 `p2`：分别是左侧和右侧摄像机的投影矩阵，用于将左右摄像机的校正后的图像映射到一个新的视图中。
+     - `q`：深度映射矩阵，用于将图像中的像素点映射到三维空间中，并计算出每个像素点的深度信息。
+     
+     `stereoRectify` 函数的作用是校正立体摄像机的图像，使得左右摄像机的图像在几何上保持一致，以便后续的立体视觉处理。校正过程包括旋转和平移操作，以及投影矩阵的计算，以确保左右摄像机的图像具有相同的视图和几何结构。最终的深度映射矩阵 `q` 可以用于将图像中的像素点映射到三维空间中，并计算出每个像素点的深度信息，用于后续的立体视觉分析和深度估计。
+
+- main.cc
+
+  **主要功能**：通过摄像头获取图像数据，并使用自定义的测距算法对图像中的障碍物进行距离测量。测量结果以自定义的ROS消息类型发布，用于机器人导航或其他需要环境感知的应用。
+
+  **主要函数：** 
+
+  1. **宏定义**
+
+     ```c++
+     #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) // 角度转化为弧度
+     ```
+
+     - 将角度值转换为弧度值，便于后续计算。
+
+  2. **全局变量**
+
+     ```c++
+     Ranging r(40, 640, 480);
+     std::queue<std::vector<cv::Mat>> frameInfo;
+     std::mutex g_mutex;
+     ```
+
+     - `Ranging r(40, 640, 480)`：初始化测距对象，假设摄像头索引为40，分辨率为640x480。
+     - `std::queue<std::vector<cv::Mat>> frameInfo`：存储图像帧信息的队列。
+     - `std::mutex g_mutex`：用于同步访问队列的互斥锁。
+
+  3. **`void *ranging(void *args)`**
+
+     ```c++
+     void *ranging(void *args) {
+         ros::Publisher dis_pub_;
+         ros::NodeHandle nh;
+         dis_pub_ = nh.advertise<pibot_msgs::dis_info_array>("ceju_info",10);
+     
+         while (true) {
+             std::vector<cv::Mat> result = r.get_range();
+             cv::Mat info = result[2];
+             pibot_msgs::dis_info_array dis_array;
+     
+             if (!info.empty()) {
+                 for (int i = 0; i < info.rows; i++) {
+                     if (info.at<float>(i,0) > 0 && info.at<float>(i,0) < 200) {
+                         pibot_msgs::dis_info data;
+                         data.distance = info.at<float>(i,0);
+                         data.width = info.at<float>(i,1);
+                         data.height = info.at<float>(i,2);
+                         data.angle = info.at<float>(i,3);
+                         dis_array.dis.push_back(data);
+                         std::cout << data.distance << std::endl;
+                     }
+                 }
+                 dis_pub_.publish(dis_array);
+             }
+     
+             g_mutex.lock();
+             while (!frameInfo.empty()) frameInfo.pop();
+             frameInfo.push(result);
+             g_mutex.unlock();
+         }
+     }
+     ```
+
+     - 该函数是一个独立的线程函数，持续从摄像头获取图像数据，并进行障碍物测距。
+     - 使用`r.get_range()`获取测距信息，并将结果存入`pibot_msgs::dis_info_array`中，通过ROS话题发布。
+     - 使用互斥锁同步访问全局队列`frameInfo`，确保线程安全。
+
+  4. **`int main(int argc, char \**argv)`**
+
+     ```c++
+     int main(int argc, char **argv) {
+         ros::init(argc, argv, "stereo");
+         ros::NodeHandle nh;
+         pthread_t tids[1];
+         int ret = pthread_create(&tids[0], NULL, ranging, NULL);
+         pthread_join(tids[0], NULL);
+     
+         return 0;
+     }
+     ```
+
+     - 初始化ROS节点`ros::init(argc, argv, "stereo")`。
+     - 创建并执行测距线程`pthread_create(&tids[0], NULL, ranging, NULL)`。
+     - 使用`pthread_join(tids[0], NULL)`等待测距线程结束，确保主程序在测距线程结束前不会退出。
+
+### 三.代码优化
+
+#### 1.运行速度慢
+
+####  可能存在的时间瓶颈
+
+- **模板匹配** (`muban`)
+
+  ```cpp
+  std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
+  ```
+  
+  - **原因**: 模板匹配通常是一个计算密集型操作，特别是当模板和搜索区域较大时，计算时间会显著增加。
+  
+  - **优化建议**: 可以尝试缩小模板区域，从减小匹配区域，加快匹配速度。
+  
+- **重投影到3D** (`reprojectImageTo3D`)
+  
+  ```cpp
+  Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x));
+  Mat threed_pixel_xyz, threedImage;  
+  reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
+  ```
+  
+  - **原因**: `reprojectImageTo3D`将2D图像重新投影到3D空间，这个过程涉及大量的矩阵运算，可能会比较耗时；而且代码中是针对图像中所有像素点在每个循环中都计算三维矩阵，由于图像分辨率的原因会导致矩阵很大，从而影响计算速度。
+  - **优化建议**:可以针对图像中的目标区域计算三维矩阵。
+  
+- **计算欧式距离** (`forEach` 操作)
+  
+  ```cpp
+  threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); });
+  ```
+  
+  - **原因**: 遍历每个像素并计算平方根操作对于大图像来说是非常耗时的。
+  - **优化建议**: 可以针对图像中的目标区域计算深度信息。
+  
+- **繁琐的矩阵和向量操作**
+  
+  ```cpp
+  std::vector<float> ltPoint = pic2cam(x1, y1);
+  std::vector<float> rbPoint = pic2cam(x2, y2);
+  float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
+                  pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
+                  pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
+  ```
+  
+  - **原因**: 计算距离和角度涉及到多个矩阵和向量操作，这些操作在大图像和多目标的情况下会非常耗时。
+  
+  - **优化建议**: 尽量减少冗余计算，优化数学运算部分，必要时使用并行计算或SIMD指令集加速。
+  
+- **Image Rectification (图像校正)**
+  
+    图像校正涉及重映射操作，可能是时间消耗较大的部分：
+  
+    ```cpp
+    rectifyImage(lframe, rframe);
+    ```
+    
+    **优化建议**:
+  
+    - 确保校正的映射矩阵 (`mapX1`, `mapX2`, `mapY1`, `mapY2`) 只计算一次，不在每帧都重新计算。
+  - 如果可以，尝试对输入图像进行裁剪，只对感兴趣的部分进行校正。
+
+#### 2.优化空间（代码规范性）
+
+##### 2.1 线程管理
+
+- **减少锁的使用**：使用标准库提供的线程安全队列来替代手动加锁。
+
+- **代码结构优化**：将线程函数独立成类成员函数，增加代码的可读性和可维护性。
+
+- **合理使用多线程**：如果某些部分不需要频繁执行，可以考虑使用条件变量来减少无效的循环。
+
+  ```cpp
+  #include <iostream>
+  #include <ros/ros.h>
+  #include <opencv2/opencv.hpp>
+  #include <thread>
+  #include <mutex>
+  #include <queue>
+  #include <condition_variable>
+  #include "upbot_vision/ranging.h"
+  #include "pibot_msgs/dis_info.h"
+  #include "pibot_msgs/dis_info_array.h"
+  
+  #define ANGLE_TO_RADIAN(angle) ((angle) * 3.14159 / 180.0) //角度转化为弧度
+  
+  class RangingSystem {
+  public:
+      RangingSystem()
+          : r(40, 640, 480), nh(), dis_pub_(nh.advertise<pibot_msgs::dis_info_array>("ceju_info", 10)) {
+          startThreads();
+      }
+  
+      ~RangingSystem() {
+          running = false;
+          cv.notify_all();
+          if (ranging_thread.joinable()) ranging_thread.join();
+          if (process_thread.joinable()) process_thread.join();
+      }
+  
+  private:
+      Ranging r;
+      ros::NodeHandle nh;
+      ros::Publisher dis_pub_;
+      std::queue<std::vector<cv::Mat>> frameQueue;
+      std::mutex mtx;
+      std::condition_variable cv;
+      std::thread ranging_thread;
+      std::thread process_thread;
+      bool running = true;
+  
+      void startThreads() {
+          ranging_thread = std::thread(&RangingSystem::rangingTask, this);
+          process_thread = std::thread(&RangingSystem::processTask, this);
+      }
+      //负责从 Ranging 对象中获取数据，并将其推入一个线程安全的队列。
+      void rangingTask() {
+          while (running) {
+              std::vector<cv::Mat> result = r.get_range();
+              {
+                  std::lock_guard<std::mutex> lock(mtx);
+                  if (frameQueue.size() > 0) frameQueue.pop(); // 保持队列最新的帧
+                  frameQueue.push(result);
+              }
+              cv.notify_one();
+          }
+      }
+      //负责处理队列中的数据并发布。
+      void processTask() {
+          while (running) {
+              std::vector<cv::Mat> result;
+              {
+                  std::unique_lock<std::mutex> lock(mtx);
+                  cv.wait(lock, [this] { return !frameQueue.empty() || !running; });
+                  if (!running) break;
+                  result = frameQueue.front();
+                  frameQueue.pop();
+              }
+  
+              cv::Mat info = result[2];
+              pibot_msgs::dis_info_array dis_array;
+              if (!info.empty()) {
+                  for (int i = 0; i < info.rows; ++i) {
+                      if (info.at<float>(i, 0) > 0 && info.at<float>(i, 0) < 200) {
+                          pibot_msgs::dis_info data;
+                          data.distance = info.at<float>(i, 0);
+                          data.width = info.at<float>(i, 1);
+                          data.height = info.at<float>(i, 2);
+                          data.angle = info.at<float>(i, 3);
+                          dis_array.dis.push_back(data);
+                          std::cout << data.distance << std::endl;
+                      }
+                  }
+                  dis_pub_.publish(dis_array);
+              }
+          }
+      }
+  };
+  
+  int main(int argc, char **argv) {
+      ros::init(argc, argv, "stereo");
+      RangingSystem rangingSystem;
+      ros::spin();
+      return 0;
+  }
+  
+  ```
+
+  - **线程安全队列**：使用 `std::condition_variable` 和 `std::unique_lock` 来保证队列的线程安全，避免手动使用互斥锁和条件变量时可能出现的错误。
+  
+  - **条件变量**：用条件变量来管理线程的等待和通知，避免忙等，提高效率。
+  
+  - **类封装**：将相关函数封装到一个类中，增强代码的可读性和可维护性。
+  
+  - **线程管理**：在析构函数中停止线程并等待其结束，确保资源的正确释放。
+  
+##### 2.2 注释
+
+​    对于代码中的部分变量解释不够清晰，影响代码的可读性。
+
+#### 3.通过双目视觉计算深度的核心原理优化深度计算算法
+
+**1.省去模板匹配**
+
+利用rk3588的两个npu分别进行检测，取得识别得到的目标中心的像素坐标，进行计算视差，再而通过重投影矩阵然后计算深度。
+
+**2. 缩小模板匹配的搜索区域** 
+
+尝试缩小待匹配模板的搜索区域，从而减小计算压力，提高运行速度。
+
+**3.引入跟踪算法**
+
+加入跟踪算法，在检测后面加上跟踪框，针对部分置信度较高的框的，可以省去模板匹配，直接使用上一帧目标框的视差值。