IT数码 购物 网址 头条 软件 日历 阅读 图书馆
TxT小说阅读器
↓语音阅读,小说下载,古典文学↓ 		图片批量下载器
↓批量下载图片,美女图库↓ 		图片自动播放器
↓图片自动播放器↓ 		一键清除垃圾
↓轻轻一点,清除系统垃圾↓
开发: C++知识库 Java知识库 JavaScript Python PHP知识库 人工智能 区块链 大数据 移动开发 嵌入式 开发工具 数据结构与算法 开发测试 游戏开发 网络协议 系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑 笔记本 显卡 显示器 固态硬盘 硬盘 耳机 手机 iphone vivo oppo 小米 华为 单反 装机 图拉丁
 
   -> 人工智能 -> ROS——基于PCL实现点云处理 -> 正文阅读

[人工智能]ROS——基于PCL实现点云处理

文章目录

sudo apt-get install ros-melodic-navigation

IPM-ROS

git clone https://github.com/Irvingao/IPM-mapping-ros.git
# catkin_create_pkg IPM-mapping-ros rospy rosmsg roscpp

编译

cd ~/your_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="IPM-mapping-ros"

在这里插入图片描述

打开Realsense发布点云数据

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

rviz

在这里插入图片描述

PCL点云处理

接收点云信息并用pcl显示

pcl_view.cpp

//common headers
#include <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>

using namespace std;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("realtime pcl"));

ros::Publisher pub;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
   // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;    //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  // 转化为PCL中的点云的数据格式
  pcl_conversions::toPCL(*input, *cloud);

  pub.publish (*cloud);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1;
  cloud1.reset (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input, *cloud1);

//  pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//  viewer.showCloud(cloud1,"Simple Cloud Viewer");

  viewer1->removeAllPointClouds();  // 移除当前所有点云
  viewer1->addPointCloud(cloud1, "realtime pcl");
  viewer1->updatePointCloud(cloud1, "realtime pcl");
  viewer1->spinOnce(0.001);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "pcl");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  //"/camera/depth/color/points"realsense发布的点云数据
  ros::Subscriber sub = nh.subscribe ("/camera/depth/color/points", 10, cloud_cb);
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<pcl::PCLPointCloud2> ("output", 10);
  // Spin
  ros::spin ();
}

CMakeLists.txt

find_package(PCL REQUIRED)

catkin_package()

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

add_executable(pcl_view src/pcl_view.cpp)
target_link_libraries(pcl_view ${catkin_LIBRARIES} ${PCL_LIBRARIES})

在这里插入图片描述

接收到点云并进行色彩处理并发布

pcl_process.cpp

//common headers
#include <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>


class pcl_sub
{
private:
  ros::NodeHandle n;
  ros::Subscriber subCloud;
  ros::Publisher pubCloud;
  sensor_msgs::PointCloud2 msg;  //接收到的点云消息
  sensor_msgs::PointCloud2 adjust_msg;  //等待发送的点云消息
  pcl::PointCloud<pcl::PointXYZRGB> adjust_pcl;   //建立了一个pcl的点云，作为中间过程

public:
  pcl_sub():
    n("~"){
    subCloud = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 1, &pcl_sub::getcloud, this); //接收点云数据，进入回调函数getcloud()
    pubCloud = n.advertise<sensor_msgs::PointCloud2>("/adjustd_cloud", 1000);  //建立了一个发布器，主题是/adjusted_cloud，方便之后发布调整后的点云
  }

  //回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr adjust_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);   //放在这里是因为，每次都需要重新初始化
    pcl::fromROSMsg(*laserCloudMsg, *adjust_pcl_ptr);  //把msg消息转化为点云
    adjust_pcl = *adjust_pcl_ptr;  
    for (int i = 0; i < adjust_pcl.points.size(); i++)
    //把接收到的点云位置不变，颜色全部变为绿色
    {
      adjust_pcl.points[i].r = 0;
      adjust_pcl.points[i].g = 255;
      adjust_pcl.points[i].b = 0;
    }
    pcl::toROSMsg(adjust_pcl, adjust_msg);  //将点云转化为消息才能发布
    pubCloud.publish(adjust_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

  ~pcl_sub(){}

};

