1.sdk实现
程序流程:
(1) 创建窗口
(2)创建数据管道以及配置信息
(3)打开数据管道,并传入配置信息,打开的函数返回一个profile对象
(4)选择要将深度图对齐到什么数据流(这里选择对齐到RS2_STREAM_COLOR流)
(6)定义一个align对象
(7)取新的一帧,通过align对象即可获得对齐后的帧(再重复一遍,这个例子是将深度图对齐到彩色图,即彩色图不变,深度图发生变化)
(8)将对应的帧输出(详细可参见 ----)
(10)注意:这里虽然有个 rs2::等待捕获下一帧,但是opencv的waitkey()函数不能少,否则不能显示
全部代码及原版英文注释和翻译注释:
#include <iostream>
using namespace std;
#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstring>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
using namespace cv;
#include<librealsense2/rs.hpp>
//获取深度像素对应长度单位转换
float get_depth_scale(rs2::device dev);
//检查摄像头数据管道设置是否改变
bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev);
int main(int argc, char * argv[]) try
{
// Create and initialize GUI related objects
//创建gui窗口
//window app(1280, 720, "CPP - Align Example"); // Simple window handling
//ImGui_ImplGlfw_Init(app, false); // ImGui library intializition
const char* depth_win="depth_Image";
namedWindow(depth_win,WINDOW_AUTOSIZE);
const char* color_win="color_Image";
namedWindow(color_win,WINDOW_AUTOSIZE);
//深度图像颜色map
rs2::colorizer c; // Helper to colorize depth images
//helper用于渲染图片
//texture renderer; // Helper for renderig images
// Create a pipeline to easily configure and start the camera
//创建数据管道
rs2::pipeline pipe;
rs2::config pipe_config;
pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);
//Calling pipeline's start() without any additional parameters will start the first device
//直接start(),不添加配置参数,则默认打开第一个设备
// with its default streams.
//以及以默认的配置进行流输出
//The start function returns the pipeline profile which the pipeline used to start the device
//start()函数返回数据管道的profile
rs2::pipeline_profile profile = pipe.start(pipe_config);
// Each depth camera might have different units for depth pixels, so we get it here
//每个深度摄像头有不同单元的像素,我们这里获取
// Using the pipeline's profile, we can retrieve the device that the pipeline uses
//使用数据管道的profile获取深度图像像素对应于长度单位(米)的转换比例
float depth_scale = get_depth_scale(profile.get_device());
//Pipeline could choose a device that does not have a color stream
//数据管道可以选择一个没有彩色图像数据流的设备
//If there is no color stream, choose to align depth to another stream
//选择彩色图像数据流来作为对齐对象
rs2_stream align_to = RS2_STREAM_COLOR;//find_stream_to_align(profile.get_stream());
/*
@这里的对齐是改变深度图,而不改变color图
*/
// Create a rs2::align object.
//创建一个rs2::align的对象
// rs2::align allows us to perform alignment of depth frames to others frames
//rs2::align 允许我们去实现深度图像对齐其他图像
//The "align_to" is the stream type to which we plan to align depth frames.
// "align_to"是我们打算用深度图像对齐的图像流
rs2::align align(align_to);
// Define a variable for controlling the distance to clip
//定义一个变量去转换深度到距离
float depth_clipping_distance = 1.f;
while (cvGetWindowHandle(depth_win)&&cvGetWindowHandle(color_win)) // Application still alive?
{
// Using the align object, we block the application until a frameset is available
//堵塞程序直到新的一帧捕获
rs2::frameset frameset = pipe.wait_for_frames();
// rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
// Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
//因为rs2::align 正在对齐深度图像到其他图像流,我们要确保对齐的图像流不发生改变
// after the call to wait_for_frames();
if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
{
//If the profile was changed, update the align object, and also get the new device's depth scale
//如果profile发生改变,则更新align对象,重新获取深度图像像素到长度单位的转换比例
profile = pipe.get_active_profile();
align = rs2::align(align_to);
depth_scale = get_depth_scale(profile.get_device());
}
//Get processed aligned frame
//获取对齐后的帧
auto processed = align.process(frameset);
// Trying to get both other and aligned depth frames
//尝试获取对齐后的深度图像帧和其他帧
rs2::frame aligned_color_frame = processed.get_color_frame();//processed.first(align_to);
rs2::frame aligned_depth_frame = processed.get_depth_frame().apply_filter(c);;
//获取对齐之前的color图像
rs2::frame before_depth_frame=frameset.get_depth_frame().apply_filter(c);
//获取宽高
const int depth_w=aligned_depth_frame.as<rs2::video_frame>().get_width();
const int depth_h=aligned_depth_frame.as<rs2::video_frame>().get_height();
const int color_w=aligned_color_frame.as<rs2::video_frame>().get_width();
const int color_h=aligned_color_frame.as<rs2::video_frame>().get_height();
const int b_color_w=before_depth_frame.as<rs2::video_frame>().get_width();
const int b_color_h=before_depth_frame.as<rs2::video_frame>().get_height();
//If one of them is unavailable, continue iteration
if (!aligned_depth_frame || !aligned_color_frame)
{
continue;
}
//创建OPENCV类型 并传入数据
Mat aligned_depth_image(Size(depth_w,depth_h),CV_8UC3,(void*)aligned_depth_frame.get_data(),Mat::AUTO_STEP);
Mat aligned_color_image(Size(color_w,color_h),CV_8UC3,(void*)aligned_color_frame.get_data(),Mat::AUTO_STEP);
Mat before_color_image(Size(b_color_w,b_color_h),CV_8UC3,(void*)before_depth_frame.get_data(),Mat::AUTO_STEP);
//显示
imshow(depth_win,aligned_depth_image);
imshow(color_win,aligned_color_image);
imshow("before aligned",before_color_image);
waitKey(10);
}
return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception & e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
float get_depth_scale(rs2::device dev)
{
// Go over the device's sensors
for (rs2::sensor& sensor : dev.query_sensors())
{
// Check if the sensor if a depth sensor
if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
{
return dpt.get_depth_scale();
}
}
throw std::runtime_error("Device does not have a depth sensor");
}
bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev)
{
for (auto&& sp : prev)
{
//If previous profile is in current (maybe just added another)
auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); });
if (itr == std::end(current)) //If it previous stream wasn't found in current
{
return true;
}
}
return false;
}
float get_depth_scale(rs2::device dev)
{
// Go over the device's sensors
for (rs2::sensor& sensor : dev.query_sensors())
{
// Check if the sensor if a depth sensor
if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
{
return dpt.get_depth_scale();
}
}
throw std::runtime_error("Device does not have a depth sensor");
}
CMakeLists.txt
project(align_test)
cmake_minimum_required(VERSION 2.8)
aux_source_directory(. SRC_LIST)
add_executable(${PROJECT_NAME} ${SRC_LIST})
set(CMAKE_CXX_FLAGS "-std=c++11")
#寻找opencv库
find_package(OpenCV REQUIRED)
#message(STATUS ${OpenCV_INCLUDE_DIRS})
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})
#链接Opencv库
target_link_libraries(align_test ${OpenCV_LIBS} )
#添加后可进行调试
set( CMAKE_BUILD_TYPE Debug )
set(DEPENDENCIES realsense2 )
target_link_libraries(align_test ${DEPENDENCIES})
实现效果:
左上为对齐之前的深度图,
左下为对齐后的深度图
原文链接:https://blog.csdn.net/dieju8330/article/details/85272919
2.代码实现
大概原理
目的:将深度坐标系下的点转换到彩色坐标系下
基本步骤:
(1)将深度图的像素点还原到深度坐标系下
(2)深度坐标系下的深度点还原到世界坐标系
(3)世界坐标系的深度点转换到彩色坐标系下
(4)彩色坐标系的深度点映射到Z=1的平面上,即与彩色图像的像素点对应起来
代码部分:
#include <iostream>
using namespace std;
#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstring>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
using namespace cv;
#include<librealsense2/rs.hpp>
#include<librealsense2/rsutil.h>
//获取深度像素对应长度单位(米)的换算比例
float get_depth_scale(rs2::device dev)
{
// Go over the device's sensors
for (rs2::sensor& sensor : dev.query_sensors())
{
// Check if the sensor if a depth sensor
if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
{
return dpt.get_depth_scale();
}
}
throw std::runtime_error("Device does not have a depth sensor");
}
//深度图对齐到彩色图函数
Mat align_Depth2Color(Mat depth,Mat color,rs2::pipeline_profile profile){
//声明数据流
auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
//获取内参
const auto intrinDepth=depth_stream.get_intrinsics();
const auto intrinColor=color_stream.get_intrinsics();
//直接获取从深度摄像头坐标系到彩色摄像头坐标系的欧式变换矩阵
//auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
rs2_extrinsics extrinDepth2Color;
rs2_error *error;
rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);
//平面点定义
float pd_uv[2],pc_uv[2];
//空间点定义
float Pdc3[3],Pcc3[3];
//获取深度像素与现实单位比例(D435默认1毫米)
float depth_scale = get_depth_scale(profile.get_device());
// uint16_t depth_max=0;
// for(int row=0;row<depth.rows;row++){
// for(int col=0;col<depth.cols;col++){
// if(depth_max<depth.at<uint16_t>(row,col))
// depth_max=depth.at<uint16_t>(row,col);
// }
// }
int y=0,x=0;
//初始化结果
Mat result=Mat::zeros(color.rows,color.cols,CV_8UC3);
//对深度图像遍历
for(int row=0;row<depth.rows;row++){
for(int col=0;col<depth.cols;col++){
//将当前的(x,y)放入数组pd_uv,表示当前深度图的点
pd_uv[0]=col;
pd_uv[1]=row;
//取当前点对应的深度值
uint16_t depth_value=depth.at<uint16_t>(row,col);
//换算到米
float depth_m=depth_value*depth_scale;
//将深度图的像素点根据内参转换到深度摄像头坐标系下的三维点
rs2_deproject_pixel_to_point(Pdc3,&intrinDepth,pd_uv,depth_m);
//将深度摄像头坐标系的三维点转化到彩色摄像头坐标系下
rs2_transform_point_to_point(Pcc3,&extrinDepth2Color,Pdc3);
//将彩色摄像头坐标系下的深度三维点映射到二维平面上
rs2_project_point_to_pixel(pc_uv,&intrinColor,Pcc3);
//取得映射后的(u,v)
x=(int)pc_uv[0];
y=(int)pc_uv[1];
// if(x<0||x>color.cols)
// continue;
// if(y<0||y>color.rows)
// continue;
//最值限定
x=x<0? 0:x;
x=x>depth.cols-1 ? depth.cols-1:x;
y=y<0? 0:y;
y=y>depth.rows-1 ? depth.rows-1:y;
//将成功映射的点用彩色图对应点的RGB数据覆盖
for(int k=0;k<3;k++){
//这里设置了只显示1米距离内的东西
if(depth_m<1)
result.at<cv::Vec3b>(y,x)[k]=
color.at<cv::Vec3b>(y,x)[k];
}
}
}
return result;
}
int main()
{
const char* depth_win="depth_Image";
namedWindow(depth_win,WINDOW_AUTOSIZE);
const char* color_win="color_Image";
namedWindow(color_win,WINDOW_AUTOSIZE);
//深度图像颜色map
rs2::colorizer c; // Helper to colorize depth images
//创建数据管道
rs2::pipeline pipe;
rs2::config pipe_config;
pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);
//start()函数返回数据管道的profile
rs2::pipeline_profile profile = pipe.start(pipe_config);
//定义一个变量去转换深度到距离
float depth_clipping_distance = 1.f;
//声明数据流
auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
//获取内参
auto intrinDepth=depth_stream.get_intrinsics();
auto intrinColor=color_stream.get_intrinsics();
//直接获取从深度摄像头坐标系到彩色摄像头坐标系的欧式变换矩阵
auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
while (cvGetWindowHandle(depth_win)&&cvGetWindowHandle(color_win)) // Application still alive?
{
//堵塞程序直到新的一帧捕获
rs2::frameset frameset = pipe.wait_for_frames();
//取深度图和彩色图
rs2::frame color_frame = frameset.get_color_frame();//processed.first(align_to);
rs2::frame depth_frame = frameset.get_depth_frame();
rs2::frame depth_frame_4_show = frameset.get_depth_frame().apply_filter(c);
//获取宽高
const int depth_w=depth_frame.as<rs2::video_frame>().get_width();
const int depth_h=depth_frame.as<rs2::video_frame>().get_height();
const int color_w=color_frame.as<rs2::video_frame>().get_width();
const int color_h=color_frame.as<rs2::video_frame>().get_height();
//创建OPENCV类型 并传入数据
Mat depth_image(Size(depth_w,depth_h),
CV_16U,(void*)depth_frame.get_data(),Mat::AUTO_STEP);
Mat depth_image_4_show(Size(depth_w,depth_h),
CV_8UC3,(void*)depth_frame_4_show.get_data(),Mat::AUTO_STEP);
Mat color_image(Size(color_w,color_h),
CV_8UC3,(void*)color_frame.get_data(),Mat::AUTO_STEP);
//实现深度图对齐到彩色图
Mat result=align_Depth2Color(depth_image,color_image,profile);
//显示
imshow(depth_win,depth_image_4_show);
imshow(color_win,color_image);
imshow("result",result);
waitKey(10);
}
return 0;
}
实现效果:
原文链接:https://blog.csdn.net/dieju8330/article/details/85300976