在ROS下利用OpenCV的Mat类，将激光点云展开为深度图像(从零开始，超详细)

激光雷达3D目标检测任务需要将地面滤除，滤除地面的方法多种多样：基于深度学习、基于栅格、基于平面拟合、基于条件随机场、基于深度图像等等。关于将点云转化为深度图像，PCL库中有相关函数，但使用起来有一定的局限性，在这里不再展开，读者可自行体会。这里主要介绍利用OpenCV的Mat类，将ROS接收到的点云消息展开为深度图像的方法。

一、ROS工作空间及包的构建

mkdir -p ~/depthMap_ws/srccd ~/catkin_ws/srccatkin_init_workspacecd ~/depthMap_ws/catkin_make

catkin_make执行过后，就成功创建了一个新的ROS工作空间，此时depthMap_ws目录下有三个文件夹:

接下来构建工作包，需要使用的库为PCL、OpenCV以及ROS自带的一些库：

cd depthMap_ws/src
catkin_create_pkg depth_make rospy roscpp std_msgs sensor_msgs pcl_ros pcl_conversions cv_bridge

其中depth_make 为包名，后面跟着的为需要的依赖。这一步进行完后，在depthMap_ws/src目录下会生成一个depth_make文件夹，该文件夹中的内容如下：

接着编辑package.xml文件，加入以下两行:

gedit package.xml#在package.xml加入以下两行:
<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>

进行到这一步，就初步完成了工作空间以及包的构建，接下来编写节点cpp文件

二、将激光点云转化为深度图的节点文件编写：

首先在depth_make工作包的src文件夹下，新建名为depth_make.cpp的文件并编辑:

cd src/
touch depth_make.cpp
gedit depth_make.cpp

(说明1：文件中接收到的激光雷达点云消息为，禾赛AT_128激光雷达录制的消息包，消息类型为sensor_msgs::PointCloud2，话题名为/hesai/pandar_points,如果要改为自己的点云包，要针对使用到的消息类型和话题名做相应的更改。)

(说明2：由于深度图是创建了一个大小为 (雷达线束x每条线上的点数) 的矩阵，为了方便操作，在此编写了一个头文件，用来自定义PCL格式的点云消息，该点云消息中除了三维坐标和强度intensity，还包含了每个点所在激光雷达线束的ring以及时间戳timestamp。该头文件位于depth_make功能包中的include的目录下，关于文件中内容，我放在depth_make.cpp文件之后)

depth_make.cpp中的内容如下:

#include<iostream>
#include<ros/ros.h>
#include<std_msgs/String.h>
#include<sensor_msgs/PointCloud2.h>
#include<pcl/PCLPointCloud2.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl/common/transforms.h>
#include<pcl_ros/filters/filter.h>
#include<pcl_ros/point_cloud.h>
#include<opencv2/core/eigen.hpp>
#include<opencv2/opencv.hpp>
#include<cv_bridge/cv_bridge.h>
#include<opencv2/highgui/highgui.hpp>
#include"depth_make/make_pointxyzitr.h"
//make_pointxyzitr.h为自定义PCL点云消息pcl::PointCloud<pcl::PointXYZITR>的头文件，该自定义点云消息中的每个点都包括了XYZ坐标、反射强度intensity、时间戳time_stamp和点所在线束ring。using namespace std;
ros::Publisher pub;void PointCallback(const sensor_msgs::PointCloud2::ConstPtr& point_msg)
{//首先将接收到的ROS格式的点云消息sensor_msgs::PointCloud2转化为PCL格式的消息    PointCloud<pcl::PointXYZITR>pcl::PointCloud<pcl::PointXYZITR> cloud_msg;pcl::fromROSMsg(*point_msg, cloud_msg);cloud_msg.header.frame_id = "map";//根据雷达型号，创建Mat矩阵，由于在此使用的雷达为128线，每条线上有1281个点，所以创建了一个大小为128*1281尺寸的矩阵，并用0元素初始化。cv::Mat range_mat = cv::Mat(128, 1281, CV_8UC3, cv::Scalar::all(0));float range; //range为该点的深度值int row_i, col_i; int scan_num = 128;int horizon_num = 1281;const float PI = 3.1415926;const float ANG = 57.2957795;//遍历每个点，计算该点对应的Mat矩阵中的索引，并为该索引对应的Mat矩阵元素赋值for(const auto pt : cloud.points){if(pt.ring != 0)  //忽略NAN点(NAN点的ring值为0){row_i = pt.ring;col_i = ((128.1/2) - atan2(pt.y, pt.x) * ANG) / 0.1;//计算该点的深度值range = (float)sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z); //忽略索引不合法的点if(row_i < 0 || row_i >= scan_num)continue;if(col_i < 0 || col_i >= horizon_num)continue;range_mat.at<float>(row_i, col_i) = range;//如果想转化为彩色的深度图，可以注释上面这一句，改用下面这一句；//range_mat.at<cv::Vec3b>(row_i, col_i) = cv::Vec3b(254-int(pt.x *2), 254- int(fabs(pt.y) / 0.5), 254-int(fabs((pt.z + 1.0f) /0.05)));}}cv::namedWindow("map",CV_WINDOW_NORMAL);//AUTOSIZE //创建一个窗口，用于显示深度图cv::imshow("map",range_mat); //在这个窗口输出图片。cv::waitKey(10); //设置显示时间pub.publish(cloud_msg); //发布点云
}int main (int argc, char **argv)
{ros::init (argc, argv, "at_128");ros::NodeHandle nh;pub = nh.advertise<pcl::PointCloud<pcl::PointXYZITR>>("pcl_points", 100, true);ros::Subscriber mapSub = nh.subscribe<sensor_msgs::PointCloud2>("/hesai/pandar_points", 100, PointCallback);ros::spin();return 0;
}