int main(int argc, char** argv) {

  ros::init(argc, argv, "colored");  //初始化了一个节点，名字为colored

  pcl_sub ps;

  ros::spin();
  return 0;
}

在这里插入图片描述

接收到图片进行处理并转换为彩色点云发布

//common headers
#include <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of opencv
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>


// 全局变量
ros::Publisher pubCloud;
int R, G, B; // R,G,B值
cv::Mat image; // 接收到图像信息
pcl::PointCloud<pcl::PointXYZRGB> cloud;  //建立了一个pcl的点云中间量（不能直接发布）
sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云

void imagepointcloudCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    image = cv_bridge::toCvShare(msg, "bgr8") -> image; // 获取图像

    pcl::PointCloud<pcl::PointXYZRGB> cloud;  //建立了一个pcl的点云（不能直接发布）
    cloud.width = image.rows;
    cloud.height =image.cols;
    cloud.points.resize(cloud.width * cloud.height); // 创建等于像素个数的点云

    sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云
    output_msg.header.stamp = ros::Time::now();

    int n = 0; // 点云中的点的标号
    for (int i = 0; i < image.rows; i++)
    { 
      for(int j=0;j<image.cols;j++)
      {
        R = image.at<cv::Vec3b>(i, j)[2];
        G = image.at<cv::Vec3b>(i, j)[1];
        B = image.at<cv::Vec3b>(i, j)[0];
        if (R==0 && G==0 && B==0){
          continue;
        } else {
          cloud.points[n].x = 5 * j / (double)cloud.height; // 放缩比例长
          cloud.points[n].y = 5 * i / (double)cloud.width; // 宽
          cloud.points[n].z = 0;
          cloud.points[n].r = R; // R通道
          cloud.points[n].g = G; // G通道
          cloud.points[n].b = B; // B通道
        }
        n = n + 1;
      }
    }
    pcl::toROSMsg(cloud, output_msg); //将点云转化为消息才能发布
    output_msg.header.frame_id = "map";
    pubCloud.publish(output_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
  cv::waitKey(1);
}

int main(int argc, char** argv) {
  // 初始化节点
  ros::init(argc, argv, "pcl_process_node");  //初始化了一个节点，名字为pcl_process_node
  // 局部变量
  ros::NodeHandle n;
  ros::Subscriber subCloud;

  // opencv节点
  image_transport::ImageTransport it(n);
  image_transport::Subscriber sub = it.subscribe("/rgb_camera/image_ipm", 1, imagepointcloudCallback);
  
  // 点云节点
  pubCloud = n.advertise<sensor_msgs::PointCloud2>("/map/pointcloud_ipm", 1000);  //建立了一个发布器，主题是/adjusted_cloud，方便之后发布调整后的点云

  ros::spin();
  return 0;
}

读取图像转成点云并合并已有点云

//common headers
#include <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of opencv
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>

// 全局变量
ros::Publisher pubCloud;
// 点云发布相关
cv::Mat image; // 接收到图像信息
int R, G, B; // 像素RGB信息

// 点云接收相关
pcl::PointCloud<pcl::PointXYZRGB> final_rec_cloud;   //建立了一个pcl的点云，作为全局的接收与发送端的合成点云变量


//回调函数
void getcloud(const sensor_msgs::PointCloud2ConstPtr& input_msg){
  pcl::PointCloud<pcl::PointXYZRGB> recive_cloud;   //建立了一个pcl的点云，作为中间过程
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr recive_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>); //建立一个接收ROS信息的点云指针
  pcl::fromROSMsg(*input_msg, *recive_cloud_ptr);  //把msg消息转化为点云
  recive_cloud = *recive_cloud_ptr;  
  final_rec_cloud = recive_cloud; // 将接收到的点云传递给全局点云变量
}

