How should I reconstruct pointcloud by .raw depth images and color images

[Li-Yidong]

• Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view):

  • Consider checking out SDK examples.
  • Have you looked in our documentations?
  • Is you question a frequently asked one?
  • Try searching our GitHub Issues (open and closed) for a similar issue.

• All users are welcomed to report bugs, ask questions, suggest or request enhancements and generally feel free to open new issue, even if they haven't followed any of the suggestions above :)

---

Required Info
Camera Model	{ R200 / F200 / SR300 / ZR300 / D400 }
Firmware Version	(Open RealSense Viewer --> Click info)
Operating System & Version	{Win (8.1/10) / Linux (Ubuntu 14/16/17) / MacOS
Kernel Version (Linux Only)	(e.g. 4.14.13)
Platform	PC/Raspberry Pi/ NVIDIA Jetson / etc..
SDK Version	{ legacy / 2.. }
Language	{C/C#/labview/nodejs/opencv/pcl/python/unity }
Segment	{Robot/Smartphone/VR/AR/others }

Issue Description

Hi, I saved .raw format depth images and color images from realsense viewer, then use rs2::software_device to reconstruct pointclouds. The code below was working well on .png format depth images, so mabey when I reading .raw depth images, something happend? Could anyone help me about this issue?

Read raw depth image function:

cv::Mat read_raw(std::string file_path, int w, int h)
{
	FILE* fp = NULL; 
	fp = fopen(file_path.c_str(), "rb+");
	unsigned char* data = (unsigned char*)malloc(w * h * sizeof(unsigned char)); 
	fread(data, sizeof(unsigned char), w * h, fp); 
	cv::Mat img;
	int bufLen = w * h; 
	img.create(h, w, CV_16UC1);
	memcpy(img.data, data, bufLen * sizeof(unsigned char)); 

	return img;
}

Reconstruct function:

std::vector<cloud_pointer> RealsenseUtils::ReconstructPointCloud(
	std::vector<cv::Mat> color_imgs, 
	std::vector<cv::Mat> depth_imgs,
	cam_params color_params,
	cam_params depth_params)
{
	rs2::software_device dev;

	auto depth_sensor = dev.add_sensor("Depth");
	auto color_sensor = dev.add_sensor("Color");

	rs2_intrinsics color_intrinsics{ color_params.W, color_params.H,
									color_params.cx, color_params.cy, 
									color_params.fx, color_params.fy,
									RS2_DISTORTION_INVERSE_BROWN_CONRADY,
									{0, 0, 0, 0, 0} };
	rs2_intrinsics depth_intrinsics{ depth_params.W, depth_params.H,
									depth_params.cx, depth_params.cy, 
									depth_params.fx, depth_params.fy,
									RS2_DISTORTION_BROWN_CONRADY,
									{0, 0, 0, 0, 0} };

	auto depth_stream = depth_sensor.add_video_stream(
		{ RS2_STREAM_DEPTH, 0, 0, 
		(int)depth_params.W, (int)depth_params.H, 
		60, 2, RS2_FORMAT_Z16,
		 depth_intrinsics });
	auto color_stream = color_sensor.add_video_stream(
		{ RS2_STREAM_COLOR, 0, 1, 
		(int)color_params.W, (int)color_params.H, 
		60, 3, RS2_FORMAT_BGR8,
		 color_intrinsics });

	depth_sensor.add_read_only_option(RS2_OPTION_DEPTH_UNITS, 0.001f);
	depth_sensor.add_read_only_option(RS2_OPTION_STEREO_BASELINE, 0.001f);

	depth_stream.register_extrinsics_to(color_stream, { { 1,0,0,0,1,0,0,0,1 },{ 0,0,0 } });

	dev.create_matcher(RS2_MATCHER_DEFAULT);
	rs2::syncer sync;

	depth_sensor.open(depth_stream);
	color_sensor.open(color_stream);

	depth_sensor.start(sync);
	color_sensor.start(sync);

	rs2::align align(RS2_STREAM_DEPTH);

	rs2::frameset fs;
	rs2::frame depth_frame;
	rs2::frame color_frame;

	cv::Mat color_image;
	cv::Mat depth_image;

	int frame = 0;

	std::vector<cloud_pointer> clouds;
	//inject the images
	for (int idx = 0; idx < color_imgs.size() + 1; ++idx) {
		if (idx == 0)
		{
			color_image = cv::Mat::zeros(color_imgs[0].size(), color_imgs[0].type());
			depth_image = cv::Mat::zeros(depth_imgs[0].size(), depth_imgs[0].type());
		}
		else
		{
			color_image = color_imgs[frame].clone();
			depth_image = depth_imgs[frame].clone();

			frame++;
		}

		color_sensor.on_video_frame({ (void*)color_image.data, // Frame pixels from capture API
									 [](void*) {}, // Custom deleter (if required)
									 3 * (int)color_params.W, 3, // Stride and Bytes-per-pixel
									 double(idx * 16), RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME,
									 idx, // Timestamp, Frame# for potential sync services
									 color_stream });
		depth_sensor.on_video_frame({ (void*)depth_image.data, // Frame pixels from capture API
									 [](void*) {}, // Custom deleter (if required)
									 2 * (int)depth_params.W, 2, // Stride and Bytes-per-pixel
									 double(idx * 16), RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME,
									 idx, // Timestamp, Frame# for potential sync services
									 depth_stream });

		fs = sync.wait_for_frames();
		if (fs.size() == 2) {
			fs = align.process(fs);
			rs2::frame depth_frame = fs.get_depth_frame();
			rs2::frame color_frame = fs.get_color_frame();

			rs2::pointcloud pc;
			pc.map_to(color_frame);
			rs2::points points = pc.calculate(depth_frame);

			// Convert generated Point Cloud to PCL Formatting
			cloud_pointer cloud = RealsenseUtils::PCL_Conversion(points, color_frame);

			clouds.push_back(cloud);
		}
	}

	return clouds;
}

main function:

int main()
{
	std::string rgb_dir = R"(D:\Work_Files\DMSDev\AutoMeasurementSeg\BlankStage\rgb_image)";
	//std::string rgb_dir = R"(D:\Work_Files\DMSDev\AutoMeasurementSeg\SAMRealSense\rgb_frames_sub)";
	std::string depth_dir = R"(D:\Work_Files\DMSDev\AutoMeasurementSeg\BlankStage\depth_image)";
	//std::string depth_dir = R"(D:\Work_Files\DMSDev\AutoMeasurementSeg\SAMRealSense\depth_frames_sub)";
	std::string mask_dir = R"(D:\Work_Files\DMSDev\AutoMeasurementSeg\SAMRealSense\DJ06\mask)";
	std::string save_dir = R"(D:\Work_Files\DMSDev\AutoMeasurementSeg\SAMRealSense\3Dmodels\)";

	// 图片路径
	std::vector<std::string> rgb_path, depth_path, mask_path;
	getFiles(rgb_dir, rgb_path);
	getFiles(depth_dir, depth_path);
	getFiles(mask_dir, mask_path);

	// 图片名
	std::vector<std::string> rgb_names = getFilenames(rgb_dir);
	std::vector<std::string> depth_names = getFilenames(depth_dir);
	std::vector<std::string> mask_names = getFilenames(mask_dir);

	cv::Mat depth_image = read_raw(depth_path[0], 420, 240);
	//cv::Mat depth_image = cv::imread(depth_path[0], cv::IMREAD_UNCHANGED);
	cv::Mat color_image = cv::imread(rgb_path[0], cv::IMREAD_COLOR);
	cv::Mat mask_image;

	cv::Mat Ellipse = cv::Mat::zeros(color_image.size(), CV_8UC3);
	cv::ellipse(
		Ellipse,
		cv::Point(623.444, 353.994),
		cv::Size(315.598, 260.41),
		RadToDeg(1.55576), 0, 360,
		cv::Scalar(255, 255, 255), -1);
	cv::Mat Ellipse2;
	cv::resize(Ellipse, Ellipse2, depth_image.size());
	cv::cvtColor(Ellipse2, Ellipse2, cv::COLOR_BGR2GRAY);

	std::vector<cv::Mat> color_imgs;
	std::vector<cv::Mat> depth_imgs;
	for (int idx = 0; idx < depth_path.size(); idx++)
	{
		depth_image = read_raw(depth_path[idx], 420, 240);
		//depth_image = cv::imread(depth_path[idx], cv::IMREAD_UNCHANGED);
		color_image = cv::imread(rgb_path[idx], cv::IMREAD_COLOR);
		mask_image = cv::imread(mask_path[idx], 0);

		// Preprocess
		cv::Mat sub_color_image;
		cv::Mat sub_depth_image;
		color_image.copyTo(sub_color_image, Ellipse);
		depth_image.copyTo(sub_depth_image, Ellipse2);

		color_imgs.push_back(sub_color_image);
		depth_imgs.push_back(sub_depth_image);
		//cv::Mat masked_color_image;
		//bool result = MakeMaskedImage(sub_color_image, mask_image, masked_color_image);
		/*if (!result)
			continue;*/

	}

	RealsenseUtils::cam_params color_params;
	RealsenseUtils::cam_params depth_params;
	depth_params.cx = 210.830948;
	depth_params.cy = 118.059273;
	depth_params.fx = 215.65152;
	depth_params.fy = 215.65152;
	depth_params.W = 420;
	depth_params.H = 240;

	std::vector<cloud_pointer> clouds = RealsenseUtils::ReconstructPointCloud(
		color_imgs, depth_imgs, color_params, depth_params);

	for (int idx = 0; idx < clouds.size(); idx++)
	{
		pcl::io::savePLYFile(save_dir + depth_names[idx] + ".ply", *clouds[idx]);
		
	}

	return 1;
}

[MartyG-RealSense]

Hi @Li-Yidong There are not many references available about reading depth from a .raw file with C++ code. #2231 (comment) and #2200 (comment) may be helpful discussions if you have not seen them already.

#2231 (comment) provides advice about aligning a depth .raw and a color .png by using software_device.