realsense D455+ROS+OpenCV4.5完成目标距离检测

ROS-OpenCV

---

1. 环境配置

1.1 realsense SDK2.0安装

①通过官网找到最新的SDK包并下载
Intel RealSense SDK 2.0
②解压安装包（librealsense-2.47.0.tar.gz）
③注册服务器的公钥：

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

④将服务器添加到存储库列表中：

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

⑤安装所用的库

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

⑥可选安装开发和调试包（最好全部安装）

sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg

⑦检查内核是否更新应包含realsense字符串

modinfo uvcvideo | grep "version:"

⑧重新链接摄像头运行:realsense-viewer打开摄像头

1.2 摄像头打开失败

①检查内核是否更新应包含realsense字符串

modinfo uvcvideo | grep "version:"

②可出现内核版本则是驱动没有安装

1.3 realsense驱动安装

参考信息：https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md
①安装环境

• 更新 Ubuntu 发行版，包括获取最新的稳定内核
  sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade
• 更新操作系统启动并重新启动以强制执行正确的内核选择
  sudo update-grub && sudo reboot

②拔下任何连接的摄像头

• 安装构建librealsense二进制文件和受影响的内核模块所需的核心包：

  sudo apt-get install git libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev
  

• 特定于发行版的包：

  sudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev at
  

• 定位到上述解压到安装包(librealsense-2.47.0.tar.gz)
  在根目录下运行Intel Realsense 权限脚本：
  ./scripts/setup_udev_rules.sh

③构建librealsense2 SDK

• 导航到ibrealsense根目录并运行mkdir build && cd build
• 运行Cmake:
  cmake ../ -DBUILD_EXAMPLES=true
• 重新编译并安装ibrealsense(开启8线程编译，8倍的快乐)
  sudo make uninstall && make clean && make -j8&& sudo make install
• 安装完成既可以打开摄像头

④测试摄像头

• 打开librealsense根目录下的demo程序
  cd librealsense/examples/capture
• 打开程序rs-capture
  rs-capture

⑤能打开深度图与彩色图则安装完成

1.4 ROS-OpenCV安装

①OpenCV官网

• 官网链接如下：
  https://opencv.org/releases/page/3/
  在版本下找OpenCV4.5.3
• 下载解压文件到特定文件夹中

②进入OpenCV文件夹

• cd opencv-4.5.3
  

• 安装依赖库

  sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev libavformat-dev libjpeg.dev libtiff5.dev libswscale-dev libjasper-dev  
  

• 在OpenCV下创建编译文件夹

  mkdir build
  cd build
  

• 进行文件配置

  cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local/opencv453 ..
  

• 进行源码包的编译
  sudo make -j8
• 对OpenCV-4.5.3进行安装
  sudo make install

③对OpenCV进行配置

• 将OpenCV的库添加到路径
  sudo gedit /etc/ld.so.conf.d/opencv.conf
• 在文本中加入代码

  /usr/local/lib  
  /usr/local/opencv453/lib/
  

• 让配置路径生效
  sudo ldconfig
• 打开环境变量配置文件
  sudo gedit /etc/bash.bashrc
• 文本末尾添加环境变量

  PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig  
  export PKG_CONFIG_PATH  
  

• 保存后关闭
• 使得环境生效
  source /etc/bash.bashrc
• 更新数据
  sudo updatedb
• 重启
• 查询版本
  pkg-config --modversion opencv
• 安装成功

④尝试打开OpenCV-4.5.3文件下的例程

cd opencv-4.5.3/samples/cpp/example_cmake

• 编译文件
  make
• 出现opencv_example,打开程序可以看到经典的Hello OpenCV
  ./opencv_example