自定义PCL格式点云消息的头文件make_pointxyzitr.h内容如下，在此不再详细解释:

#define PCL_NO_PRECOMPILE
#ifndef MAKE_POINTXYZITR_H
#define MAKE_POINTXYZITR_H
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>namespace pcl
{
struct PointXYZITR
{PCL_ADD_POINT4D;          uint8_t intensity;double timestamp;uint16_t ring;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
}POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZITR,(float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(double, timestamp, timestamp)(uint16_t, ring, ring))#endif

至此，就完成了节点文件的编写，接下来进行最后一步。

三、编写CMakeLists.txt,编译功能包，运行节点:

CmakeLists.txt内容如下:

cmake_minimum_required(VERSION 2.8.3)
project(depth_make)#add_compile_options(-std=c++11)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS_RELEASE -O2)find_package(catkin REQUIRED COMPONENTSpcl_conversionspcl_rosroscpprospysensor_msgsstd_msgscv_bridge
)find_package(OpenCV REQUIRED)catkin_package(INCLUDE_DIRS includeLIBRARIES depth_makeCATKIN_DEPENDS pcl_conversions pcl_ros roscpp rospy sensor_msgs std_msgs
)include_directories(${catkin_INCLUDE_DIRS}${PCL_INCLUDE_DIRS}${OpenCV_INCLUDE_DIRS}
)link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable(depth_make src/depth_make.cpp)
target_link_libraries(depth_make ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
add_dependencies(depth_make beginner_tutorials_generate_messages_cpp) #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -msse3 -std=c++14")

编写完以上所有文件后，进入工作空间所在目录，命令行编译:

cd depthMap_ws/
source devel/setup.bash
catkin_make//如果该功能包是在自己的工作空间下创建的，想单独编译该功能包，可以将catkin_make改为单独编译:
catkin_make -DCATKIN_WHITELIST_PACKAGES="depth_make"

然后启动roscore、启动节点、播放点云bag包，即可显示每一帧点云的深度图:

#开启三个终端
#第一个:
roscore#第二个:
cd ~/depthMap_ws
source devel/setup.bash
rosrun depth_make depth_make#第三个:
cd /点云包所在路径
rosbag play 包名.bag

然后屏幕上就有了以下深度图:

如果想显示为彩色，可以改用节点文件中注释掉的那一句代码，在此不再演示效果。

在ROS下利用OpenCV的Mat类，将激光点云展开为深度图像(从零开始，超详细)相关推荐

Opencv中Mat类详细解读（学习笔记）
基于windows10.vs2015.OpenCv4.1.0. 目录 1.Mat类简介 2.Mat类的构造与赋值 2.1.Mat类的构造 2.2.Mat类的赋值 3.Mat类支持的运算 3.1Mat类 ...
Java OpenCV之Mat类的概述、常用构造方法、常用函数
Java OpenCV之Mat类概述头部信息数据类型数据部分 Mat对象的构造方法 Mat() Mat(long addr) Mat(int row,int cols,int type) Ma ...
C++下的OpenCV中Mat类型存储的图像格式
在调用CV进行图像处理时,经常涉及图像格式转换,经常出现原始图像输入后CV_32F与ushort之间的数据差异导致程序报错,可使用std::cout << "dilated_ty ...
Linux命令模式下打开摄像头,Linux下利用Opencv打开笔记本摄像头问题
新建test文件夹,文件夹存在test.cpp和CMakeLists.txttest.cpp#include #include #include #include #include #include ...
linux打开笔记本摄像头驱动程序,Linux下利用Opencv打开笔记本摄像头问题
新建test文件夹,文件夹存在test.cpp和CMakeLists.txttest.cpp#include @H_404_8@ #include #include #include #include ...
利用PCL库从点云数据生成深度图像及关键点提取
利用PCL库从点云数据生成生成深度图像及关键点提取利用PCL库从点云数据生成深度图像及关键点提取本想利用标准点云数据库分割成若干块,利用标准点云数据生成深度图像作为数据库用来验证算法,目前效果不是 ...
c++ python opencv_ubuntu下C++与Python混编，opencv中mat类转换
C++ 与 Python 混编因为赶项目进度,需要使用到深度学习的内容,不过现有的深度学习框架大多使用python代码,对于不会改写C++的朋友来说,需要耗费大量的时间去改写,因此,使用python ...
IOS下利用OpenCV框架去除视频水印
想做个去水印的APP,第一个想到的就是CV里的inpaint图像修复技术.就想着把CV框架放在IOS中用,由于第一次接触IOS的开发,就看了两本实习时候导师大神推荐的书,很多东西都不太了解,虽然CV官 ...
利用OpenCV的VideoCapture类实现视频读操作
图像处理开发需求.图像处理接私活挣零花钱,请加微信/QQ 2487872782 图像处理开发资料.图像处理技术交流请加QQ群,群号 271891601 博主注:后来,博主又写了一篇更为详细介绍Vide ...

在ROS下利用OpenCV的Mat类，将激光点云展开为深度图像(从零开始，超详细)

在ROS下利用OpenCV的Mat类，将激光点云展开为深度图像(从零开始，超详细)相关推荐

最新文章

热门文章