D435利用深度信息做測距工具
利用D435的深度信息,可以做出類似望遠鏡測距的效果,但是距離比較短,估計撐死10+m,但是0.1米到6米這段距離還是挺準確的。
先上個效果圖:
這裏顯示的深度圖是原深度圖(沒有對齊),已經對齊的深度圖用來保存深度信息了,就沒顯示出來。
思路:
- 取深度信息和彩色圖
- 將深度圖對齊到彩色圖,具體可參見《Intel Realsense SDK2.0學習::(二)D435深度圖片對齊到彩色圖片-SDK實現》
- 定義測量函數,用來計算指定範圍內深度信息所代表的距離信息
- 輸出
這個思路比較簡單,直接上代碼:
#include <iostream>
using namespace std;
#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include<string>
#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());
int y=0,x=0;
//初始化結果
//Mat result=Mat(color.rows,color.cols,CV_8UC3,Scalar(0,0,0));
Mat result=Mat(color.rows,color.cols,CV_16U,Scalar(0));
//對深度圖像遍歷
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;
result.at<uint16_t>(y,x)=depth_value;
}
}
//返回一個與彩色圖對齊了的深度信息圖像
return result;
}
void measure_distance(Mat &color,Mat depth,cv::Size range,rs2::pipeline_profile profile)
{
//獲取深度像素與現實單位比例(D435默認1毫米)
float depth_scale = get_depth_scale(profile.get_device());
//定義圖像中心點
cv::Point center(color.cols/2,color.rows/2);
//定義計算距離的範圍
cv::Rect RectRange(center.x-range.width/2,center.y-range.height/2,range.width,range.height);
//遍歷該範圍
float distance_sum=0;
int effective_pixel=0;
for(int y=RectRange.y;y<RectRange.y+RectRange.height;y++){
for(int x=RectRange.x;x<RectRange.x+RectRange.width;x++){
//如果深度圖下該點像素不爲0,表示有距離信息
if(depth.at<uint16_t>(y,x)){
distance_sum+=depth_scale*depth.at<uint16_t>(y,x);
effective_pixel++;
}
}
}
cout<<"遍歷完成,有效像素點:"<<effective_pixel<<endl;
float effective_distance=distance_sum/effective_pixel;
cout<<"目標距離:"<<effective_distance<<" m"<<endl;
char distance_str[30];
sprintf(distance_str,"the distance is:%f m",effective_distance);
cv::rectangle(color,RectRange,Scalar(0,0,255),2,8);
cv::putText(color,(string)distance_str,cv::Point(color.cols*0.02,color.rows*0.05),
cv::FONT_HERSHEY_PLAIN,2,Scalar(0,255,0),2,8);
}
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);
measure_distance(color_image,result,cv::Size(20,20),profile);
//顯示
imshow(depth_win,depth_image_4_show);
imshow(color_win,color_image);
//imshow("result",result);
waitKey(1);
}
return 0;
}
CMakeList.txt
project(measure_distance)
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(measure_distance ${OpenCV_LIBS} )
#添加後可進行調試
set( CMAKE_BUILD_TYPE Debug )
set(DEPENDENCIES realsense2 )
target_link_libraries(measure_distance ${DEPENDENCIES})