从ros语义点云数据 获取多个RGB点云片段

以下内容记录:

i

1、局部语义点云的全局化（借用一楠代码）

2、将点云的语义信息转换为RGB信息 ：a、读取所有点云的语义标签值为0-300中的24个，将这些语义标签值存入label[300]的对应下标位置处； b、对应每个语义值得到按位存储的RGB值。建立RGB[300],选中语义标签值为RGB[300]的下标，RGB[每个语义标签值对应下标]=对应的RGB值。

3、 从rosbag特定位置起的一定数量点云帧的x、y、z、RGB信息 存入txt文件，之后自行生成pcd文件。

ii

4、得到pcd文件中的RGB值

i的代码

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <geometry_msgs/Pose.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>

#include <pcl_ros/transforms.h>
#include <pcl/common/transforms.h>
#include <pcl/conversions.h>
#include <pcl_ros/impl/transforms.hpp>

#include <typeinfo>      //查看数据类型
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h> //下采样所需的头文件
#include <vector>

#include <iostream>
#include <fstream>
#include <string>

class SegMapROSWraper  
{
private:
  ros::NodeHandle m_nh;  
  ros::Publisher m_globalcloudPub;  //发布局部地图点云
  message_filters::Subscriber<sensor_msgs::PointCloud2> *m_pointCloudSub;  //接收点云
  tf::MessageFilter<sensor_msgs::PointCloud2> *m_tfPointCloudSub;  //接收/tf消息的过滤器，应该是接收点云和tf同步化
  tf::TransformListener m_tfListener;  // 转化坐标系
  std::vector <pcl::PointCloud<pcl::PointXYZL>>  global_point_array;   //存放所有的全局点云
  int num_zhen=0;  //表示帧数
  int num_of_points=0;  //表示已叠加的点云的点的数量
  int label[300];
  unsigned int RGB[300];
public:
  SegMapROSWraper()    //用于初始化
      : m_nh("~")  
  {
    //下标为语义标签， RGB数组的值为pcd文件中的RGB值
      RGB[0]=0;  RGB[1]=100; RGB[10]=6592100;  RGB[11]=16115300;
      RGB[15]=9845790;  RGB[18]=11804240; RGB[20]=16711680;  RGB[30]=1974015;
      RGB[40]=16711935;  RGB[44]=16750335; RGB[48]=4915275;  RGB[50]=51455;
      RGB[51]=3307775;  RGB[52]=38655; RGB[70]=44800;  RGB[71]=15495;
      RGB[72]=5304470;  RGB[80]=9892095; RGB[81]=255;  RGB[99]=16777010;
      RGB[252]=16094820;  RGB[253]=13117695; RGB[254]=6561535;  RGB[258]=11804240;
      m_pointCloudSub = new message_filters::Subscriber<sensor_msgs::PointCloud2>(m_nh,"/seg_kitti_velo/pointcloud", 100);    //接收rosbag中的点云消息
      m_tfPointCloudSub = new tf::MessageFilter<sensor_msgs::PointCloud2>(*m_pointCloudSub, m_tfListener, "/world", 100);  //接收tf和点云之后触发接收  world是frameid
      m_tfPointCloudSub->registerCallback(boost::bind(&SegMapROSWraper::insertCloudCallback, this, _1));   //回调函数
      m_globalcloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("/global_map", 2, true);   //发布全局地图，用于rviz展示
  }

  ~SegMapROSWraper()
  {
      delete m_pointCloudSub;
      delete m_tfPointCloudSub;
  }
  