void imagepointcloudCallback(const sensor_msgs::ImageConstPtr& msg) {
  // 在循环时需要清空变量中的点云；因此需要重新定义。
  pcl::PointCloud<pcl::PointXYZRGB> cloud;  //建立了一个pcl的存储图像的点云中间量（不能直接发布）
  pcl::PointCloud<pcl::PointXYZRGB> output_cloud;   //建立了一个pcl的点云，作为合成点云的中间点云变量
  sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云
  try {
    image = cv_bridge::toCvShare(msg, "bgr8") -> image; // 获取图像
    // 初始化一个和图像同样大小的点云数据
    cloud.width = image.rows;
    cloud.height =image.cols;
    cloud.points.resize(cloud.width * cloud.height); // 创建这么多个点

    output_msg.header.stamp = ros::Time::now();

    int n = 0; // 点云中的点的标号
    for (int i = 0; i < image.rows; i++) { 
      for(int j=0;j<image.cols;j++) {
        R = image.at<cv::Vec3b>(i, j)[2];
        G = image.at<cv::Vec3b>(i, j)[1];
        B = image.at<cv::Vec3b>(i, j)[0];
        if (R==0 && G==0 && B==0){
          continue;
        } else {
          cloud.points[n].x = 5 * j / (double)cloud.height; // 放缩比例长
          cloud.points[n].y = 5 * i / (double)cloud.width; // 宽
          cloud.points[n].z = 0;
          cloud.points[n].r = R; // R通道
          cloud.points[n].g = G; // G通道
          cloud.points[n].b = B; // B通道
        }
        n = n + 1;
      }
    }
    if (final_rec_cloud.points.size() != 0) {
      ROS_INFO_STREAM("merge two cloud");
      output_cloud.points.resize(final_rec_cloud.points.size() + image.rows*image.cols);
      output_cloud = cloud + final_rec_cloud;
    } else {
      output_cloud = cloud;
    }
    pcl::toROSMsg(output_cloud, output_msg); //将点云转化为消息才能发布
    output_msg.header.frame_id = "camera_link";
    pubCloud.publish(output_msg); //发布调整之后的点云数据
  }
  catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
  cv::waitKey(1);
}

int main(int argc, char** argv) {
  // 初始化节点
  ros::init(argc, argv, "pcl_process_node");  //初始化了一个节点，名字为pcl_process_node
  // 局部变量
  ros::NodeHandle n;
  ros::Subscriber subCloud;

  // opencv节点
  image_transport::ImageTransport it(n);
  // image_transport::Subscriber sub = it.subscribe("/camera/color/image_raw", 1, imagepointcloudCallback);
  image_transport::Subscriber sub = it.subscribe("/rgb_camera/image_ipm", 1, imagepointcloudCallback);
  
  // 点云节点
  subCloud = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 1, getcloud); //接收点云数据，进入回调函数getcloud()
  pubCloud = n.advertise<sensor_msgs::PointCloud2>("/merge_cloud", 1000);  //建立了一个发布器，主题是/adjusted_cloud，方便之后发布调整后的点云

  ros::spin();
  return 0;
}

在这里插入图片描述
  人工智能 最新文章
2022吴恩达机器学习课程——第二课（神经网
第十五章 规则学习
FixMatch: Simplifying Semi-Supervised Le
数据挖掘Java——Kmeans算法的实现
大脑皮层的分割方法
【翻译】GPT-3是如何工作的
论文笔记:TEACHTEXT: CrossModal Generaliz
python从零学（六）
详解Python 3.x 导入(import)
【答读者问27】backtrader不支持最新版本的
上一篇文章      下一篇文章      查看所有文章
加:2022-04-27 11:19:34  更:2022-04-27 11:20:38 
 
开发: C++知识库 Java知识库 JavaScript Python PHP知识库 人工智能 区块链 大数据 移动开发 嵌入式 开发工具 数据结构与算法 开发测试 游戏开发 网络协议 系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑 笔记本 显卡 显示器 固态硬盘 硬盘 耳机 手机 iphone vivo oppo 小米 华为 单反 装机 图拉丁

360图书馆 购物 三丰科技 阅读网 日历 万年历 2025年1日历 -2025/1/6 18:20:06-

图片自动播放器
↓图片自动播放器↓ 		TxT小说阅读器
↓语音阅读,小说下载,古典文学↓ 		一键清除垃圾
↓轻轻一点,清除系统垃圾↓ 		图片批量下载器
↓批量下载图片,美女图库↓
  网站联系: qq:121756557 email:121756557@qq.com  IT数码