双目矫正+测距+深度图+点云
因为只有一个视角的深度图，所以生成的点云图很粗糙只有个轮廓，顶多算是稀疏原始点云。还需要后期点云滤波、多点云拼接。至于为什么会有对称的两个轮廓，我觉得可能生成了左右两个视角的点云图，还有我的相机参数不完全匹配，导致两个分离了没有融合，有时间再研究研究。
/******************************/
/* 立体匹配和测距 */
/******************************/
#include (3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量，对应matlab om参数 我
Mat rec = (Mat_<double>(3, 3) << 0.999996782379644, 5.706428486117677e-04, -0.002471759919034,
-5.720218896103840e-04, 0.999999681132956, -5.572476509248812e-04,
0.002471441141484, 5.586597586930527e-04, 0.999996789933827); //rec旋转向量，对应matlab om参数 我
/* 0.999341122700880 0.000660748031451783 -0.0362888948713456
-0.00206388651740061 0.999250989651683 -0.0386419468010579
0.0362361815232777 0.0386913826603732 0.998593969567432 */
//Mat T = (Mat_(3, 1) << -48.4, 0.241, -0.0344);//T平移向量
//[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
//对应Matlab所得T参数
//Mat rec = (Mat_(3, 1) << -0.039, -0.04658, 0.00106);//rec旋转向量，对应matlab om参数 倬华
Mat R;//R 旋转矩阵
Mat frame, frame_L, frame_R;
static void saveXYZ(const char* filename, const Mat& mat)
{
const double max_z = 1.0e4;
FILE* fp = fopen(filename, "wt");
for (int y = 0; y < mat.rows; y++)
{
for (int x = 0; x < mat.cols; x++)
{
Vec3f point = mat.at<Vec3f>(y, x);
if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
}
}
fclose(fp);
}
void viewerOneOff(visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor(0.0, 0.0, 0.0);
}
/*****立体匹配*****/
void stereo_match(int, void*)
{
bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小，5~21之间为宜
bm->setROI1(validROIL);
bm->setROI2(validROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0); //最小视差，默认值为0, 可以是负值，int型
bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
Mat disp;
bm->compute(grayImageL, grayImageR, disp);//输入图像必须为灰度图
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16) * 16.));//计算出的视差是CV_16S格式
reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
xyz = xyz * 16;
//saveXYZ("point_cloud.txt", xyz);
imshow("disparity", disp8);
}
/*****描述：鼠标操作回调*****/
static void onMouse(int event, int x, int y, int, void*)
{
if (selectObject)
{
selection.x = MIN(x, origin.x);
selection.y = MIN(y, origin.y);
selection.width = std::abs(x - origin.x);
selection.height = std::abs(y - origin.y);
}
switch (event)
{
case EVENT_LBUTTONDOWN: //鼠标左按钮按下的事件
origin = Point(x, y);
selection = Rect(x, y, 0, 0);
selectObject = true;
//cout << origin << "in world coordinate is: " << xyz.at(origin) << endl;
point3 = xyz.at<Vec3f>(origin);
point3[0];
//cout << "point3[0]:" << point3[0] << "point3[1]:" << point3[1] << "point3[2]:" << point3[2]<
cout << "世界坐标：" << endl;
cout << "x: " << point3[0] << " y: " << point3[1] << " z: " << point3[2] << endl;
d = point3[0] * point3[0] + point3[1] * point3[1] + point3[2] * point3[2];
d = sqrt(d); //mm
// cout << "距离是:" << d << "mm" << endl;
d = d / 10.0; //cm
cout << "距离是:" << d << "cm" << endl;
// d = d/1000.0; //m
// cout << "距离是:" << d << "m" << endl;
break;
case EVENT_LBUTTONUP: //鼠标左按钮释放的事件
selectObject = false;
if (selection.width > 0 && selection.height > 0)
break;
}
}
/*****主函数*****/
int main()
{
/*
立体校正
*/
Rodrigues(rec, R); //Rodrigues变换
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
0, imageSize, &validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
/*
打开摄像头
*/
VideoCapture cap;
cap.open(1); //打开相机，电脑自带摄像头一般编号为0，外接摄像头编号为1，主要是在设备管理器中查看自己摄像头的编号。
cap.set(CAP_PROP_FRAME_WIDTH, 640*2); //设置捕获视频的宽度
cap.set(CAP_PROP_FRAME_HEIGHT, 240*2); //设置捕获视频的高度
cap.set(CAP_PROP_FPS,15);
if (!cap.isOpened()) //判断是否成功打开相机
{
cout << "摄像头打开失败!" << endl;
return -1;
}
cout << "Painted ImageL" << endl;
cout << "Painted ImageR" << endl;
while (1) {
cap >> frame; //从相机捕获一帧图像
double fScale = 1; //定义缩放系数，对2560*720图像进行缩放显示（2560*720图像过大，液晶屏分辨率较小时，需要缩放才可完整显示在屏幕）
Size dsize = Size(frame.cols * fScale, frame.rows * fScale);
Mat imagedst = Mat(dsize, CV_32S);
resize(frame, imagedst, dsize);
char image_left[200];
char image_right[200];
frame_L = imagedst(Rect(0,0,320*2,240*2)); //获取缩放后左Camera的图像
// namedWindow("Video_L", 1);
imshow("Video_L", frame_L);
frame_R = imagedst(Rect(320*2, 0, 320*2, 240*2)); //获取缩放后右Camera的图像
// namedWindow("Video_R", 2);
imshow("Video_R", frame_R);
//cap >> frame;
/*
读取图片
*/
//rgbImageL = imread("image_left_1.jpg", CV_LOAD_IMAGE_COLOR);
//rgbImageR = imread("image_right_1.jpg", CV_LOAD_IMAGE_COLOR);
/*
经过remap之后，左右相机的图像已经共面并且行对准了
*/
remap(frame_L, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
remap(frame_R, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
cvtColor(rectifyImageL, grayImageL, COLOR_BGR2GRAY);
cvtColor(rectifyImageR, grayImageR, COLOR_BGR2GRAY);
imshow("Rectify grayImageL", grayImageL);
imshow("Rectify grayImageR", grayImageR);
//imshow("rectifyImageL", rectifyImageL);
//imshow("rectifyImageR", rectifyImageR);
/*
把校正结果显示出来
*/
//Mat rgbRectifyImageL, rgbRectifyImageR;
//cvtColor(rectifyImageL, rgbRectifyImageL, COLOR_GRAY2BGR); //伪彩色图
//cvtColor(rectifyImageR, rgbRectifyImageR, COLOR_GRAY2BGR);
//单独显示
rectangle(rectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
rectangle(rectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
imshow("ImageL After Rectify", rectifyImageL);
imshow("ImageR After Rectify", rectifyImageR);
//显示在同一张图上
Mat canvas;
double sf;
int w, h;
sf = 300. / MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width * sf);
h = cvRound(imageSize.height * sf);
canvas.create(h, w * 2, CV_8UC3); //注意通道
//左图像画到画布上
Mat canvasPart = canvas(Rect(w * 0, 0, w, h)); //得到画布的一部分
resize(rectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); //把图像缩放到跟canvasPart一样大小
Rect vroiL(cvRound(validROIL.x * sf), cvRound(validROIL.y * sf), //获得被截取的区域
cvRound(validROIL.width * sf), cvRound(validROIL.height * sf));
//rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8); //画上一个矩形
// cout << "Painted ImageL" << endl;
//右图像画到画布上
canvasPart = canvas(Rect(w, 0, w, h)); //获得画布的另一部分
resize(rectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y * sf),
cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
//rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
// cout << "Painted ImageR" << endl;
//画上对应的线条
for (int i = 0; i < canvas.rows; i += 16)
line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);
/*
立体匹配
*/
namedWindow("disparity", WINDOW_AUTOSIZE);
// 创建SAD窗口 Trackbar
createTrackbar("BlockSize:\n", "disparity", &blockSize, 18, stereo_match);
// 创建视差唯一性百分比窗口 Trackbar
createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 30, stereo_match);
// 创建视差窗口 Trackbar
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
//鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
setMouseCallback("disparity", onMouse, 0);
stereo_match(0, 0);
//saveXYZ("point_cloud.txt", xyz);
//生成点云
//if (waitKey(0) != 32) break;
if(waitKey(10)==32)
{
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
double fx = 2.762165790037839e+02*2, fy = 2.762317738515432e+02*2, u0 = 1.765880468329375e+02*2, v0 = 1.272320444598781e+02*2;
// 间距
double baseline = 65; // (注意此处的间距为双目相机左右光圈的间距)
double doffs = 4.1; // 代表两个相机主点在x方向上的差距, doffs = |u1 - u0|
// 相机坐标系下的点云
PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
for (int row = 0; row < disp8.rows; row++)
{
for (int col = 0; col < disp8.cols; col++)
{
ushort d = disp8.ptr<ushort>(row)[col];
if (d == 0)
continue;
PointXYZRGB p;
// depth
p.z = fx * baseline / (d + doffs); // Zc = baseline * f / (d + doffs)
p.x = (col - u0) * p.z / fx; // Xc向右，Yc向下为正
p.y = (row - v0) * p.z / fy;
p.y = -p.y; // 为便于显示，绕x轴三维旋转180°
p.z = -p.z;
// RGB
p.b = rectifyImageL.ptr<uchar>(row)[col * 3];
p.g = rectifyImageL.ptr<uchar>(row)[col * 3 + 1];
p.r = rectifyImageL.ptr<uchar>(row)[col * 3 + 2];
cloud->points.push_back(p);
}
}
cloud->height = disp8.rows;
cloud->width = disp8.cols;
cloud->points.resize(cloud->height * cloud->width);
visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(cloud);
viewer.runOnVisualizationThreadOnce(viewerOneOff);
while (!viewer.wasStopped())
{
int user_data = 9;
}
}
if (waitKey(10) == 27) break;
} //wheil
return 0;
}