ROS实现pcd点云文件转前视图

鲍汁捞饭

已于 2022-03-07 16:16:39 修改

阅读量852

点赞数 3

分类专栏： ROS_PCL 文章标签：自动驾驶

于 2021-10-01 19:26:18 首次发布

本文链接：https://blog.csdn.net/weixin_47275328/article/details/120579452

版权

ROS_PCL 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

本文将实现在ROS下将pcd点云文件转为深度前视图。

一准备工作

系统：ubuntu 18.04
ROS版本：ros-melodic
IDE：VScode
语言：C++

二程序实现

直接上代码！

1.创建工作空间

创建工作空间存，src下存放执行文件。

// 创建工作空间
mkdir -p ~/deep_image/src
//进入工作空间
cd ~/deep_image
//编译
catkin_make
//进入VScode
code .

主要的VS配置主要参考赵虚左老师的教程，具体配置点此处链接: http://www.autolabor.com.cn/book/ROSTutorials/chapter1/14-ros-ji-cheng-kai-fa-huan-jing-da-jian/142-an-zhuang-vscode.html.

2.功能包及头文件

创建功能包：
包名：deep_image
依赖：

image_transport pcl_conversions pcl_ros roscpp rospy sensor_msgs std_msgs

创建头文件：
文件名：image_pro.h

//包含的头文件
#pragma once
#include <iostream>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <vector>
#include <ros/ros.h>
#include <opencv2/core/core.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <std_msgs/Header.h>
#include <pcl/conversions.h>
#include <limits>
#include <cstring>
#include <cmath>
#include <pcl/range_image/range_image_spherical.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/Pose.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/filters/impl/passthrough.hpp> 
#include <pcl/common/common_headers.h>
#include <pcl/console/parse.h>//控制台参数解析
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>

//类定义
class pcl_image_pro
{
private:
    //订阅雷达点云节点
    ros::Subscriber sub_point_cloud_; 
    //发布图像节点（本文中没用到，习惯性的创建了）
    ros::Publisher pub_Img_;  
    // 回调函数
    void point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud); 
    //给3D视窗viewer设置观察角度
    void setViewerPose(pcl::visualization::PCLVisualizer& viewer, 
                            const Eigen::Affine3f& viewer_pose);
public:
    pcl_image_pro(ros::NodeHandle &nh);
~pcl_image_pro();
void Spin();
};

3.节点文件

文件名：image_pro_node

#include "image_pro.h"


int main(int argc, char* argv[])
{
	setlocale(LC_ALL,"");
    ros::init(argc, argv, "deep_image"); //初始化了一个节点，名字为图片
    ros::NodeHandle nh;
	pcl_image_pro core(nh);
    //ros::spin();   
	return 0;
}

4.具体实现文件

文件名：image_pro

#include "image_pro.h"
    typedef pcl::PointXYZ PointType;

//类实现
 pcl_image_pro::pcl_image_pro(ros::NodeHandle &nh)
 {
    //接受点云信息
    sub_point_cloud_ = nh.subscribe("/velodyne_points", 10, &pcl_image_pro::point_cb, this);
    //发布图片信息
    pub_Img_ = nh.advertise<sensor_msgs::PointCloud2>("/pub_image", 10);
    
    ros::spin();
}
pcl_image_pro::~pcl_image_pro() {}
void pcl_image_pro::Spin()
{
}

void pcl_image_pro::setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
	Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
	Eigen::Vector3f look_at_vector = viewer_pose.rotation()*Eigen::Vector3f(0, 0, 1) + pos_vector;
	Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
	// 使用给定的参数设置相机的位置，视窗等
	viewer.setCameraPosition(
	    pos_vector[0], pos_vector[1], pos_vector[2],
		look_at_vector[0], look_at_vector[1], look_at_vector[2],
		up_vector[0], up_vector[1], up_vector[2]);
}
//回调函数 
void pcl_image_pro::point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud)
{ 
    //接收点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_per(new pcl::PointCloud<pcl::PointXYZ>);
	
    //转换为指针类型
    pcl::fromROSMsg(*in_cloud, *cloud_in_per);

    pcl::PointCloud<PointType>& point_cloud = *cloud_in_per;
   float angular_resolution_x = 0.5f, 
        angular_resolution_y = angular_resolution_x;
   
    //深度图遵循的坐标系统
    //CAMERA_FRAME系统的X轴是向右的，Y轴是向下的，Z轴是向前的。
    //LASER_FRAME：系统的X轴是向前的，Y轴是向左的，Z是向上的。
    pcl::RangeImage::CoordinateFrame coordinate_frame =pcl::RangeImage::CAMERA_FRAME;
    
    bool live_update = false;//是否实时更新的标志
    //度转弧度
	angular_resolution_x = pcl::deg2rad(angular_resolution_x);
	angular_resolution_y = pcl::deg2rad(angular_resolution_y);
 //sensorPose定义模拟深度传感器的6自由度位置：
    //横滚角roll、俯仰角pitch、偏航角yaw，默认初始值均为0。
    //传感器位姿
	Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());//初始化

    scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
		                                point_cloud.sensor_origin_[1],
			                            point_cloud.sensor_origin_[2])) *
			                            Eigen::Affine3f(point_cloud.sensor_orientation_); 