• 若编译失败，可能是路径问题，查询路径是否出现opencv-4.5.3/include/opencv4/opencv2,将opencv2移动到include中，顺便删除opencv4
• 若编译找不到opencv2/***文件时，如上移动文件

⑤ROS配置OpenCV

• 进入ros的文件夹中

  cd /opt/ros/noetic/share/cv_bridge/cmake/
  

• 打开cmake文件

  sudo gedit cv_bridgeConfig.cmake
  

• 第94行改成 "include;/usr/local/opencv453/include "
• 第96行改成 "/usr/local/opencv453/include "
• 其中/usr/local/opencv453/include等地方出现opencv4同上将opencv2移出（参考④）
• 完成ROS的配置

2. 使用ROS中的OpenCV开发

2.1 创建ROS工作包环境

①生成ROS的依赖包

cd /catkin_ws/src
catkin create pkg YOUR_PKG_NAME --catkin-deps roscpp rospy
cd ~/catkin_ws
source ~/.bashrc
catkin build

②在生成的包下新建新的cpp文件，写入测试代码
参考文献：realsense SDK2.0学习

• 测试源码

#include "iostream"
#include "sstream"
#include "fstream"
#include "algorithm"
#include "cstring"
using namespace std;

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/highgui/highgui_c.h"
using namespace cv;

#include "librealsense2/rs.hpp"
#include "librealsense2/rsutil.h"

//获取深度像素对应长度单位转换
float get_depth_scale(const rs2::device& dev)
{
    //前往摄像头传感器
    for (rs2::sensor & sensor : dev.query_sensors())//使用与，两者发生一个既可
    {
        //检查是否有深度图像
        if(rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())//检查是否有深度图
        {
            return dpt.get_depth_scale();//在数组中返回数值
        }
    }
    throw std::runtime_error("Device Error!");//发生错误打印
}

//深度图对齐到彩色图像
Mat align_Depth2Color(Mat depth, const Mat &color, rs2::pipeline_profile profile) {
    //定义数据流深度与图像//auto默认类型rs2::video_stream_profile
    auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();//使用auto可以直接定义之前的数据类型
    auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();

    //获得内部参数(使用const只能在内部使用，与静态变量相似)
    const auto intrinDepth = depth_stream.get_intrinsics();//只能在函数内使用
    const auto intrinColor = color_stream.get_intrinsics();

    //直接获取从深度相机坐标系到彩色摄像头坐标系的欧拉转换矩阵
    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];

    //获得深度像素与现实单位比例
    float depth_scale = get_depth_scale(profile.get_device());
    int y,x;
    //初始化结果
    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++)
        {
            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];

            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)//声明profile
{
    //获得深度像素与现实单位比例
    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;//声明

    //创建数据管道
    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);

    //开始函数返回值的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))
    {
        //堵塞程序到新的帧出现
        rs2::frameset frameset = pipe.wait_for_frames();
        //取得深度图和彩色图
        rs2::frame color_frame = frameset.get_color_frame();//取得彩色图像
        rs2::frame depth_frame = frameset.get_depth_frame();//取得深度图像
        rs2::frame depth_frame_4_show = frameset.get_depth_frame().apply_filter(c);//c为rs2::colorizer
        //获得宽高
        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);//在彩色图显示彩色

        waitKey(1);//延时1
    }
    return 0;
}

• 在ROS依赖包中的CMakeLists.txt中添加OpenCV依赖：

cmake_minimum_required(VERSION 3.16.3)
project(measure_distance)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++14)


## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
        OpenCV REQUIRED
        roscpp
        rospy
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES measure_distance
#  CATKIN_DEPENDS roscpp rospy
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(${OpenCV_INCLUDE_DIRS})

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/measure_distance.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
 add_executable(realsense_distance src/realsense_distance.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
 target_link_libraries(realsense_distance ${OpenCV_LIBS})

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_measure_distance.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
#添加后进行调试
set( CMAKE_BUILD_TYPE Debug )
set( DEPENDENCIES realsense2)

target_link_libraries(realsense_distance ${DEPENDENCIES})

③添加launch文件夹

• 在生成包文件夹下
  mkdir launch & cd launch
• 新建新的文本文件,名字为YOUR_NAME.launch

<launch>
    <node pkg="measure_distance" type="realsense_distance" name="realsense_distance" />
</launch>

④返回工作区，到catkin_ws目录下catkin build
⑤编译完成后开启launchroslaunch YOUR_PKG_NAME LAUNCH_NAME.launch