基于深度攝像頭的障礙物檢測（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;

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 Images

cv::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)

depthCam.release();