OpenCV学习[1]-基于深度摄像头障碍物检测的一个例子

前言

本文原载于：《基于深度摄像头的障碍物检测（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) //大于临界值的设为0
                th.at<ushort>(i, j) = 0;
        }
    }
}

//定义了一个获取深度图障碍物的函数，返回一个vector
vector< vector<Point> > find_obstacle(Mat &depth, int thresh = 20, int max_thresh = 255, int area = 500){
    Mat dep;
    depth.copyTo(dep);  //CopyTo函数，将深度图depth复制到新的dep中去
    mask_depth(depth, dep, 1000);
    dep.convertTo(dep, CV_8UC1, 1.0 / 16);  //convertTo函数，将数据的类型进行了一些变化
    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);   //调用convexHull函数
    }

// 绘出轮廓及其凸包
    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);   //可能是intel库函数
    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) break;
}

depthCam.release();
}

示例