Intel Realsense D435i图像采集与相机标定

1 intel realsense D435i图像采集

1.1 下载、安装sdk：

下载链接：https://www.intelrealsense.com/zh-hans/sdk-2/

预览工具：Intel.RealSense.Viewer.exe

1.2 读取左右泛光图——python：

a) 关掉散斑投射（realsense默认采集到的flood图带有散斑）

• 打开Intel RealSense Viewer软件 --Stereo Module --Emitter Enabled:laser改选off

b）打开双目泛光

• 打开Intel RealSense Viewer软件 --Stereo Module --Available Streams --选中Infraed1和Infrared2，不选Depth（如果只弹出一个Infraed1,是USB接触导致的）

c）帧率和分辨率设置

• 采集标定图时，把帧率设置为最小6fps，分辨率调整为最大：1280*720
  打开Intel RealSense Viewer软件 --Stereo Module --Frame Rate
  打开Intel RealSense Viewer软件 --Stereo Module --Resolution

d）程序获取泛光图

• 打开当前文件夹下read.py文件，运行文件，采集图像将分左、右保存在path=“./save/”
  注意：新采集数据会覆盖，代码如下：

# read.py

import pyrealsense2 as rs
import numpy as np
import cv2

# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)  # ���1280*720, ֡��6��15��25
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 6)
config.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 6)
config.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 6)

# config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# config.enable_stream(rs.stream.infrared, 1, 640, 480, rs.format.y8, 30)
# config.enable_stream(rs.stream.infrared, 2, 640, 480, rs.format.y8, 30)

# Start streaming
pipeline.start(config)
num = 0

try:
    while True:

        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        ir_frame_left = frames.get_infrared_frame(1)
        ir_frame_right = frames.get_infrared_frame(2)
        if not depth_frame or not color_frame:
            continue

        # Convert images to numpy arrays
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())
        ir_left_image = np.asanyarray(ir_frame_left.get_data())
        ir_right_image = np.asanyarray(ir_frame_right.get_data())

        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
                # Stack both images horizontally
        images1 = np.hstack((color_image, depth_colormap))
        images2 = np.hstack((ir_left_image, ir_right_image))
        # Show images
        cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
        cv2.imshow('RealSense', images1)
        cv2.imshow("Display pic_irt", images2)

        key = cv2.waitKey(1)
        # Press esc or 'q' to close the image window
        if key & 0xFF == ord('q') or key == 27:
            cv2.destroyAllWindows()
            break
        path = "./save/"
        cv2.imwrite(path+str(num)+"left.jpg", ir_left_image)
        cv2.imwrite(path+str(num)+"right.jpg", ir_right_image)
        num += 1
finally:
    # Stop streaming
    pipeline.stop()

1.3 读取左右泛光图——c++：

（1）无法#include，#include需要找到rs.hpp的位置：右键Intel RealSense Viewer图标，打开文件所在的位置；找到“C:\Program Files (x86)\Intel RealSense SDK 2.0\include”，配置到VS
（2）关于找不到cvGetWindowHandle()的解决方法:
添加上 #include
（3） LNK2001 无法解析的外部符号 rs2_create_pipeline：
找到库目录：添加“C:\Program Files (x86)\Intel RealSense SDK 2.0\lib\x64”
再添加链接器输入：realsense2.lib
（4）由于找不到realsense2.dll，无法继续执行代码。重新安装程序可能会解决此问题。
把：“C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64”路径下的realsense2.dll复制到”C:\Windows“下
获取代码如下：

#include 
#include 
#include 
#include 
#include 
#include
#include
#include
#include
#include
#include
#include

using namespace cv;
using namespace std;
using namespace rs2;

const int width = 1280;
const int height = 720;
const int fps = 30;
//const int fps = 60;



