CHUSEI 3D Webcam双目图像、视差图以及点云显示

环境：Ubuntu系统+opencv+pcl
需要安装uvcdynctrl，执行如下命令

sudo apt-get install uvcdynctrl

安装完成后并把uvcdynctrl命令写成shell脚本来实现四个模式的切换。
下面的脚本将会在cpp程序中调用，不需要自己手动执行。

camera.sh

uvcdynctrl -d /dev/video0 -S 6:8  '(LE)0x50ff'
uvcdynctrl -d /dev/video0 -S 6:15 '(LE)0x00f6'
uvcdynctrl -d /dev/video0 -S 6:8  '(LE)0x2500'
uvcdynctrl -d /dev/video0 -S 6:8  '(LE)0x5ffe'
uvcdynctrl -d /dev/video0 -S 6:15 '(LE)0x0003'
uvcdynctrl -d /dev/video0 -S 6:15 '(LE)0x0002'
uvcdynctrl -d /dev/video0 -S 6:15 '(LE)0x0012'
uvcdynctrl -d /dev/video0 -S 6:15 '(LE)0x0004'
uvcdynctrl -d /dev/video0 -S 6:8  '(LE)0x76c3'
uvcdynctrl -d /dev/video0 -S 6:10 '(LE)0x0400'

camera.cpp

#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/io/io.h>  
#include <pcl/visualization/cloud_viewer.h>


