D435深度圖片對齊到彩色圖片-Eigen實現
爲了更深入瞭解深度圖對齊彩色圖的過程，本例將座標變換部分使用Eigen庫來實現
本例基本按上篇：Intel Realsense SDK2.0學習：：（三）D435深度圖片對齊到彩色圖片-代碼實現 ，只不過是將座標運算部分用Eigen實現而不是直接用 rs2 庫實現
原理部分見上一篇，這裏直接給代碼 ：
一、不使用MKL加速
#define SPEED 1
#ifndef SPEED
#define EIGEN_USE_MKL_ALL
#define EIGEN_VECTORIZE_SSE4_2
#else
#endif
#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>
#include</usr/include/eigen3/Eigen/Core>
#include</usr/include/eigen3/Eigen/Dense>
//獲取深度像素對應長度單位（米）的換算比例
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 rs2_intrinsics intrinDepth=depth_stream.get_intrinsics();
const rs2_intrinsics intrinColor=color_stream.get_intrinsics();
//利用Eigen存放內參
Eigen::Matrix3d intrinDepth_matrix;
intrinDepth_matrix<<intrinDepth.fx,0,intrinDepth.ppx,0,intrinDepth.fy,intrinDepth.ppy,
0,0,1;
Eigen::Matrix3d intrinColor_matrix;
intrinColor_matrix<<intrinColor.fx,0,intrinColor.ppx,0,intrinColor.fy,intrinColor.ppy,
0,0,1;
//直接獲取從深度攝像頭座標系到彩色攝像頭座標系的歐式變換矩陣
//auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
rs2_extrinsics extrinDepth2Color;
rs2_error *error;
rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);
//Eigen 用3*3矩陣表示旋轉矩陣
Eigen::Matrix3d R_matrix;
R_matrix<<extrinDepth2Color.rotation[0],extrinDepth2Color.rotation[1],extrinDepth2Color.rotation[2],
extrinDepth2Color.rotation[3],extrinDepth2Color.rotation[4],extrinDepth2Color.rotation[5],
extrinDepth2Color.rotation[6],extrinDepth2Color.rotation[7],extrinDepth2Color.rotation[8];
//Eigen 用三維向量表示平移量
Eigen::Vector3d t_vector;
t_vector<<extrinDepth2Color.translation[0],
extrinDepth2Color.translation[1],
extrinDepth2Color.translation[2];
//Eigen 聲明歐氏變換轉移矩陣T,用上面的旋轉矩陣和平移量構造
Eigen::Isometry3d T_Depth2Color=Eigen::Isometry3d::Identity();
T_Depth2Color.rotate(R_matrix);
T_Depth2Color.pretranslate(t_vector);
//平面點定義
float pd_uv[2],pc_uv[2];
//空間點定義
float Pdc3[3],Pcc3[3];
//獲取深度像素與現實單位比例（D435默認1毫米）
float depth_scale = get_depth_scale(profile.get_device());
int y=0,x=0;
//初始化結果
Mat result=Mat(color.rows,color.cols,CV_8UC3,Scalar(0,0,0));
Eigen::Vector3d Pd_uv;//深度圖平面座標
Eigen::Matrix3d Z;//深度
Eigen::Vector3d pdc3;//深度座標系座標
Eigen::Vector3d pcc3;//彩色座標系座標
Eigen::Vector3d Pc_uv;//彩色圖平面座標
//對深度圖像遍歷
double start = clock();
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);
Pd_uv<<col,row,1;
//求逆太慢了，可選擇直接計算
pdc3=intrinDepth_matrix.inverse()*Pd_uv;
//pdc3[0]=(Pd_uv[0]-intrinDepth_matrix(0,2))*depth_m/intrinDepth_matrix(0,0);
//pdc3[1]=(Pd_uv[1]-intrinDepth_matrix(1,2))*depth_m/intrinDepth_matrix(1,1);
//pdc3[2]=depth_m;
pdc3[0]*=depth_m;
pdc3[1]*=depth_m;
pdc3[2]=depth_m;
//將深度攝像頭座標系的三維點轉化到彩色攝像頭座標系下
pcc3=T_Depth2Color*pdc3;
//將彩色攝像頭座標系下的深度三維點映射到二維平面上
//將pcc3按z軸歸一化
pcc3[0]=pcc3[0]/pcc3[2];
pcc3[1]=pcc3[1]/pcc3[2];
pcc3[2]=pcc3[2]/pcc3[2];
Pc_uv=intrinColor_matrix*pcc3;
//取得映射後的（u,v)
x=(int)Pc_uv[0];
y=(int)Pc_uv[1];
//最值限定
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<6)
result.at<cv::Vec3b>(y,x)[k]=
color.at<cv::Vec3b>(y,x)[k];
}
}
}
double endd = clock();
double thisTime = (double)(endd - start) / CLOCKS_PER_SEC;
#ifndef SPEED
cout << "加速後: " << thisTime << endl;
#else
cout << "未加速: " << thisTime << endl;
#endif // 1
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;
}
運行效果：
運行十分卡頓，看來矩陣運算花費時間太長
二、EIgen使用MKL加速
（1）Intel MKL 安裝 配置 （不展開）
代碼部分：
//#define SPEED 1
#ifndef SPEED
#define EIGEN_USE_MKL_ALL
#define EIGEN_VECTORIZE_SSE4_2
#else
#endif
#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>
#include</usr/include/eigen3/Eigen/Core>
#include</usr/include/eigen3/Eigen/Dense>
//獲取深度像素對應長度單位（米）的換算比例
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 rs2_intrinsics intrinDepth=depth_stream.get_intrinsics();
const rs2_intrinsics intrinColor=color_stream.get_intrinsics();
//利用Eigen存放內參
Eigen::Matrix3d intrinDepth_matrix;
intrinDepth_matrix<<intrinDepth.fx,0,intrinDepth.ppx,0,intrinDepth.fy,intrinDepth.ppy,
0,0,1;
Eigen::Matrix3d intrinColor_matrix;
intrinColor_matrix<<intrinColor.fx,0,intrinColor.ppx,0,intrinColor.fy,intrinColor.ppy,
0,0,1;
//直接獲取從深度攝像頭座標系到彩色攝像頭座標系的歐式變換矩陣
//auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
rs2_extrinsics extrinDepth2Color;
rs2_error *error;
rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);
//Eigen 用3*3矩陣表示旋轉矩陣
Eigen::Matrix3d R_matrix;
R_matrix<<extrinDepth2Color.rotation[0],extrinDepth2Color.rotation[1],extrinDepth2Color.rotation[2],
extrinDepth2Color.rotation[3],extrinDepth2Color.rotation[4],extrinDepth2Color.rotation[5],
extrinDepth2Color.rotation[6],extrinDepth2Color.rotation[7],extrinDepth2Color.rotation[8];
//Eigen 用三維向量表示平移量
Eigen::Vector3d t_vector;
t_vector<<extrinDepth2Color.translation[0],
extrinDepth2Color.translation[1],
extrinDepth2Color.translation[2];
//Eigen 聲明歐氏變換轉移矩陣T,用上面的旋轉矩陣和平移量構造
Eigen::Isometry3d T_Depth2Color=Eigen::Isometry3d::Identity();
T_Depth2Color.rotate(R_matrix);
T_Depth2Color.pretranslate(t_vector);
//平面點定義
float pd_uv[2],pc_uv[2];
//空間點定義
float Pdc3[3],Pcc3[3];
//獲取深度像素與現實單位比例（D435默認1毫米）
float depth_scale = get_depth_scale(profile.get_device());
int y=0,x=0;
//初始化結果
Mat result=Mat(color.rows,color.cols,CV_8UC3,Scalar(0,0,0));
Eigen::Vector3d Pd_uv;//深度圖平面座標
Eigen::Matrix3d Z;//深度
Eigen::Vector3d pdc3;//深度座標系座標
Eigen::Vector3d pcc3;//彩色座標系座標
Eigen::Vector3d Pc_uv;//彩色圖平面座標
//對深度圖像遍歷
double start = clock();
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);
Pd_uv<<col,row,1;
//求逆太慢了，可選擇直接計算
pdc3=intrinDepth_matrix.inverse()*Pd_uv;
//pdc3[0]=(Pd_uv[0]-intrinDepth_matrix(0,2))*depth_m/intrinDepth_matrix(0,0);
//pdc3[1]=(Pd_uv[1]-intrinDepth_matrix(1,2))*depth_m/intrinDepth_matrix(1,1);
//pdc3[2]=depth_m;
pdc3[0]*=depth_m;
pdc3[1]*=depth_m;
pdc3[2]=depth_m;
//將深度攝像頭座標系的三維點轉化到彩色攝像頭座標系下
pcc3=T_Depth2Color*pdc3;
//將彩色攝像頭座標系下的深度三維點映射到二維平面上
//將pcc3按z軸歸一化
pcc3[0]=pcc3[0]/pcc3[2];
pcc3[1]=pcc3[1]/pcc3[2];
pcc3[2]=pcc3[2]/pcc3[2];
Pc_uv=intrinColor_matrix*pcc3;
//取得映射後的（u,v)
x=(int)Pc_uv[0];
y=(int)Pc_uv[1];
//最值限定
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<6)
result.at<cv::Vec3b>(y,x)[k]=
color.at<cv::Vec3b>(y,x)[k];
}
}
}
double endd = clock();
double thisTime = (double)(endd - start) / CLOCKS_PER_SEC;
#ifndef SPEED
cout << "加速後: " << thisTime << endl;
#else
cout << "未加速: " << thisTime << endl;
#endif // 1
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;
}
爲了方便參考，給出使用MKL的 CMAKeLIST.txt
project(depth_align2_color_with_Eigen)
cmake_minimum_required(VERSION 2.8)
aux_source_directory(. SRC_LIST)
add_executable(${PROJECT_NAME} ${SRC_LIST})
#c++ 11
set(CMAKE_CXX_FLAGS "-std=c++11")
#尋找opencv庫
find_package(OpenCV REQUIRED)
#查找openmp
FIND_PACKAGE( OpenMP REQUIRED)
if(OPENMP_FOUND)
message("OPENMP FOUND")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
set(CMAKE_SHARE_LINKER_FLAGS "${CMAKE_SHARE_LINKER_FLAGS} ${OpenMP_SHARE_LINKER_FLAGS}")
endif()
#添加頭文件
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(/opt/intel/mkl/include)
INCLUDE_DIRECTORIES(/usr/include/eigen3)
#鏈接Opencv庫
target_link_libraries(depth_align2_color_with_Eigen ${OpenCV_LIBS} )
#添加後可進行調試
#set( CMAKE_BUILD_TYPE Debug )
#realsense2 庫鏈接
set(DEPENDENCIES realsense2 )
target_link_libraries(depth_align2_color_with_Eigen ${DEPENDENCIES})
#mkl庫鏈接， omp加速優化等
ADD_DEFINITIONS(
-L/opt/intel/lib/intel64 -lmkl_intel_lp64 -lmkl_intel_thread -lmkl_core -liomp5 -lpthread -ldl -lm )
#g++ main.cpp -L/opt/intel/mkl/lib/intel64 -std=c++11 -lmkl_intel_lp64 -lmkl_intel_thread -lmkl_core -lrealsense2 `pkg-config --cflags --libs opencv` -L/opt/intel/lib/intel64 -liomp5 -lpthread -ldl -lm
運行效果：
真是奇怪，使用MKL後效果沒有提升，反而有更慢的趨勢。
總結
可能由於計算的是小矩陣，運算速度上用不用MKL其實差別不大，只是這個小矩陣的運算次數非常多，是像素點次，即640*480次，最終下來花費時間很長。 大概看了一下rs2庫對應的座標變換函數，並非用矩陣運算，而是直接四則運算，怪不得效果好那麼多，而且rs2的線程優化貌似也已經很好了。
一句話：realsense sdk2.0 的四則運算實現的座標變換比Eigen矩陣實現流暢N倍，