int main()
{
	//Initialization
	//Depth 
	const char* depth_win = "depth_Image";
	namedWindow(depth_win, WINDOW_AUTOSIZE);
	//IR Left & Right
	const char* left_win = "left_Image";
	namedWindow(left_win, WINDOW_AUTOSIZE);
	const char* right_win = "right_Image";
	namedWindow(right_win, WINDOW_AUTOSIZE);
	//Color
	const char* color_win = "color_Image";
	namedWindow(color_win, WINDOW_AUTOSIZE);

	char LName[100];//left 
	char RName[100];//right 
	char DName[100];//depth
	char CName[100];//color
	long long  i = 0;//counter

	//Pipeline
	rs2::pipeline pipe;
	rs2::config pipe_config;
	pipe_config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
	pipe_config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, fps);
	pipe_config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, fps);
	pipe_config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, fps);

	rs2::pipeline_profile profile = pipe.start(pipe_config);

	//stream
	auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
	
	//关于找不到cvGetWindowHandle()的解决方法:添加上 #include
	while (cvGetWindowHandle(depth_win) && cvGetWindowHandle(right_win) && cvGetWindowHandle(left_win) && cvGetWindowHandle(color_win)) // Application still alive?
	{
		//堵塞程序直到新的一帧捕获
		rs2::frameset frameset = pipe.wait_for_frames();
		//取深度图和彩色图
		frame depth_frame = frameset.get_depth_frame();
		video_frame ir_frame_left = frameset.get_infrared_frame(1);
		video_frame ir_frame_right = frameset.get_infrared_frame(2);
		frame color_frame = frameset.get_color_frame();

		Mat dMat_left(Size(width, height), CV_8UC1, (void*)ir_frame_left.get_data());
		Mat dMat_right(Size(width, height), CV_8UC1, (void*)ir_frame_right.get_data());
		Mat depth_image(Size(width, height), CV_16U, (void*)depth_frame.get_data(), Mat::AUTO_STEP);
		Mat color_image(Size(width, height), CV_8UC3, (void*)color_frame.get_data(), Mat::AUTO_STEP);

		imshow(left_win, dMat_left);
		imshow(right_win, dMat_right);
		imshow(depth_win, depth_image);
		imshow(color_win, color_image);
		/*waitKey(1);*/
		char c = waitKey(1);
		if (c == 'p')
		{
			sprintf_s(LName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\left_eye\\%d.png", i);
			imwrite(LName, dMat_left);
			sprintf_s(RName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\right_eye\\%d.png", i);
			imwrite(RName, dMat_right);
			sprintf_s(DName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\depth\\%d.png", i);
			imwrite(DName, depth_image);
			sprintf_s(CName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\color\\%d.png", i);
			imwrite(CName, color_image);
			i++;
		}
		else if (c == 'q')
			break;
		/*sprintf_s(LName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\left_eye\\%d.png", i);
		imwrite(LName, dMat_left);
		sprintf_s(RName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\right_eye\\%d.png", i);
		imwrite(RName, dMat_right);
		sprintf_s(DName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\depth\\%d.png", i);
		imwrite(DName, depth_image);
		sprintf_s(CName, "F:\\cpppractice\\D435iimshow\\x64\\Debug\\color\\%d.png", i);
		imwrite(CName, color_image);
		i++;*/
	}
	return 0;
}

2 intel realsense D435i相机标定

标定图、yml文件链接：https://download.csdn.net/download/weixin_41874898/14920719

#if 1//运行前改成1

/*
 需要修改的参数：
 1）w,h,chessboardSquareSize
 2）cal_filename,intri_filename,test_left_img,test_right_img
*/

#include 
#include 
#define  w  9      //棋盘格宽的黑白交叉点个数    
#define  h  6      //棋盘格高的黑白交叉点个数    

using namespace std;
using namespace cv;
const  float chessboardSquareSize = 22;  //每个棋盘格方块的边长 单位 为 mm

Mat rectifyImageL, rectifyImageR;
Mat Q;
Rect validROIL; //图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域  
Rect validROIR;


//从 xml 文件中读取图片存储路径 
static bool readStringList(const string& filename, vector<string>& list)
{
	list.resize(0);
	FileStorage fs(filename, FileStorage::READ);
	if (!fs.isOpened())
	 return false;
	FileNode n = fs.getFirstTopLevelNode();
	if (n.type() != FileNode::SEQ)
	 return false;
	FileNodeIterator it = n.begin(), it_end = n.end();
	for (; it != it_end; ++it)
	 list.push_back((string)*it);
	return true;
}

//记录棋盘格角点个数
static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
{
	corners.resize(0);
	for (int i = 0; i < boardSize.height; i++)        //height和width位置不能颠倒
		for (int j = 0; j < boardSize.width; j++)
		{
		 corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
		}
}

bool calibrate(Mat& intrMat, Mat& distCoeffs, vector<vector<Point2f>>& imagePoints,
 vector<vector<Point3f>>& ObjectPoints, Size& imageSize, const int cameraId,
 vector<string> imageList)
{

	double rms = 0;  //重投影误差
	
	Size boardSize;
	boardSize.width = w;
	boardSize.height = h;
	
	vector<Point2f> pointBuf;
	float squareSize = chessboardSquareSize;
	
	vector<Mat> rvecs, tvecs; //定义两个摄像头的旋转矩阵 和平移向量
	
	bool ok = false;
	
	int nImages = (int)imageList.size() / 2;
	cout << "图片张数" << nImages;
	namedWindow("View", 1);
	
	int nums = 0; //有效棋盘格图片张数
	
	for (int i = 0; i < nImages; i++)
	{
		 Mat view, viewGray;
		 view = imread(imageList[i * 2 + cameraId], 1); //读取图片
		 imageSize = view.size();
		 cvtColor(view, viewGray, COLOR_BGR2GRAY); //转化成灰度图
		
		 bool found = findChessboardCorners(view, boardSize, pointBuf,
		  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);  //寻找棋盘格角点
		 cout << "是否检测到棋盘格角点：" << found << endl;
		 if (found)
		 {
			  nums++;
			  cornerSubPix(viewGray, pointBuf, Size(11, 11),
			   Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1));
			  //Size(-1, -1), TermCriteria(cv::TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
			  drawChessboardCorners(view, boardSize, Mat(pointBuf), found);
			  bitwise_not(view, view);
			  imagePoints.push_back(pointBuf);
			  cout << '.';
		 }
		 imshow("View", view);
		 waitKey(1);
		}
	 cout << "有效棋盘格张数" << nums << endl;
	
	//calculate chessboardCorners
	calcChessboardCorners(boardSize, squareSize, ObjectPoints[0]);
	ObjectPoints.resize(imagePoints.size(), ObjectPoints[0]);
	
	rms = calibrateCamera(ObjectPoints, imagePoints, imageSize, intrMat, distCoeffs,
	 rvecs, tvecs);
	ok = checkRange(intrMat) && checkRange(distCoeffs);
	
	if (ok)
	{
		 cout << "done with RMS error=" << rms << endl;
		 return true;
	}
	else
		return false;
}


int main()
{
	//init
	string cal_filename = "stereo_calibration.xml";  //输入：标定图文件名提前写入该xml文件中，需要在xml中去修改
	string intri_filename = "intrinsics.yml";  //输出：存放相机内参
	
	string test_left_img = "./pic_test/13left.jpg";  //输入：用于立体校正测试的图片
	string test_right_img = "./pic_test/13right.jpg";
	
	string rectify_L_filename = "rectify_L.jpg";  //输出：保存立体校正后的图片
	string rectify_R_filename = "rectify_R.jpg";
	
	bool okcalib = false;
	Mat intrMatFirst, intrMatSec, distCoeffsFirst, distCoffesSec;
	Mat R, T, E, F, RFirst, RSec, PFirst, PSec;
	vector<vector<Point2f>> imagePointsFirst, imagePointsSec;
	vector<vector<Point3f>> ObjectPoints(1);
	Rect validRoi[2];
	Size imageSize;
	int cameraIdFirst = 0, cameraIdSec = 1;
	double rms = 0;
	
	//get pictures and calibrate
	vector<string> imageList;
	bool okread = readStringList(cal_filename, imageList);
	if (!okread || imageList.empty())
	{
		cout << "can not open " << cal_filename << " or the string list is empty" << endl;
		return false;
	}
	 if (imageList.size() % 2 != 0)
	{
		cout << "Error: the image list contains odd (non-even) number of elements\n";
		return false;
	}
	
	//calibrate left camera
	cout << "calibrate left camera..." << endl;
	okcalib = calibrate(intrMatFirst, distCoeffsFirst, imagePointsFirst, ObjectPoints,
	 imageSize, cameraIdFirst, imageList);
	
	if (!okcalib)
	{
		cout << "fail to calibrate left camera" << endl;
		return -1;
	}
	else
	{
		cout << "calibrate right camera..." << endl;
	}
	
	//calibrate right camera
	okcalib = calibrate(intrMatSec, distCoffesSec, imagePointsSec, ObjectPoints,
	 imageSize, cameraIdSec, imageList);
	
	if (!okcalib)
	{
		cout << "fail to calibrate the right camera" << endl;
		return -1;
	}
	destroyAllWindows();
	
	//estimate position and orientation
	cout << "estimate position and orientation of the second camera" << endl
	 << "relative to the first camera..." << endl;
	cout << intrMatFirst;
	cout << distCoeffsFirst;
	cout << intrMatSec;
	cout << distCoffesSec;
	cout << "M1 before cali" << intrMatFirst << endl;
	rms = stereoCalibrate(ObjectPoints, imagePointsFirst, imagePointsSec,
	 intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec,
	 imageSize, R, T, E, F, CALIB_USE_INTRINSIC_GUESS,//CV_CALIB_FIX_INTRINSIC,
	 TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 1e-6));          //计算重投影误差
	cout << "done with RMS error=" << rms << endl;
	cout << "R" << R;
	
	//stereo rectify
	cout << "stereo rectify..." << endl;
	{
		cout << "M1" << intrMatFirst << endl << "D1" << distCoeffsFirst << endl << "M2" << intrMatSec << endl << "D2" << distCoffesSec << endl;
	
	}
	FileStorage fs(intri_filename, FileStorage::WRITE);
	fs << "M1" << intrMatFirst << "D1" << distCoeffsFirst <<
	 "M2" << intrMatSec << "D2" << distCoffesSec;
	stereoRectify(intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec, imageSize, R, T, RFirst,
	 RSec, PFirst, PSec, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &validROIL, &validROIR);
	
	if (fs.isOpened())
	{
		cout << "in";
		fs << "R" << R << "T" << T << "R1" << RFirst << "R2" << RSec << "P1" << PFirst << "P2" << PSec << "Q" << Q;
		fs.release();
	}
	
	namedWindow("canvas", 1);
	cout << "read the picture for 3d-reconstruction...";
	Mat canvas(imageSize.height, imageSize.width * 2, CV_8UC3), viewLeft, viewRight;
	Mat canLeft = canvas(Rect(0, 0, imageSize.width, imageSize.height));
	Mat canRight = canvas(Rect(imageSize.width, 0, imageSize.width, imageSize.height));
	viewLeft = imread(test_left_img, 1);//cameraIdFirst
	viewRight = imread(test_right_img, 1); //cameraIdSec
	viewLeft.copyTo(canLeft);
	viewRight.copyTo(canRight);
	cout << "done" << endl;
	cv::Size canvas_size = cv::Size((int)canvas.cols*0.5, (int)canvas.rows * 0.5);  //cols不是col
	cv::Mat dst_canvas;
	cv::resize(canvas, dst_canvas, canvas_size);
	imshow("校正前", dst_canvas);
	waitKey(40); //必须要加waitKey ，否则可能存在无法显示图像问题
	cout << "size" << imageSize;
	 //stereoRectify
	Mat rmapFirst[2], rmapSec[2], rviewFirst, rviewSec;
	initUndistortRectifyMap(intrMatFirst, distCoeffsFirst, RFirst, PFirst,
	 imageSize, CV_16SC2, rmapFirst[0], rmapFirst[1]);//CV_16SC2
	initUndistortRectifyMap(intrMatSec, distCoffesSec, RSec, PSec,//CV_16SC2
	 imageSize, CV_16SC2, rmapSec[0], rmapSec[1]);
	
	clock_t t1 = clock();
	remap(viewLeft, rectifyImageL, rmapFirst[0], rmapFirst[1], INTER_LINEAR);
	remap(viewRight, rectifyImageR, rmapSec[0], rmapSec[1], INTER_LINEAR);
	clock_t t2 = clock();
	cout << "t2 - t1: " << t2 - t1 << endl;
	
	imwrite(rectify_L_filename, rectifyImageL);  //存储校正后的图片
	imwrite(rectify_R_filename, rectifyImageR);
	rectifyImageL.copyTo(canLeft);
	rectifyImageR.copyTo(canRight);
	cv::resize(canvas, dst_canvas, canvas_size);
	imshow("校正后", dst_canvas);
	cvtColor(rectifyImageL, rectifyImageL, cv::COLOR_BGR2GRAY);
	cvtColor(rectifyImageR, rectifyImageR, cv::COLOR_BGR2GRAY);
	waitKey(500);
	
	for (int j = 0; j <= canvas.rows; j += 50)  //画绿线
		line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
	
	cv::resize(canvas, dst_canvas, canvas_size);
	imshow("校正后", dst_canvas); //显示画绿线的校正前图像
	cv::waitKey(0);
	
	return 0;
}
#endif