// 点云数据创建深度图像
	float noise_level = 0.0;
	float min_range = 0.0f;
	int border_size = 1;
	pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
	pcl::RangeImage & range_image = *range_image_ptr;
	range_image.createFromPointCloud(point_cloud, angular_resolution_x, angular_resolution_y,
	            pcl::deg2rad(360.0f), pcl::deg2rad(180.f), scene_sensor_pose, coordinate_frame,
                                            noise_level, min_range, border_size);


// 打开3D视窗并添加点云
	pcl::visualization::PCLVisualizer viewer("3D Viewer");//创建窗口，并设置名字为3D Viewer
	viewer.setBackgroundColor(1, 1, 1);//设置背景颜色,白色

//添加颜色为黑色，point size为1的深度图像
	pcl::visualization::PointCloudColorHandlerCustom < pcl::PointWithRange>range_image_color_handler(range_image_ptr, 0, 0, 0);
	viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");

//添加一个坐标系，并可视化原始点云
viewer.initCameraParameters();
//将视窗的查看位置设置为深度图像中的传感器位置
setViewerPose(viewer, range_image.getTransformationToWorldSystem());

// 显示深度图像
	pcl::visualization::RangeImageVisualizer range_image_wiget("Range Image");
	range_image_wiget.showRangeImage(range_image);
	while (!viewer.wasStopped())
	{
		range_image_wiget.spinOnce();//处理深度图像可视化类
		viewer.spinOnce();//处理3D窗口的当前事件
		pcl_sleep(0.01);
 
		//实时更新2D深度图像，以响应可视化窗口中的当前视角，通过命令 -l 来激活
		if (live_update)
		{
			scene_sensor_pose = viewer.getViewerPose();
			range_image.createFromPointCloud(point_cloud,angular_resolution_x,angular_resolution_y,
				pcl::deg2rad(360.0f),pcl::deg2rad(180.0f), scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
			range_image_wiget.showRangeImage(range_image);
		}
 
	}

}

5.发布pcd点云

文件名：clouds_to_image_point_pub

#include <iostream>
#include <ros/ros.h>
#include <sstream>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
#include <cmath>
#include <pcl/filters/statistical_outlier_removal.h>
#include <vector>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>

/**发布点云信息**/

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "pointclouds_grid_pub");
    ros::NodeHandle nh; 
    //发布打开的pcb点云
   ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points",1);
   pcl::PointCloud<pcl::PointXYZRGB> cloud;
   sensor_msgs::PointCloud2 output;
   //打开pcb文件
   pcl::io::loadPCDFile ("/media/tzy/linux/jjj/cloud_cluster_6.pcd", cloud);
    //将点云数据转换为ros数据发布出去
    pcl::toROSMsg(cloud, output);
    //rviz中的名字
    output.header.frame_id = "/velodyne";
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

6.配置CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(deep_image)
find_package(catkin REQUIRED COMPONENTS
  image_transport
  pcl_conversions
  pcl_ros
  roscpp
  rospy
  sensor_msgs
  std_msgs
)
find_package(PCL 1.8 REQUIRED)
find_package(OpenCV REQUIRED)
catkin_package(
  INCLUDE_DIRS include

  CATKIN_DEPENDS image_transport pcl_conversions pcl_ros roscpp rospy sensor_msgs std_msgs

  DEPENDS PCL
)
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})
add_executable(${PROJECT_NAME}_node src/image_pro_node.cpp src/image_pro.cpp)
add_executable(clouds_to_image_point_pub src/clouds_to_image_point_pub.cpp)
 target_link_libraries(${PROJECT_NAME}_node
   ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${OpenCV_LIBS}
 )
 target_link_libraries(clouds_to_image_point_pub
   ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${OpenCV_LIBS}
 )

7.launch文件

文件名：deep_image

<?xml version="1.0"?>
<launch>
    <node pkg="deep_image" type="deep_image_node" name="deep_image_node" />
    <node pkg="deep_image" type="clouds_to_image_point_pub" name="clouds_to_image_point_pub" /> 
  
</launch>

8.运行及结果

到此为止完成代码编写，，VS编译通过后，在工作空间下打开终端输入

source ./devel/setup.bash
roslaunch deep_image deep_image.launch

range_image图片
请添加图片描述

请添加图片描述

9.参考

感谢CSDN各位大牛，参考了你们的博客才有了今天这篇文章，也是本人的第一篇博客，在此附上各位的链接。

http://www.autolabor.com.cn/book/ROSTutorials/chapter1/14-ros-ji-cheng-kai-fa-huan-jing-da-jian/142-an-zhuang-vscode.html.

https://pcl.readthedocs.io/projects/tutorials/en/latest/range_image_visualization.html#range-image-visualization.

https://blog.csdn.net/luolaihua2018/article/details/117383916?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link.

https://blog.csdn.net/RuoQiQingCheDi/article/details/83987405.

https://adamshan.blog.csdn.net/article/details/82901295?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.no_search_link.

https://blog.csdn.net/suyunzzz/article/details/105930636.