int main()
{
	cv::VideoCapture Camera(0);
	if (!Camera.isOpened())
	{
		cout << "Could not open the Camera " << endl;
		return -1;
	}

	cv::Mat Frame, DoubleImage;
	Camera >> Frame;
	cv::namedWindow("DoubleImage");
	cv::namedWindow("Disparity");

	system("./camera.sh");  //脚本存放路径

	char key = 0;
	pcl::visualization::CloudViewer viewer("Cloud Viewer");

	while (key != 32)
	{
		//按ESC退出
		if (viewer.wasStopped())
		{
			cv::destroyWindow("DoubleImage");
			cv::destroyWindow("Disparity");
			break;
		}

		Camera >> Frame;
		if (Frame.empty()) break;

		resize(Frame, DoubleImage, cv::Size(640, 240), (0, 0), (0, 0), cv::INTER_AREA);
		cv::imshow("DoubleImage", DoubleImage);
		cv::Mat	LeftImage = DoubleImage(cv::Rect(0, 0, 320, 240));
		cv::Mat RightImage = DoubleImage(cv::Rect(320, 0, 320, 240));

		cv::Mat intrMatFirst= (cv::Mat_<double>(3, 3) << 2.7647846653266811e+02, 0., 1.3658322295813980e+02, 0.,
			2.7681170126978412e+02, 1.3919490598437602e+02, 0., 0., 1.);
		cv::Mat intrMatSec = (cv::Mat_<double>(3, 3) << 3.2557485062902185e+02, 0., 1.3714262678280906e+02, 0.,
			3.2504070414475200e+02, 1.1438651517732795e+02, 0., 0., 1.);
		cv::Mat distCoeffsFirst = (cv::Mat_<double>(5, 1) << 4.7359935063009624e-02, -2.5971469469815223e-01,
			2.7463835292016435e-04, 2.4313004134302986e-03, 3.4473649080807156e-01);
		cv::Mat distCoffesSec = (cv::Mat_<double>(5, 1) << 1.0651786710439973e-01, -1.9203536317765910e+00,
			-3.3360259626232986e-03, 2.9862284984416717e-04, 8.5437903440274656e+00);
		cv::Mat R = (cv::Mat_<double>(3, 3) << 9.9995813490498642e-01, 1.0678546960460038e-03, 9.0878008169866884e-03,
			-1.1670590644186746e-03, 9.9993971667992876e-01, 1.0917920095149152e-02,
			-9.0755942219366333e-03, -1.0928069015706110e-02, 9.9989910035818452e-01);
		cv::Mat T = (cv::Mat_<double>(3, 1) << -3.9375682530226491e+01, -2.0271483032372781e-02, 4.1437635734962114e-01);
		cv::Mat Q = (cv::Mat_<double>(4, 4) << 1., 0., 0., -1.5396846389770508e+02, 0., 1., 0.,
			-1.2518226909637451e+02, 0., 0., 0., 3.0515958394504935e+02, 0.,
			0., 2.5394975632634070e-02, 0.);
		cv::Mat RFirst = (cv::Mat_<double>(3, 3) << 9.9999753798854729e-01, 1.6975551296879428e-03, -1.4290988158594921e-03,
			-1.6897306595994685e-03, 9.9998367397319954e-01, 5.4586259589697950e-03,
			1.4383418028516221e-03, -5.4561977276854157e-03, 9.9998408041289077e-01);
		cv::Mat RSec = (cv::Mat_<double>(3, 3) << 9.9994449837343669e-01, 5.1479381764392407e-04, -1.0523077497630422e-02,
			-5.7221575119432186e-04, 9.9998496049484742e-01, -5.4544801083754356e-03,
			1.0520111303113922e-02, 5.4601988465525463e-03, 9.9992975427613229e-01);
		cv::Mat PFirst = (cv::Mat_<double>(3, 4) << 3.0515958394504935e+02, 0., 1.5396846389770508e+02, 0., 0.,
			3.0515958394504935e+02, 1.2518226909637451e+02, 0., 0., 0., 1., 0.);
		cv::Mat PSec = (cv::Mat_<double>(3, 4) << 3.0515958394504935e+02, 0., 1.5396846389770508e+02,
			-1.2016533835649794e+04, 0., 3.0515958394504935e+02,
			1.2518226909637451e+02, 0., 0., 0., 1., 0.);

		stereoRectify(intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec, cv::Size(320, 240), R, T, RFirst,
			RSec, PFirst, PSec, Q, cv::CALIB_ZERO_DISPARITY, -1, cv::Size(320, 240));

		//stereoRectify
		cv::Mat rmapFirst[2], rmapSec[2], rviewFirst, rviewSec;
		initUndistortRectifyMap(intrMatFirst, distCoeffsFirst, RFirst, PFirst,
			cv::Size(320, 240), CV_16SC2, rmapFirst[0], rmapFirst[1]);		initUndistortRectifyMap(intrMatSec, distCoffesSec, RSec, PSec,
			cv::Size(320, 240), CV_16SC2, rmapSec[0], rmapSec[1]);

		cv::Mat rectifyImageL, rectifyImageR;
		remap(LeftImage, rectifyImageL, rmapFirst[0], rmapFirst[1], cv::INTER_LINEAR);
		remap(RightImage, rectifyImageR, rmapSec[0], rmapSec[1], cv::INTER_LINEAR);

		cv::cvtColor(rectifyImageL, rectifyImageL, cv::COLOR_BGR2GRAY);
		cv::cvtColor(rectifyImageR, rectifyImageR, cv::COLOR_BGR2GRAY);

		cv::Ptr<cv::StereoBM> bm = cv::StereoBM::create(16, 9);
		int blockSize = 1, uniquenessRatio = 0, numDisparities = 5;

		bm->setBlockSize(2 * blockSize + 5);     //SAD窗口大小，5~21之间为宜
		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);
		cv::Mat disp, disp8, xyz;
		bm->compute(rectifyImageL, rectifyImageR, 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 *= 16;
		cv::imshow("Disparity", disp8);

		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

		int rowNumber = LeftImage.rows;
		int colNumber = LeftImage.cols;

		cloud->height = rowNumber;
		cloud->width = colNumber;
		cloud->points.resize(cloud->width * cloud->height);

		for (unsigned int u = 0; u < rowNumber; ++u)
		{
			for (unsigned int v = 0; v < colNumber; ++v)
			{
				unsigned int num = u * colNumber + v;

				cloud->points[num].b = LeftImage.at<cv::Vec3b>(u, v)[0];
				cloud->points[num].g = LeftImage.at<cv::Vec3b>(u, v)[1];
				cloud->points[num].r = LeftImage.at<cv::Vec3b>(u, v)[2];
				cloud->points[num].x = u; //xyz.at<Vec3f>(u, v)[0];
				cloud->points[num].y = v;//xyz.at<Vec3f>(u, v)[1];
				cloud->points[num].z = xyz.at<cv::Vec3f>(u, v)[2];
			}
		}
		viewer.showCloud(cloud);

		char key = cv::waitKey(100);
	}
	return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(camera)

# 添加opencv库
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS} ${OpenCV2_INCLUDE_DIRS})
link_directories(${OpenCV_LIBRARY_DIRS})

# 添加pcl库
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})

add_executable(camera  camera.cpp)
target_link_libraries(camera  ${OpenCV_LIBRARIES} ${PCL_LIBRARIES})

最后效果图很一般，应该是这个摄像头质量太差了。