目录
1. 开发背景及目标概述
2. 方案设计
2.1. 在ROS平台上发布图像话题
2.1.1. 发布策略
2.1.2. 消息封装
2.2. 基于深度的边缘检测算法设计
2.2.1. 落差环境深度特征
2.2.2. 不完全深度信息下的边缘检测
2.2.3. 基于深度的直线落差边缘检测
3. 代码架构
4. 总结
1. 开发背景及目标概述
本课题项目是在 Linux 系统下进行开发，旨在依靠 MV-EB435i 实现落差（即高度差）环境的边缘信息检测，由于种种原因（最后会提），算法的使用环境被具体为上层落差环境，即高度差位于地面正上方的环境。该算法的主要功能如下：
2. 方案设计
2.1. 在ROS平台上发布图像话题
2.1.1. 发布策略
由相机发布图像至 ROS 平台可以通过 OpenCV 库、cv_bridge 和 image_transport 功能包实现。具体实现逻辑是将相机采集到的图像转化成 cv::Mat 格式，再由
cv_bridge 将 Mat 格式图像转化成 ROS 图像消息，最后由 image_transport 发布出去。其中，相机采集图像步骤使用的是 SDK 中的 FetchFrame 函数。预期实现流程如下图所示：
图2. 预期实现流程
然而在实际运行过程中，当 main 函数中调用相机设备检测函数时 image_transport 会在
image_transport::ImageTransport it(nh) 这一步报错(Segmentation fault)…于是转变发布策略，通过设计客户端和服务器将 SDK 中的函数与
image_transport 作切割，实现流程如下：
图3. 通过客户端/服务器发布ROS图像
2.1.2. 消息封装
由于从客户端发布的消息中包含 sensor_msgs::Image 格式的图像消息和 sensor_msgs::CameraInfo 格式的相机信息，这些信息被整合进 srv 文件中，如下所示：
<![CDATA[ ---------------------------------------------------------------------------
# 请求响应的数据
sensor_msgs/Image rgb
sensor_msgs/Image depth
sensor_msgs/CameraInfo camInfo
---
# 回复响应的数据
string result
---------------------------------------------------------------------------]]>
要获取请求响应的数据，需要专门设计一个 HKcamInterface 类来进行消息转换。
<![CDATA[ ---------------------------------------------------------------------------
class HKcamInterface
|- HKcamInterface() // 构造函数
|- vector<sensor_msgs::ImagePtr> fetchFrame() // 图像获取函数
|- sensor_msgs::CameraInfo getHKCameraInfo() // 相机信息获取函数
|- void stop() // 关闭设备
---------------------------------------------------------------------------]]>
构造函数在创建对象时完成相机的初始化操作，内含多个与其相关的函数，如下所示：
<![CDATA[ ---------------------------------------------------------------------------
HKcamInterface() // 构造函数
|- MV3D_RGBD_GetSDKVersion() // 获取 SDK 版本
|- MV3D_RGBD_Initialize() // 初始化设备
|- MV3D_RGBD_GetDeviceNumber() // 获取设备编号
|- MV3D_RGBD_GetDeviceList() // 获取设备列表
|- MV3D_RGBD_OpenDevice() // 打开设备
|- MV3D_RGBD_SetParam() // 设置参数
|- MV3D_RGBD_Start() // 开始图像采集
---------------------------------------------------------------------------]]>
其中，对参数的设置主要包含分辨率和曝光时间，可以在 config/cam.yaml 中修改。
图像获取函数主要负责将 rgb /
depth 图中的 pData 数据转化成 cv::Mat 格式，再经 cv_bridge 转换，获得 ROS
图像消息。其主要部分代码如下：
<![CDATA[ ---------------------------------------------------------------------------
nRet = MV3D_RGBD_FetchFrame(handle, &stFrameData, 500);
if (MV3D_RGBD_OK == nRet)
{
// 获取图像信息
parseFrame(&stFrameData, &depth, &rgb);
// 将图像信息转化为 cv::Mat 格式
cv::Mat rgbImg = cv::Mat(cv::Size(stFrameData.stImageData->nWidth, stFrameData.stImageData->nHeight), CV_8UC3, rgb.pData);
cv::Mat depthImg = cv::Mat(cv::Size(stFrameData.stImageData->nWidth, stFrameData.stImageData->nHeight), CV_16UC1, depth.pData);
// 将 cv::Mat 图像转化为 ROS 图像消息
pRgb = cv_bridge::CvImage(std_msgs::Header(), "rgb8", rgbImg).toImageMsg();
pDepth = cv_bridge::CvImage(std_msgs::Header(), "mono16", depthImg).toImageMsg();
// 设备关闭判断
if (_kbhit())
{
if ('c' == _getch() || 'q' == _getch())
{
LOGD("recieve exit cmd!");
m_flagStop = true;
}
}
if(m_flagStop){
stop();
}
// 返回图像消息
return {pRgb, pDepth};
}
---------------------------------------------------------------------------
]]>
相机参数获取函数主要是对 sensor_msgs::CameraInfo 类中的各属性赋值，主要包括畸变系数 D、相机内参 K、投影矩阵 P 和旋转矩阵 R。这些参数可以通过 SDK 中的
MV3D_RGBD_CALIB_INFO 获得。
通过以上函数获取相应数据后封装进请求中，通过客户端发送请求，当服务器接收到数据包后进行处理，即通过 image_transport::CameraPublisher 发布话题至 ROS 平台。其主要部分代码如下：
<![CDATA[ ---------------------------------------------------------------------------
// 1. 导入ros及服务消息相关包
#include <ros/ros.h>
#include <string>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include "../common/common.hpp"
#include "sed_hk/image.h"
using namespace std;
image_transport::CameraPublisher rgbPub;
image_transport::CameraPublisher depthPub;
// 4. 设置回调函数
bool imageProcess(sed_hk::image::Request &request, sed_hk::image::Response &response)
{
rgbPub.publish(request.rgb, request.camInfo);
depthPub.publish(request.depth, request.camInfo);
return true;
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2. 初始化ros节点
ros::init(argc, argv, "sed_hk_server");
ros::NodeHandle nh;
// 3. 建立服务器
ros::ServiceServer server;
server = nh.advertiseService("imgProcess", imageProcess);
// 5. 发布话题
image_transport::ImageTransport it(nh);
string rgbTopic = "/hkcamera/rgb";
string depthTopic = "/hkcamera/depth";
rgbPub = it.advertiseCamera(rgbTopic, 1);
depthPub = it.advertiseCamera(depthTopic, 1);
// 6. spin()
ros::spin();
return 0;
}
---------------------------------------------------------------------------]]>
图像话题可在 ROS 平台上被订阅，也可在 rviz 中查看，如下图所示：
图4. 通过
rviz 查看图像
通过这种方法发布的图像话题在 360p 分辨率下的实时性较好，而在 720p 分辨率下则会有 0.5s 左右的延迟。当然，以上的测试都是在 USB3.0 数据线的传输下进行，因为收到的设备数据线没做正反插，以至于不小心使用 USB2.0 接口传输数据时会有更大且不稳定的延迟。
2.2. 基于深度的边缘检测算法设计
2.2.1. 落差环境深度特征
本算法针对的落差环境（即高度差环境）为与地面存在垂直高度差且值为正的上层落差环境，典型的有上行的阶梯，纸箱等。并且类似障碍物的前置条件是位于地面目标框内，即当相机距离地面 0.6m 左右（移动机器人高度）时，正前方 1.2 - 2.0m 的地面区域，如下图所示：
图5. 地面目标框与上层落差环境
这一类落差环境在深度图上有其独特的特征，即地面区域的深度随纵坐标变化而变化，而落差区域的深度基本保持不变，利用这一点可以将其边缘与地面进行分割。
当然，这种方法可行的前提是有充足且准确的地面深度信息。然而，在拿到 MV-EB435i 相机的初期进行简单测试的时候，发现用相机的默认参数获取的深度图地面信息缺失严重，如下图所示：
图6. 地面深度信息缺失
后续群里的伙伴指出可能是曝光和光照问题，在更改曝光参数后确实有所改变，但不能做到稳定获得完整的地面信息，个人猜测还是光照影响更大。如果忽略缺失地面信息带来的影响使用上述方法，会获得类似如下的结果：
图7. 错误地面信息下的边缘检测结果
这里点 1 - 12 表示的是右边红线上的深度值，可以看到大部分深度值都是丢失的，并且获得的深度也没有呈现理想的顺序形式，种种错误信息累计导致蓝线，即检测端点部分超出了落差边缘，因此单纯依靠上述的方法没办法很好的区分落差边缘。
2.2.2. 不完全深度信息下的边缘检测
针对部分地面深度信息缺失这个问题，可以采取的方法是深度过滤，即在进行深度特征处理前先过滤掉缺失的深度信息。为了减少计算量，先由中线开始进行纵向遍历，一旦识别到有效深度，就将此深度与固定纵向间距（m_biasRow）的深度相减获得深度差，如下图所示：
图8. 深度差特征
由之前的推理不难得出，理想障碍物的深度差应该趋近于 0，而地面深度信息在像素自下往上的纵向上的深度差应该是顺序增大的值，即便特殊情况下，即有效值减去缺失信息 0，所得到的也是远大于 0 的值，依靠这种方法可以在地面信息部分丢失的情况下获得有效的落差边缘信息。
依靠上述方法在中线上获得了起点后，以起点为初始点，向左右方向遍历，将纵向的范围缩减到前一个点的纵坐标 Ypre ± m_rangeVal 的像素范围内，可以确保获得的边缘是连续可靠的。一旦存在连续的 m_gapVal 个纵向没有特征点，即可认为检测到了落差端点。将获得的点用直线相连，表现在彩色图上，即可认为完成边缘检测。如下图所示：
图9. 算法测试
图中，算法测得的边缘信息与实际存在一定偏差，原因是深度图和彩色图没有对齐。针对这种情况，可以在计算前先将深度图与彩色图对齐，也可以在计算后对获得的点进行坐标修正。这里采用的是后者，利用 m_biasX 和 m_biasY 两个参数完成坐标修正后获得的边缘检测效果如下：
图10.
坐标修正后的边缘检测算法测试
由上图可以看出，该算法能够获取落差环境的边缘信息，其计算的边缘与实际情况基本吻合。值得注意的是，④ 在检测出甜瓜后没有再检测果篮，是因为两物之间存在一定距离，由于纵向范围的限制，无法从甜瓜的端点再连续延伸至果篮，因此出现了分割。这也说明由中线向左右检测的边缘只能是连续边缘，而对目标框内的落差检测也只针对视觉中心连续的物体。另外，⑤ 中间部分是悬空的椅边，并非与地面接触，但其深度特征依旧满足算法逻辑，因此也能被检测出来。
此外，可以通过修改 config/config.yaml 中的 m_flagRT 来达到实时检测的目的。该算法在 MV-EB435i 上实测可以达到
360p / 30FPS 速度的落差边缘检测，效果如下图所示：
图11. MV-EB435i 实时落差边缘检测
最后，由于算法的输入是如下图所示的不完全的深度信息，难免会有边边角角的计算误差，加之实验时相机一直是手持，无法确保相机的垂直度，以及摇晃带来的影响。除此之外，相机可调的参数被写在 config/cam.yaml 配置文件中；而算法可调的各项参数统一写在 config/config.yaml 配置文件中，具体参数说明参见这两个文件，这里不做赘述。
图12. 不完全深度信息
2.2.3. 基于深度的直线落差边缘检测
对直线边缘的检测在彩色图上可以通过 Hough 变换实现，但这种方法在边缘颜色与环境颜色相近时就没办法很好的进行识别。本课题项目设计的算法是基于深度特征，因此不存在颜色干扰问题。考虑到检测对象是直线，而计算获取的信息是各个像素点坐标，可以直接对获取的点组进行直线拟合，由此完成直线落差边缘检测。相关函数为 Sed_hk::lineProcess() ，主要代码如下：
<![CDATA[ ---------------------------------------------------------------------------
vector<int> Sed_hk::lineProcess(){
// 创建点组
vector<cv::Point2f> points;
// 创建直线对象
cv::Vec4f lines;
// 将计算求得的左右点信息 (m_edgeL / m_edgeR) 输入点组
for(int i = 0; i < m_numL; i++){
points.push_back(cv::Point2f(m_edgeL[2 * i], m_edgeL[2 * i + 1]));
}
for(int i = 1; i < m_numR; i++){
points.push_back(cv::Point2f(m_edgeR[2 * i], m_edgeR[2 * i + 1]));
}
// 调用 OpenCV 中的直线拟合函数
cv::fitLine(points, lines, cv::DIST_L2, m_param, m_reps, m_aeps);
// y = k * x + b;
// x = (y - b) / k;
// b = y - k * x;
// 计算直线
float k = lines[1] / lines[0];
float b = lines[3] - (k * lines[2]);
// 线段起点 (x1, y1)
int x1 = m_edgeL[2 * m_numL];
int y1 = k * x1 + b;
// 线段终点 (x2, y2)
int x2 = m_edgeR[2 * m_numR];
int y2 = k * x2 + b;
// 返回端点信息
return {x1, y1, x2, y2};
}
---------------------------------------------------------------------------]]>
由此获得的直线检测效果如下图所示：
图13.
基于深度的直线落差边缘检测 MV-EB435i 实测
由上图可以看出，在深度信息较为完整的情况下，原本计算得到的点组就已经比较接近实际边缘，而在此基础上进行的直线拟合得到的端点与实际直线落差边缘非常接近。而深度信息缺失严重的直线检测如下图所示：
图14. 深度信息缺失严重的直线落差边缘检测 MV-EB435i 实测
如上图所示，由于光照等各项因素的干扰，导致深度图目标框部分出现大量的信息丢失。同样是直线落差边缘，在此基础上进行的边缘检测获得的点组与前面的点组相对比波动更大。但是经过直线拟合后获取的直线端点与实际直线落差边缘端点依旧相近，由此证明该算法在深度信息缺失的情况下针对直线落差边缘依旧有良好的表现。此外，上图所示的是阶梯颜色与地面颜色相同的情况，此类环境的边缘在彩色图上的表现是连肉眼也比较难以分辨的，但通过本算法对深度信息的计算，还是能够成功检测出边缘。
3. 代码架构
图15.
ROS平台下基于 MV-EB435i SDK 二次开发的落差边缘检测代码架构
4. 总结
着手本次课题研究是从六月初开始到八月初，这期间遇到了很多困难，大多是算法设计上的，也有硬件上的一些问题，这里感谢群里小伙伴的热心帮助。当然，办法总比困难多，对于克服了种种难关获得的成果个人还是比较满意的。不过也有遗憾的地方，那就是当前获得的成果与预期其实是有较大差别的。在课题活动申报的时候计划的应用背景是辅助移动机器人跟随，那时手头有的相机是 IntelRealsenseD435i。想要与跟随结合，需要设计的模块就不光是检测，还有各种情况下的识别、上下层判断、避障策略的考量等。在完成上述内容的代码设计以及 gazebo 建模仿真后，剩余的时间已经来不及整理一篇期刊论文先发表出来，所以这部分的设计和代码目前都没办法公开（课题相关研究学校要求先发表…）。迫不得已，只能在七月中旬请假回家，设计一种新的算法与之前的内容做切割，而这期间设计的目标就转变成了现在的“针对上层落差环境基于深度的边缘检测”。写了这么多，其实主要是想表达要做好时间规划（不要忽略文档整理的工作量…），然后就是最简单的道理，不要放弃。
来自：浙江大学——陈连杰团队
开源代码：https://github.com/Ahoclairl/sed_hk
浙公网安备 33010802013223号