librealsense2 sample

CMakeLists.txt

cmake_minimum_required(VERSION 3.1.0)
project(sample_librealsense2)

# Find librealsense2 installed package
find_package(realsense2 REQUIRED)
find_package(OpenCV REQUIRED)

# Enable C++11
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED TRUE)
set(CMAKE_CXX_EXTENSIONS OFF)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANGCXX)
	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
endif()

if(OpenCV_FOUND)
	include_directories(${OpenCV_INCLUDE_DIRS})
	add_executable(${PROJECT_NAME} sample_librealsense2.cpp)
	target_link_libraries(${PROJECT_NAME} ${realsense2_LIBRARY} ${OpenCV_LIBS})
endif(OpenCV_FOUND)

cv-helpers.hpp

#pragma once

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <opencv2/opencv.hpp>   // Include OpenCV API
#include <exception>

// Convert rs2::frame to cv::Mat
cv::Mat frame_to_mat(const rs2::frame& f)
{
	using namespace cv;
	using namespace rs2;

	auto vf = f.as<video_frame>();
	const int w = vf.get_width();
	const int h = vf.get_height();

	if (f.get_profile().format() == RS2_FORMAT_BGR8)
	{
		return Mat(Size(w, h), CV_8UC3, (void*)f.get_data(), Mat::AUTO_STEP);
	}
	else if (f.get_profile().format() == RS2_FORMAT_RGB8)
	{
		auto r = Mat(Size(w, h), CV_8UC3, (void*)f.get_data(), Mat::AUTO_STEP);
		cvtColor(r, r, COLOR_RGB2BGR);
		return r;
	}
	else if (f.get_profile().format() == RS2_FORMAT_Z16)
	{
		return Mat(Size(w, h), CV_16UC1, (void*)f.get_data(), Mat::AUTO_STEP);
	}
	else if (f.get_profile().format() == RS2_FORMAT_DISPARITY32)
	{
		return Mat(Size(w, h), CV_32FC1, (void*)f.get_data(), Mat::AUTO_STEP);
	}
	else if (f.get_profile().format() == RS2_FORMAT_Y8)
	{
		return Mat(Size(w, h), CV_8UC1, (void*)f.get_data(), Mat::AUTO_STEP);
	}

	throw std::runtime_error("Frame format is not supported yet!");
}

// Converts depth frame to a matrix of doubles with distances in meters
cv::Mat depth_frame_to_meters(const rs2::pipeline& pipe, const rs2::depth_frame& f)
{
	using namespace cv;
	using namespace rs2;

	Mat dm = frame_to_mat(f);
	dm.convertTo(dm, CV_64F);
	auto depth_scale = pipe.get_active_profile()
		.get_device()
		.first<depth_sensor>()
		.get_depth_scale();
	dm = dm * depth_scale;
	return dm;
}

sample_librealsense2.cpp

#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>

#include "cv-helpers.hpp"

int main()
{
	const int width = 1280;
	const int height = 720;
	const int fps = 30;

	// configuration
	rs2::config cfg;
	cfg.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, fps);
	cfg.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);

	rs2::pipeline pipe;
	rs2::pipeline_profile selection = pipe.start(cfg);

	// [option] get some information
	// http://artteknika.hatenablog.com/entry/2018/09/21/201527
	rs2::depth_sensor sensor = selection.get_device().first<rs2::depth_sensor>();
	auto baseline = sensor.get_option(RS2_OPTION_STEREO_BASELINE);
	auto depth_scale = sensor.get_depth_scale();
	std::cout << "baseline: " << baseline << std::endl;;
	std::cout << "depth_scale: " << depth_scale << std::endl;

	rs2::video_stream_profile depth_stream = selection.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
	rs2_intrinsics i = depth_stream.get_intrinsics();
	std::cout << "[depth_stream]" << std::endl;
	std::cout << "  fx: " << i.fx << std::endl;
	std::cout << "  fy: " << i.fy << std::endl;
	std::cout << "  ppx: " << i.ppx << std::endl;
	std::cout << "  ppy: " << i.ppy << std::endl;

	while (1)
	{
		auto frames = pipe.wait_for_frames();
		auto color_frame = frames.get_color_frame();
		auto depth_frame = frames.get_depth_frame();
		rs2::disparity_transform disparity_transform(true);
		auto disparity_frame = disparity_transform.process(depth_frame);

		cv::Mat color = frame_to_mat(color_frame);
		cv::Mat depth = frame_to_mat(depth_frame);
		cv::Mat disp = frame_to_mat(disparity_frame);

		cv::Mat depth8u;
		depth.convertTo(depth8u, CV_8U, 255.0 / 10000.0, 0.0); // 10m = 10000mm
		cv::applyColorMap(depth8u, depth8u, cv::COLORMAP_JET);

		cv::Mat disp8u;
		disp.convertTo(disp8u, CV_8U, 255.0 / 10000.0, 0.0);
		cv::applyColorMap(disp8u, disp8u, cv::COLORMAP_JET);

		cv::imshow("color", color);
		cv::imshow("depth", depth8u);
		cv::imshow("disp", disp8u);

		int key = cv::waitKey(1);
		if (key == 'q')
		{
			break;
		}
	}

	cv::destroyAllWindows();

	return 0;
}