forked from logzhan/RobotKernal-UESTC
parent
99c8b58333
commit
341bd6a239
|
|
@ -0,0 +1,196 @@
|
|||
### 一.目录结构
|
||||
|
||||
include：包含头文件
|
||||
|
||||
model：存放用于目标检测的权重文件
|
||||
|
||||
src：源文件
|
||||
|
||||
### 二.主要源文件
|
||||
|
||||
- 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`: 右图像的引用。
|
||||
|
||||
##### 2. `Ranging::pic2cam`
|
||||
|
||||
```c++
|
||||
std::vector<float> Ranging::pic2cam(int u, int v)
|
||||
```
|
||||
|
||||
- **作用**: 将像素坐标 `(u, v)` 转换为相机坐标 `(x, y)`。
|
||||
- **参数:**
|
||||
- `u`: 像素坐标的横坐标。
|
||||
- `v`: 像素坐标的纵坐标。
|
||||
- **返回值**: 相机坐标 `(x, y)` 的向量。
|
||||
|
||||
##### 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]`。
|
||||
|
||||
##### 4. `Ranging::horizon_estimate`
|
||||
|
||||
```c++
|
||||
void Ranging::horizon_estimate(Mat& img, Mat& bboxs, int k)
|
||||
```
|
||||
|
||||
- **作用**: 估计目标物体的水平距离和角度。
|
||||
- **参数:**
|
||||
- `img`: 输入图像的引用。
|
||||
- `bboxs`: 包含目标框坐标的矩阵。
|
||||
- `k`: 目标框的索引。
|
||||
|
||||
##### 5. `Ranging::getInfo`
|
||||
|
||||
```c++
|
||||
void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
|
||||
```
|
||||
|
||||
- **作用**: 从左、右图像中提取测距信息，包括目标的距离、宽度、高度和角度。
|
||||
- **参数:**
|
||||
- `imgL`: 左图像的引用。
|
||||
- `imgR`: 右图像的引用。
|
||||
- `detBoxes`: 包含目标框坐标的矩阵。
|
||||
- `info`: 存储测距信息的矩阵。
|
||||
|
||||
##### 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`: 图像高度。
|
||||
|
||||
- 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)`等待测距线程结束，确保主程序在测距线程结束前不会退出。
|
||||
Loading…
Reference in New Issue