# 基于双目摄像头SGBM视差图的障碍物提取

1.双目摄像头校正

2.视差图提取

3.删除无关干扰区域

4.提取障碍物轮廓，重心，大小等属性

# 一丶相机标定

/*

fx 0 cx
0 fy cy
0 0  1
*/
//MATLAB优化前
Mat cameraMatrixL = (Mat_<double>(3, 3) << 1440.07133, 0, 954.95752,
0, 1440.87527, 537.00282,
0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.00691, -0.00898, 0.00032, 0.00235, 0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 1435.24677, 0, 1011.06019,
0, 1435.19970, 545.11155,
0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << 0.01228, -0.02151, 0.00005, 0.00199, 0);

Mat T = (Mat_<double>(3, 1) << -59.33860, -0.18628, -0.62481);//T平移向量
Mat rec = (Mat_<double>(3, 1) << 0.00484, -0.01154, 0.00018);//rec旋转向量
Mat R;//R 旋转矩阵

# 二丶视差图提取

本文采用opencv中的sgbm算法，进行视差图提取，部分代码如下：

void stereo_match(Ptr<StereoSGBM> sgbm,Mat rectifyImageL,Mat rectifyImageR,Mat &disp8,Mat  &xyz)
{
sgbm->setPreFilterCap(63);
int sgbmWinSize = 5;//根据实际情况自己设定
int NumDisparities = 416;//根据实际情况自己设定
int UniquenessRatio = 6;//根据实际情况自己设定
sgbm->setBlockSize(sgbmWinSize);
int cn = rectifyImageL.channels();

sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);
sgbm->setNumDisparities(NumDisparities);
sgbm->setUniquenessRatio(UniquenessRatio);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(10);
sgbm->setDisp12MaxDiff(1);
sgbm->setMode(StereoSGBM::MODE_SGBM);
Mat  disp,dispf;
sgbm->compute(rectifyImageL, rectifyImageR, disp);
//去黑边
Mat img1p, img2p;
copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
dispf = disp.colRange(NumDisparities, img2p.cols - NumDisparities);

dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
imshow("disparity", disp8);

}

并且在程序中对视差图进行了上色，程序如下：

/*给深度图上色*/
void GenerateFalseMap(cv::Mat &src, cv::Mat &disp)//视差图和彩色视差图
{
// color map
float max_val = 255.0f;
float map[8][4] = { { 0,0,0,114 },{ 0,0,1,185 },{ 1,0,0,114 },{ 1,0,1,174 },
{ 0,1,0,114 },{ 0,1,1,185 },{ 1,1,0,114 },{ 1,1,1,0 } };
float sum = 0;
for (int i = 0; i < 8; i++)
sum += map[i][3];

float weights[8]; // relative   weights
float cumsum[8];  // cumulative weights
cumsum[0] = 0;
for (int i = 0; i < 7; i++) {
weights[i] = sum / map[i][3];
cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
}

int height_ = src.rows;
int width_ = src.cols;
// for all pixels do
for (int v = 0; v < height_; v++) {
for (int u = 0; u < width_; u++) {

// get normalized value
float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

// find bin
int i;
for (i = 0; i < 7; i++)
if (val < cumsum[i + 1])
break;

// compute red/green/blue values
float   w = 1.0 - (val - cumsum[i])*weights[i];
uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);
//rgb内存连续存放
disp.data[v*width_ * 3 + 3 * u + 0] = b;
disp.data[v*width_ * 3 + 3 * u + 1] = g;
disp.data[v*width_ * 3 + 3 * u + 2] = r;
}
}
}

最终得到的视差图如下所示：

# 三丶干扰区域去除

## 2.地面去除

h=H-y

# 四丶轮廓提取，高度等属性

opencv中常见的一些操作，可以自行查一下，我直接放一下程序。

//阈值化
int threshValue = Otsu(result1);
Mat local;
threshold(result1, local, 20,255,CV_THRESH_BINARY);
imshow("二值化", local);
imwrite("thresholded.jpg", local);

//计算凸包
cout << "计算凸包和轮廓....." << endl;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours(local, contours, hierarchy, CV_RETR_EXTERNAL, 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);

}
cout << "轮廓凸包绘制......" << endl;
/// 绘出轮廓及其凸包
Mat drawing = Mat::zeros(local.size(), CV_8UC3);
for (int i = 0; i < contours.size(); i++)
{
if (contourArea(contours[i]) < 500)//面积小于area的凸包，可忽略
continue;
result.push_back(hull[i]);
Scalar color = Scalar(0,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);
imwrite("contours.jpg", drawing);

//计算每一个凸包的位置和高度（也就是物体高度和位置）
cout << "计算物体位置....." << endl;
Point pt[100000];
Moments moment;//矩
vector<Vec3f>Center;//创建保存物体重心的向量
Vec3f Point3v;//三维坐标点
for (int i = 0; i >= 0; i = hierarchy[i][0])//读取每一个轮廓求取重心
{
Mat temp(contours.at(i));
Scalar color(0, 0, 255);
moment = moments(temp, false);
if (contourArea(contours[i]) < 500)//面积小于area的凸包，可忽略
continue;
if (moment.m00 != 0)//除数不能为0
{
pt[i].x = cvRound(moment.m10 / moment.m00);//计算重心横坐标
pt[i].y = cvRound(moment.m01 / moment.m00);//计算重心纵坐标
}
//重心坐标
Point3v = xyz.at<Vec3f>(pt[i].y, pt[i].x);
Center.push_back(Point3v);//将重心坐标保存到Center向量中
}

//统计物体高度
Point p1, p2;//分别是物体最高点和最低点的位置
float height,width;//物体高度
Vec3f point1,point2;//物体的最高点和最低点的实际高度
vector<float>all_height;
vector<float>all_width;
for (int i = 0; i < result.size(); i++)
{
sort(hull[i].begin(), hull[i].end(), sortRuleY);
p1 = hull[i][0];
p2 = hull[i][hull[i].size() - 1];
point1 = xyz.at<Vec3f>(p1.y, p1.x);
point2 = xyz.at<Vec3f>(p2.y, p2.x);
height = abs(point1[1] - point2[1]);

sort(hull[i].begin(), hull[i].end(), sortRuleX);
p1 = hull[i][0];
p2 = hull[i][hull[i].size() - 1];
point1 = xyz.at<Vec3f>(p1.y, p1.x);
point2 = xyz.at<Vec3f>(p2.y, p2.x);
width = abs(point1[0] - point2[0]);
all_height.push_back(height);
all_width.push_back(width);
}

//输出物体的位置和高度
if (all_height.size() == Center.size()&&all_height.size()!=0)
{
for (int i = 0; i < Center.size(); i++)
{
cout << "障碍物坐标：" << Center[i] << "  " << "障碍物高度：" << all_height[i] <<"障碍物宽度："<<all_width[i]<< endl;
}
}
else
{
cout << "位置和高度数量不一致或者大小全为0！" << endl;
}


11-27

09-12
10-27
10-18 1014
02-28 5939
06-25 1609
03-03
04-18
01-24
08-26 4286
07-26 2万+
11-14
©️2020 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客