基于深度摄像头的障碍物检测（realsense+opencv）

原文链接

前几天老大给了个任务，让我帮slam组写一个基于深度摄像头的障碍物检测，捣鼓了两天弄出来了，效果还不错，就在这里记一下了。

代码的核心思路是首先通过二值化，将一米之外的安全距离置零不考虑，然后通过开运算去除掉一些噪点（这个后来发现不一定有必要），在求出所有障碍物的凸包，这个时候要计算面积，当面积小于一定的阈值的时候不予考虑，最终输出障碍物的凸包坐标。

//find_obstacle函数是获取深度图障碍物的函数,返回值是每个障碍物凸包的坐标，参数一depth是realsense返回的深度图（ushort型），
//参数二thresh和参数三max_thresh，是二值化的参数，参数四是凸包的最小有效面积，小于这个面积的障碍物可以视为噪点。
//函数首先筛选掉距离大于安全距离的点，然后进行阀值化和开运算减少一下噪点，用findContours得到轮廓图，最后用convexHull得到每个障碍物的凸包，最后返回坐标

//mask_depth函数是对深度图二值化，第一个参数image是原图，第二个参数th是目标图，第三个参数throld是最大距离，单位是mm，大于这个距离
//即为安全，不用考虑。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "RSWrapper.h"
#include "opencv2/imgproc/imgproc.hpp"using namespace std;
using namespace cv;
void mask_depth(Mat &image,Mat& th,int throld=1000)
{int nr = image.rows; // number of rows int nc = image.cols; // number of columns for (int i = 0; i<nr; i++){for (int j = 0; j<nc; j++) {if (image.at<ushort>(i, j)>throld)th.at<ushort>(i, j) = 0;}}
}vector<vector<Point> > find_obstacle(Mat &depth, int thresh = 20, int max_thresh = 255, int area = 500)
{Mat dep;depth.copyTo(dep);mask_depth(depth, dep, 1000);dep.convertTo(dep, CV_8UC1, 1.0 / 16);//imshow("color", color);imshow("depth", dep);Mat element = getStructuringElement(MORPH_RECT, Size(15, 15));//核的大小可适当调整Mat out;//进行开操作morphologyEx(dep, out, MORPH_OPEN, element);//dilate(dhc, out, element);//显示效果图imshow("opencv", out);Mat src_copy = dep.clone();Mat threshold_output;vector<vector<Point> > contours;vector<Vec4i> hierarchy;RNG rng(12345);// 对图像进行二值化threshold(dep, threshold_output, thresh, 255, CV_THRESH_BINARY);//mask_depth(src, threshold_output);// 寻找轮廓findContours(threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));// 对每个轮廓计算其凸包vector<vector<Point> >hull(contours.size());vector<vector<Point> > result;for (int i = 0; i < contours.size(); i++){convexHull(Mat(contours[i]), hull[i], false);}// 绘出轮廓及其凸包Mat drawing = Mat::zeros(threshold_output.size(), CV_8UC3);for (int i = 0; i< contours.size(); i++){if (contourArea(contours[i]) < area)//面积小于area的凸包，可忽略continue;result.push_back(hull[i]);Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));drawContours(drawing, contours, i, color, 1, 8, vector<Vec4i>(), 0, Point());drawContours(drawing, hull, i, color, 1, 8, vector<Vec4i>(), 0, Point());}imshow("contours", drawing);return result;
}int main(int argc, char* argv[])
{Mat dhc;Mat dep;int idxImageRes = 1, idxFrameRate = 30;RSWrapper depthCam(idxImageRes, idxImageRes, idxFrameRate, idxFrameRate);if (!depthCam.init()){std::cerr << "Init. RealSense Failure!" << std::endl;return -1;}while (true){//Get RGB-D Imagescv::Mat color, depth;bool ret = depthCam.capture(color, depth);if (!ret) {std::cerr << "Get realsense camera data failure!" << std::endl;break;}vector<vector<Point> > result;result = find_obstacle(depth, 20, 255, 500);if (cvWaitKey(1) == 27)break;}depthCam.release();
}

改进算法，去除地面阴影

在前文得到视差图和三维坐标的基础上，首先对地面干扰区域和行进路径以外区域进行去除。以下是地面干扰区域的去除方法，行进路径以外区域我采用的是机器人行进过程中设定一定的视角，去除视角之外的区域，通过行进路径以外物体距相机中心在x轴和z轴上的距离来就算角度。

参考论文“基于双目立体视觉的移动机器人障碍物检测技术研究”

然后对处理后的视差图进行二值化分割，大致提取到障碍物。由于立体匹配过程中一些物体文理较弱的区域未匹配，导致一个物体可能被分割成几部分，所以对图像再进行闭运算，然后再求出所有障碍物的凸包，这个时候要计算面积，当面积小于一定的阈值的时候不予考虑，最终输出障碍物的凸包坐标。
以下是实现过程：

视差图→三维测距

去除行进路径以外区域→去除地面干扰区域→二值化分割

闭运算→求凸包