  void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)  //接收到点云和tf之后，根据tf转化，然后回调函数
  {
      pcl::PointCloud<pcl::PointXYZL> pc;
      pcl::PointCloud<pcl::PointXYZL> pc_global_undowngrid;
      pcl::fromROSMsg(*cloud, pc);


      tf::StampedTransform sensorToWorldTf;   //定义存放变换关系的变量
      try
      {
          // 监听两个坐标系之间的变换， 其实就是点云坐标系（什么都行，我们的tf有很多）到世界坐标系
          m_tfListener.lookupTransform("/world", cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);   //需要从cloud->header.frame_id（left_camera）转化到/world
      }
      catch (tf::TransformException &ex)
      {
          ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
          return;
      }

      Eigen::Matrix4f sensorToWorld;
      pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);   //直接得到矩阵
      pcl::transformPointCloud(pc, pc_global_undowngrid, sensorToWorld);   //得到世界坐标系下的点云

      pcl::PointCloud<pcl::PointXYZL>::Ptr pc_global_undowngrid_ptr(new pcl::PointCloud<pcl::PointXYZL>(pc_global_undowngrid));
      pcl::PointCloud<pcl::PointXYZL> pc_global;


      //对点云下采样
      pcl::VoxelGrid<pcl::PointXYZL> sor;    // 创建滤波（下采样）对象
      sor.setInputCloud(pc_global_undowngrid_ptr);
      sor.setLeafSize(1.0f, 0.03f, 0.5f);    
      sor.filter(pc_global);        //执行滤波处理，储存输出点云 pc_global


      //存放所有帧点云
      global_point_array.push_back(pc_global);
      num_zhen++;
      //num_of_points=num_of_points + pc_global.width;
      std::string txt_name;
      txt_name=std::to_string(num_zhen);
      txt_name.append(".txt");

/*    用于寻找点云的所有label值，存储到数组label的对应位置
      for (int i=0;i<pc_global.width;i++)
      {
          if(pc_global.points[i].label==label[pc_global.points[i].label])
          {
            continue;
          }
          else
          {
            label[pc_global.points[i].label]=pc_global.points[i].label;
          }
      }
*/
      /*
              for(int i=0;i<300;i++)
        {
          std::cout<<label[i]<<std::endl;
        }
        */
       /*  显示点云的所有标签值
      if(num_zhen==1101)
      {
        for(int i=0;i<300;i++)
        {
          if(label[i]==i) std::cout<<i<<std::endl;
        }
      }
      */

      /*  用于将点云的x\y\z\RGB信息写入txt文件
      if (num_zhen==150)
      { 
        std::ofstream outfile(txt_name);
        //outfile << num_of_points << std::endl;
          for(int j=50;j<num_zhen;j++)
          {
              for (int i=0;i<int(global_point_array[j].width);i++)
              {
                if(global_point_array[j].points[i].label==0) continue;    //label为0表示未知语义的体素，人为删去这些点
                outfile << global_point_array[j].points[i].x<< ' ' << global_point_array[j].points[i].y<< ' ' 
                              << global_point_array[j].points[i].z << ' ' << RGB[global_point_array[j].points[i].label] << std::endl;
              };
          };
          outfile.close();
      }
      else if (num_zhen%50==0 && num_zhen!=50&&num_zhen%100!=0 )
      {
         std::ofstream outfile(txt_name);
         //outfile << num_of_points << std::endl;
          for(int j=num_zhen-120;j<num_zhen;j++)
          {
              for (int i=0;i<int(global_point_array[j].width);i++)
              {
                if(global_point_array[j].points[i].label==0) continue;    //label为0表示未知语义的体素，人为删去这些点
                outfile << global_point_array[j].points[i].x<< ' ' << global_point_array[j].points[i].y<< ' ' 
                              << global_point_array[j].points[i].z << ' ' << RGB[global_point_array[j].points[i].label] << std::endl;
              };
          };
          outfile.close();
      }
            if (num_zhen==100)
      { 
        std::ofstream outfile(txt_name);
        //outfile << num_of_points << std::endl;
          for(int j=0;j<num_zhen;j++)
          {
              for (int i=0;i<int(global_point_array[j].width);i++)
              {
                if(global_point_array[j].points[i].label==0) continue;    //label为0表示未知语义的体素，人为删去这些点
                outfile << global_point_array[j].points[i].x<< ' ' << global_point_array[j].points[i].y<< ' ' 
                              << global_point_array[j].points[i].z << ' ' << RGB[global_point_array[j].points[i].label] << std::endl;
              };
          };
          outfile.close();
      }
      else if (num_zhen!=1100 && num_zhen!=0&&num_zhen%100==0 )
      {
         std::ofstream outfile(txt_name);
         //outfile << num_of_points << std::endl;
          for(int j=num_zhen-120;j<num_zhen;j++)
          {
              for (int i=0;i<int(global_point_array[j].width);i++)
              {
                if(global_point_array[j].points[i].label==0) continue;    //label为0表示未知语义的体素，人为删去这些点
                outfile << global_point_array[j].points[i].x<< ' ' << global_point_array[j].points[i].y<< ' ' 
                              << global_point_array[j].points[i].z << ' ' << RGB[global_point_array[j].points[i].label] << std::endl;
              };
          };
          outfile.close();
      }
      */
      if(num_zhen==1101)
      {
        std::ofstream outfile(txt_name);
         //outfile << num_of_points << std::endl;
          for(int j=num_zhen-120;j<num_zhen;j++)
          {
              for (int i=0;i<int(global_point_array[j].width);i++)
              {
                if(global_point_array[j].points[i].label==0) continue;    //label为0表示未知语义的体素，人为删去这些点
                outfile << global_point_array[j].points[i].x<< ' ' << global_point_array[j].points[i].y<< ' ' 
                              << global_point_array[j].points[i].z << ' ' << RGB[global_point_array[j].points[i].label] << std::endl;
              };
          };
          outfile.close();
      }

      //std::cout<<pc_global_undowngrid.width<<std::endl;
      //std::cout<<pc_global.width<<std::endl;
      //std::cout << typeid(pc_global.points[0]).name() <<std::endl;
      // std::cout<< sensorToWorld <<std::endl;

      //sensor_msgs::PointCloud2 map_cloud;
      //pcl::toROSMsg(pc_global, map_cloud);  //搞成消息
      //map_cloud.header.stamp = ros::Time::now();
      //map_cloud.header.frame_id = "/map"; 
      //m_globalcloudPub .publish(map_cloud);  //加上时间戳和frameid发布出来
  }
};


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

  ros::init(argc, argv, "colored"); 

  SegMapROSWraper  SM;

  ros::spin();
  return 0;
}

ii的代码

#include<iostream>
#include<vector>
using namespace std;
int main()
{
	//加包（已知R G B，求rgb）
	int r[300],g[300],b[300];
	r[0]=0;g[0]=0;b[0]=0;
	r[1]=0;g[1]=0;b[1]=100;
	r[10]=100;g[10]=150;b[10]=100;
	r[11]=245;g[11]=230;b[11]=100;
	r[15]=150;g[15]=60;b[15]=30;
	r[18]=180;g[18]=30;b[18]=80;
	r[20]=255;g[20]=0;b[20]=0;
	r[30]=30;g[30]=30;b[30]=255;
	r[40]=255;g[40]=0;b[40]=255;
	r[44]=255;g[44]=150;b[44]=255;
	r[48]=75;g[48]=0;b[48]=75;
	r[50]=0;g[50]=200;b[50]=255;
	r[51]=50;g[51]=120;b[51]=255;
	r[52]=0;g[52]=150;b[52]=255;
	r[70]=0;g[70]=175;b[70]=0;
	r[71]=0;g[71]=60;b[71]=135;
	r[72]=80;g[72]=240;b[72]=150;
	r[80]=150;g[80]=240;b[80]=255;
	r[81]=0;g[81]=0;b[81]=255;
	r[99]=255;g[99]=255;b[99]=50;
	r[252]=245;g[252]=150;b[252]=100;
	r[253]=200;g[253]=40;b[253]=255;
	r[254]=100;g[254]=30;b[254]=255;
	r[258]=180;g[258]=30;b[258]=80;
	unsigned int rgb[300];
	vector<int> label_vector = { 0,  1,  10, 11, 15, 18, 20, 30,   40,  44,  48,  50,51, 52, 70, 71, 72, 80, 81, 99, 252,  253, 254, 258 };
	for (int i=0;i<24;i++)
	{
		rgb[label_vector[i]] = ((int)(r[label_vector[i]]) << 16 | (int)(g[label_vector[i]]) << 8 | (int)(b[label_vector[i]]));
		cout<<"rgb="<<rgb[label_vector[i]]<<endl;
	}
	return 0;
}