rosbag从.bag文件中提取图片和点云数据

最新推荐文章于 2024-01-15 09:19:44 发布
最新推荐文章于 2024-01-15 09:19:44 发布
阅读量2.5k 收藏 40
点赞数 6
分类专栏： 激光雷达/相机
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_42840360/article/details/119551054
版权

提取图片

  • 查看.bag文件的信息
rosbag info xxx.bag
  • /home/stone/Desktop/data/2021-06-09-15-16-30.bag
    此填写提取文件路径
  • /detector_img
    填写相应的topic
<launch>
      <node pkg="rosbag" type="play" name="rosbag" args=" /home/stone/Desktop/data/2021-06-09-15-16-30.bag"/>
      <node name="extract" pkg="image_view" type="extract_images" respawn="false" output="screen" cwd="ROS_HOME">
        <remap from="image" to="/detector_img"/>
      </node>
 </launch>
  • 运行
roscore
roslaunch export.launch
cd ~/.ros
mv ~/.ros/*.jpg /home/stone/Desktop/data/output
                  # 储存图片的路径

提取点云

rosrun pcl_ros bag_to_pcd /home/stone/Desktop/data/2021-06-09-15-16-30.bag /obstacle_pcl /home/stone/Desktop/data/output-obstacle

  • topic是 /obstacle_pcl

  • 保存路径是/home/stone/Desktop/data/output-obstacle

  • 点云可视化

pcl_viewer  xxx.pcd

rosws project

roslaunch ilcc2 pcd2image.launch 

<?xml version="1.0"?>
<launch>

  <node pkg="ilcc2" type="pcd2image" name="pcd2image" output="screen">

     <param name="distance_valid"         value="80" />
     <param name="extrinsic_file"         value="/config/pointgrey1.bin" />

    <param name="yaml_path"  value= "cam_matrix_b.yaml" />

    <param name="image_topic"  value= "/a_image" />
    <param name="lidar_topic"  value= "/livox/lidar" />

  </node>


</launch>

rosbag中的点云提取为png深度图

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;


// void cv::undistort	
// (	InputArray 	src,                        // 原始图像
//         OutputArray 	dst,                        // 矫正图像
//         InputArray 	cameraMatrix,               // 原相机内参矩阵
//         InputArray 	distCoeffs,                 // 相机畸变参数
//         InputArray 	newCameraMatrix = noArray() // 新相机内参矩阵
// )	

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{

  cv::Mat rectifyImage;       //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);

  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }

  inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化

      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_data/"+"abc"+".jpg", rectifyImage);
  cv::waitKey(5);
}


void callback_LidarCam(const sensor_msgs::PointCloud2ConstPtr& msg_pc,
          const sensor_msgs::ImageConstPtr& msg_img)
{


  ROS_INFO_STREAM("Velodyne scan received at " << msg_pc->header.stamp.toSec());
  ROS_INFO_STREAM("image received at " << msg_img->header.stamp.toSec());
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(*msg_pc, input_cloud);                       //转换为模板点云fromROSMsg
  cv::Mat img = cv_bridge::toCvCopy(msg_img,"bgr8")->image;    //image从ros转为opencv常用格式
  processData(img, input_cloud.makeShared());
}






int number=0;
 void chatterCallback(const sensor_msgs::PointCloud2& msg_pc)
 {   
  ROS_INFO("here");
  number=number+1;
  std::string mystring=std::to_string(number);

  
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(msg_pc, input_cloud);  
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<input_cloud.size(); index++)  //找深度的最大值和最小值
  {
      ptmp =input_cloud.points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
 inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  cv::Mat greyimage(360,640,CV_8UC1); 

for (int i = 0; i < greyimage.rows; i++)
	{
		uchar *p = greyimage.ptr<uchar>(i);
		for (int j = 0; j < greyimage.cols; j++)
		{
			
				p[j] = 0;
	
		}
  }


  for(unsigned int index=0; index<input_cloud.size(); index++)
  {

    ptmp = input_cloud.points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化
      uchar *p = greyimage.ptr<uchar>(y);
      p[x] = h;
//      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  // cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  // cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  double secs =msg_pc.header.stamp.toSec();
 
   std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_data/"+std::to_string(secs)+".png", greyimage);
  cv::waitKey(5);
  
}

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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );

  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, lidar_topic,2);      //点云接收
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, image_topic, 2);            // 图像接收

  ros::Subscriber sub = nh.subscribe(lidar_topic, 1000, chatterCallback);


  // typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;   //时间戳同步
  // Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), cloud_sub, image_sub);
  // sync.registerCallback(boost::bind(&callback_LidarCam, _1, _2));                                  //回调函数
  // ROS_INFO("waiting for lidar image topic %s %s", lidar_topic.c_str(), image_topic.c_str());       

 ros::spin();
  // while (ros::ok())
  // {
    
  //   ros::spin();
  // }
}

rosbag获取矫正过的图像png

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;


// void cv::undistort	
// (	InputArray 	src,                        // 原始图像
//         OutputArray 	dst,                        // 矫正图像
//         InputArray 	cameraMatrix,               // 原相机内参矩阵
//         InputArray 	distCoeffs,                 // 相机畸变参数
//         InputArray 	newCameraMatrix = noArray() // 新相机内参矩阵
// )	

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{

  cv::Mat rectifyImage;       //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);

  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }

  inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化

      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_data/"+"abc"+".jpg", rectifyImage);
  cv::waitKey(5);
}


void callback_LidarCam(const sensor_msgs::PointCloud2ConstPtr& msg_pc,
          const sensor_msgs::ImageConstPtr& msg_img)
{


  ROS_INFO_STREAM("Velodyne scan received at " << msg_pc->header.stamp.toSec());
  ROS_INFO_STREAM("image received at " << msg_img->header.stamp.toSec());
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(*msg_pc, input_cloud);                       //转lidar_topic换为模板点云fromROSMsg
  cv::Mat img = cv_bridge::toCvCopy(msg_img,"bgr8")->image;    //image从ros转为opencv常用格式
  processData(img, input_cloud.makeShared());



}


int number=0;

 void camCallback(const sensor_msgs::Image& msg_img)
 {   
   ROS_INFO_STREAM("image received at " << msg_img.header.stamp.toSec());
  cv::Mat img = cv_bridge::toCvCopy(msg_img,"bgr8")->image; 

  cv::Mat rectifyImage;       //矫正的图像
  cv::resize(img, img, cv::Size(640, 360));
  cv::undistort(img, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);
 

  double secs =msg_img.header.stamp.toSec(); 
  std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_image/"+std::to_string(secs)+".png", img);
  cv::waitKey(5);

   
 


 }






 void lidarCallback(const sensor_msgs::PointCloud2& msg_pc)
 {   
  ROS_INFO("here");
  number=number+1;
  std::string mystring=std::to_string(number);

  
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(msg_pc, input_cloud);  
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<input_cloud.size(); index++)  //找深度的最大值和最小值
  {
      ptmp =input_cloud.points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
 inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  cv::Mat greyimage(360,640,CV_8UC1); 

for (int i = 0; i < greyimage.rows; i++)
	{
		uchar *p = greyimage.ptr<uchar>(i);
		for (int j = 0; j < greyimage.cols; j++)
		{
			
				p[j] = 0;
	
		}
  }


  for(unsigned int index=0; index<input_cloud.size(); index++)
  {

    ptmp = input_cloud.points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化
      uchar *p = greyimage.ptr<uchar>(y);
      p[x] = h;
//      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  // cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  // cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  double secs =msg_pc.header.stamp.toSec();
 
   std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_data/"+std::to_string(secs)+".png", greyimage);
  cv::waitKey(5);
  
}

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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );

  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, lidar_topic,2);      //点云接收
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, image_topic, 2);            // 图像接收

  ros::Subscriber sub = nh.subscribe(image_topic, 1000, camCallback);


  // typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;   //时间戳同步
  // Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), cloud_sub, image_sub);
  // sync.registerCallback(boost::bind(&callback_LidarCam, _1, _2));                                  //回调函数
  // ROS_INFO("waiting for lidar image topic %s %s", lidar_topic.c_str(), image_topic.c_str());       

 ros::spin();
  // while (ros::ok())
  // {
    
  //   ros::spin();
  // }
}

从rosbag同时获取点云和图像

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;


// void cv::undistort	
// (	InputArray 	src,                        // 原始图像
//         OutputArray 	dst,                        // 矫正图像
//         InputArray 	cameraMatrix,               // 原相机内参矩阵
//         InputArray 	distCoeffs,                 // 相机畸变参数
//         InputArray 	newCameraMatrix = noArray() // 新相机内参矩阵
// )	

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{

  cv::Mat rectifyImage;       //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);

  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }

  inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化

      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_data/"+"abc"+".jpg", rectifyImage);
  cv::waitKey(5);
}


void callback_LidarCam(const sensor_msgs::PointCloud2ConstPtr& msg_pc,
          const sensor_msgs::ImageConstPtr& msg_img)
{


  ROS_INFO_STREAM("Velodyne scan received at " << msg_pc->header.stamp.toSec());
  ROS_INFO_STREAM("image received at " << msg_img->header.stamp.toSec());
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(*msg_pc, input_cloud);                       //转lidar_topic换为模板点云fromROSMsg
  cv::Mat img = cv_bridge::toCvCopy(msg_img,"bgr8")->image;    //image从ros转为opencv常用格式
  processData(img, input_cloud.makeShared());



}


int number=0;

 void camCallback(const sensor_msgs::Image& msg_img)
 {   
   ROS_INFO_STREAM("image received at " << msg_img.header.stamp.toSec());
  cv::Mat img = cv_bridge::toCvCopy(msg_img,"bgr8")->image; 

  cv::Mat rectifyImage;       //矫正的图像
  cv::resize(img, img, cv::Size(640, 360));
  cv::undistort(img, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);
 

  double secs =msg_img.header.stamp.toSec(); 
  std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_image/"+std::to_string(secs)+".png", img);
  cv::waitKey(5);

   

 }






 void lidarCallback(const sensor_msgs::PointCloud2& msg_pc)
 {   
  ROS_INFO_STREAM("lidar received at " << msg_pc.header.stamp.toSec());

  number=number+1;
  std::string mystring=std::to_string(number);

  
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(msg_pc, input_cloud);  
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<input_cloud.size(); index++)  //找深度的最大值和最小值
  {
      ptmp =input_cloud.points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
 inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  cv::Mat greyimage(360,640,CV_8UC1); 

for (int i = 0; i < greyimage.rows; i++)
	{
		uchar *p = greyimage.ptr<uchar>(i);
		for (int j = 0; j < greyimage.cols; j++)
		{
			
				p[j] = 0;
	
		}
  }


  for(unsigned int index=0; index<input_cloud.size(); index++)
  {

    ptmp = input_cloud.points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化
      uchar *p = greyimage.ptr<uchar>(y);
      p[x] = h;
//      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  // cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  // cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  double secs =msg_pc.header.stamp.toSec();
 
   std::string package_path = ros::package::getPath("ilcc2");
  cv::imwrite(package_path+"/process_data/"+std::to_string(secs)+".png", greyimage);
  cv::waitKey(5);
  
}

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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );

  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, lidar_topic,2);      //点云接收
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, image_topic, 2);            // 图像接收

  ros::Subscriber sub = nh.subscribe(image_topic, 1000, camCallback);

  ros::Subscriber asub = nh.subscribe(lidar_topic, 1000, lidarCallback);
  // typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;   //时间戳同步
  // Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), cloud_sub, image_sub);
  // sync.registerCallback(boost::bind(&callback_LidarCam, _1, _2));                                  //回调函数
  // ROS_INFO("waiting for lidar image topic %s %s", lidar_topic.c_str(), image_topic.c_str());       

 ros::spin();
  // while (ros::ok())
  // {
    
  //   ros::spin();
  // }
}

深度图和rgb重叠显示

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;









void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{

  cv::Mat rectifyImage; 
  cv::resize(image, image, cv::Size(640, 360));      //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);

  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
  ROS_INFO("max distance: %d", int(inten_high));
  inten_low = 0;
  inten_high = 60;

  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];
      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/( distance_valid -inten_low)*255;  //对深度进行归一化
      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);
      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  // std::string package_path = ros::package::getPath("ilcc2");
  // cv::imwrite(package_path+"/process_data/"+"abc"+".jpg", rectifyImage);
  cv::waitKey(5);
}






std::deque<cv::Mat> img_buffer;
std::deque<pcl::PointCloud<pcl::PointXYZI>> cloud_buffer;


void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 3) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  pcl::PointCloud<pcl::PointXYZI> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

  cloud_buffer.push_back(cloud);
  if (cloud_buffer.size() > 3) {
    cloud_buffer.pop_front();
  }
}




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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );


  ros::Subscriber sub_livox =nh.subscribe<sensor_msgs::PointCloud2>(lidar_topic, 10, LiDARHandler);
  ros::Subscriber sub_image = nh.subscribe<sensor_msgs::Image>(image_topic, 20, ImageHandler);
 
  ros::Rate loop_rate(10);



  while (ros::ok()) {
    if (img_buffer.size() > 0 && cloud_buffer.size() > 0) {
      cv::Mat tmp_image = img_buffer.back();
      pcl::PointCloud<pcl::PointXYZI> tmp_cloud = cloud_buffer.back();
      img_buffer.clear();

      processData(tmp_image, tmp_cloud.makeShared());

    }

    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;

}

融合的同时保存对应的深度图和RGB图像

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;

int num=0;

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{
  num=num+1;
  std::string package_path = ros::package::getPath("ilcc2");
  cv::Mat rectifyImage; 
  cv::resize(image, image, cv::Size(640, 360));      //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);
  cv::imwrite(package_path+"/data/image/"+std::to_string(num)+".jpg", rectifyImage);
  
  cv::Mat depthimage(360,640,CV_8UC1); 

  for (int i = 0; i < depthimage.rows; i++)
    {
      uchar *p = depthimage.ptr<uchar>(i);
      for (int j = 0; j < depthimage.cols; j++)
      {
        
          p[j] = 0;
    
      }
    }
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            #include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;

int num=0;

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{
  num=num+1;
  std::string package_path = ros::package::getPath("ilcc2");
  cv::Mat rectifyImage; 
   cv::resize(image, image, cv::Size( 640, 360));     //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);
  cv::resize(rectifyImage, rectifyImage, cv::Size( 640, 360)); 

  cv::Mat saverectifyImage;
  saverectifyImage= rectifyImage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/image/"+std::to_string(num)+".png", saverectifyImage);
  
  cv::Mat depthimage(360,640,CV_16UC1); 
 
  for (int i = 0; i < depthimage.rows; i++)
    {
      unsigned short *p = depthimage.ptr<unsigned short>(i);
      for (int j = 0; j < depthimage.cols; j++)
      {
        
          p[j] = 0;
    
      }
    }
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
  ROS_INFO("max distance: %d", int(inten_high));
  inten_low = 0;
  inten_high = 260;

  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];
      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high -inten_low)*65535;  //对深度进行归一化

      unsigned short *p = depthimage.ptr<unsigned short>(y);
      p[x] = h;

      image_corners_est->HSVtoRGB(h/256, 100, 100, &r, &g, &b);
      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
      counter++;
    }
  }
//  std::cout << counter << " points ok\n";






  //cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
 
  cv::Mat savelidar_image;
  savelidar_image= rectifyImage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/lidar_image/"+std::to_string(num)+".png", savelidar_image);
  vector<int>  compression_params;
  compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
  compression_params.push_back(0);

  cv::Mat savedepthimage;
  savedepthimage= depthimage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/depthmap/"+std::to_string(num)+".png", savedepthimage,compression_params);
  cv::waitKey(5);
}




std::deque<cv::Mat> img_buffer;
std::deque<pcl::PointCloud<pcl::PointXYZI>> cloud_buffer;


void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 3) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  pcl::PointCloud<pcl::PointXYZI> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

  cloud_buffer.push_back(cloud);
  if (cloud_buffer.size() > 3) {
    cloud_buffer.pop_front();
  }
}




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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );


  ros::Subscriber sub_livox =nh.subscribe<sensor_msgs::PointCloud2>(lidar_topic, 10, LiDARHandler);
  ros::Subscriber sub_image = nh.subscribe<sensor_msgs::Image>(image_topic, 20, ImageHandler);
 
  ros::Rate loop_rate(10);



  while (ros::ok()) {
    if (img_buffer.size() > 0 && cloud_buffer.size() > 0) {
      cv::Mat tmp_image = img_buffer.back();
      pcl::PointCloud<pcl::PointXYZI> tmp_cloud = cloud_buffer.back();
      img_buffer.clear();

      processData(tmp_image, tmp_cloud.makeShared());

    }

    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;





}




  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
  ROS_INFO("max distance: %d", int(inten_high));
  inten_low = 0;
  inten_high = 60;

  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];
      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high -inten_low)*255;  //对深度进行归一化

      uchar *p = depthimage.ptr<uchar>(y);
      p[x] = h;

      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);
      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
      counter++;
    }
  }
//  std::cout << counter << " points ok\n";






  cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  cv::imwrite(package_path+"/data/lidar_image/"+std::to_string(num)+".jpg", rectifyImage);
  cv::imwrite(package_path+"/data/depthmap/"+std::to_string(num)+".png", depthimage);
  cv::waitKey(5);
}




std::deque<cv::Mat> img_buffer;
std::deque<pcl::PointCloud<pcl::PointXYZI>> cloud_buffer;


void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 3) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  pcl::PointCloud<pcl::PointXYZI> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

  cloud_buffer.push_back(cloud);
  if (cloud_buffer.size() > 3) {
    cloud_buffer.pop_front();
  }
}




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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );


  ros::Subscriber sub_livox =nh.subscribe<sensor_msgs::PointCloud2>(lidar_topic, 10, LiDARHandler);
  ros::Subscriber sub_image = nh.subscribe<sensor_msgs::Image>(image_topic, 20, ImageHandler);
 
  ros::Rate loop_rate(10);



  while (ros::ok()) {
    if (img_buffer.size() > 0 && cloud_buffer.size() > 0) {
      cv::Mat tmp_image = img_buffer.back();
      pcl::PointCloud<pcl::PointXYZI> tmp_cloud = cloud_buffer.back();
      img_buffer.clear();

      processData(tmp_image, tmp_cloud.makeShared());

    }

    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;





}

深度图改为16bit

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;

int num=0;

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{
  num=num+1;
  std::string package_path = ros::package::getPath("ilcc2");
  cv::Mat rectifyImage; 
   cv::resize(image, image, cv::Size( 640, 360));     //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);
  cv::resize(rectifyImage, rectifyImage, cv::Size( 640, 360)); 

  cv::Mat saverectifyImage;
  saverectifyImage= rectifyImage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/image/"+std::to_string(num)+".png", saverectifyImage);
  
  cv::Mat depthimage(360,640,CV_16UC1); 
 
  for (int i = 0; i < depthimage.rows; i++)
    {
      unsigned short *p = depthimage.ptr<unsigned short>(i);
      for (int j = 0; j < depthimage.cols; j++)
      {
        
          p[j] = 0;
    
      }
    }
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }
  ROS_INFO("max distance: %d", int(inten_high));
  inten_low = 0;
  inten_high = 260;

  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];
      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high -inten_low)*65535;  //对深度进行归一化

      unsigned short *p = depthimage.ptr<unsigned short>(y);
      p[x] = h;

      image_corners_est->HSVtoRGB(h/256, 100, 100, &r, &g, &b);
      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
      counter++;
    }
  }
//  std::cout << counter << " points ok\n";






  //cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
 
  cv::Mat savelidar_image;
  savelidar_image= rectifyImage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/lidar_image/"+std::to_string(num)+".png", savelidar_image);
  vector<int>  compression_params;
  compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
  compression_params.push_back(0);

  cv::Mat savedepthimage;
  savedepthimage= depthimage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/depthmap/"+std::to_string(num)+".png", savedepthimage,compression_params);
  cv::waitKey(5);
}




std::deque<cv::Mat> img_buffer;
std::deque<pcl::PointCloud<pcl::PointXYZI>> cloud_buffer;


void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 3) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  pcl::PointCloud<pcl::PointXYZI> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

  cloud_buffer.push_back(cloud);
  if (cloud_buffer.size() > 3) {
    cloud_buffer.pop_front();
  }
}




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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );


  ros::Subscriber sub_livox =nh.subscribe<sensor_msgs::PointCloud2>(lidar_topic, 10, LiDARHandler);
  ros::Subscriber sub_image = nh.subscribe<sensor_msgs::Image>(image_topic, 20, ImageHandler);
 
  ros::Rate loop_rate(10);



  while (ros::ok()) {
    if (img_buffer.size() > 0 && cloud_buffer.size() > 0) {
      cv::Mat tmp_image = img_buffer.back();
      pcl::PointCloud<pcl::PointXYZI> tmp_cloud = cloud_buffer.back();
      img_buffer.clear();

      processData(tmp_image, tmp_cloud.makeShared());

    }

    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;





}

png转为深度图

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath
#include <Eigen/Core>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>


#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>    /// pcl::transformPointCloud
#include "ilcc2/ImageCornersEst.h"
#include "ilcc2/config.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);
ros::Publisher pubLaserCloud;
double distance_valid;


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

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

  ros::NodeHandle nh;

  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

 
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data_backup/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");

  std::string extrinsic_path = package_path + extrinsic_file;
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) );
  image_corners_est->txt2extrinsic( extrinsic_path );

  cv::Mat rgb, depth;
  rgb = cv::imread("/home/stone/fsdownload/1.png");
  const char filename[] = "/home/stone/Desktop/rosws/src/ilcc2/data/depthmap/1.png";
	//“2”拿深度
  depth = cv::imread(filename, 2);
  cv::imshow("img", depth);
  cv::waitKey(0);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgbcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  Eigen::Matrix3d m_Rinverse;
  m_Rinverse=image_corners_est-> m_R;
  cout <<"inverse.............." << endl;
  m_Rinverse = m_Rinverse.inverse().eval(); 
  cout <<"inverse get.............." << endl;

  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/rgb_lidar", 10);
  ros::Rate loop_rate(10);



  while (ros::ok()) {

	for (int m = 0; m < depth.rows; m++)
	{	
		for (int n = 0; n < depth.cols; n++)
		{ 
				ushort d = depth.ptr<ushort>(m)[n];
				if (d == 0)
					continue;
				pcl::PointXYZRGB rgbpoint;
				Eigen::Vector3d P_tmp;
				P_tmp[0] = d*(m-image_corners_est->m_cx)/image_corners_est->m_fx;
				P_tmp[1] = d*(n-(image_corners_est->m_cy-4))/image_corners_est->m_fy;
				P_tmp[2] = d;
			
				Eigen::Vector3d P_w;
				P_w = m_Rinverse*(P_tmp-image_corners_est->m_t);
				rgbpoint.x = P_w[0];
				rgbpoint.y = P_w[1];
				rgbpoint.z = P_w[2];

				rgbpoint.b =  rgb.ptr<uchar>(n)[m*3];
				rgbpoint.g =  rgb.ptr<uchar>(n)[m*3+1];
				rgbpoint.r =  rgb.ptr<uchar>(n)[m*3+2];

				rgbcloud->push_back(rgbpoint);
				
		}
	}
	  sensor_msgs::PointCloud2 cloud_msg;
      pcl::toROSMsg(*rgbcloud, cloud_msg);
      cloud_msg.header.stamp = ros::Time::now();
      cloud_msg.header.frame_id = "/livox_frame";

      pubLaserCloud.publish(cloud_msg);

	  rgbcloud->is_dense = false;
	  cout <<"here.............." << endl;
	try {
		//保存点云图
		pcl::io::savePCDFile("/home/stone/pcd.pcd", *rgbcloud);
	}
	catch (pcl::IOException &e) {
		cout << e.what() << endl;
	}

	
	ros::spinOnce();
	loop_rate.sleep();

    }

 
  

}

前面整错了，应该是深度

#include <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath

#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sstream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;

int num=0;

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{
  num=num+1;
  std::string package_path = ros::package::getPath("ilcc2");
  cv::Mat rectifyImage; 
   cv::resize(image, image, cv::Size( 640, 360));     //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);
  cv::resize(rectifyImage, rectifyImage, cv::Size( 640, 360)); 

  cv::Mat saverectifyImage;
  saverectifyImage= rectifyImage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/image/"+std::to_string(num)+".png", saverectifyImage);
  
  cv::Mat depthimage(360,640,CV_16UC1); 
 
  for (int i = 0; i < depthimage.rows; i++)
    {
      unsigned short *p = depthimage.ptr<unsigned short>(i);
      for (int j = 0; j < depthimage.cols; j++)
      {
        
          p[j] = 0;
    
      }
    }
  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double z_low=255, z_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(z_low > ptmp.z)
          z_low = ptmp.z;
      if(z_high < ptmp.z)
          z_high = ptmp.z;
      datas.push_back(ptmp.z);
  }
  ROS_INFO("max distance: %lf   ,min distance: %lf", z_high, z_low);
  z_low = -2;
  z_high = 40;

  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
   // std::cout << ptmp.z << endl;
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];
      unsigned char r, g, b;

      double h = (ptmp.z-z_low)/(z_high -z_low)*65535;  //对深度进行归一化

      unsigned short *p = depthimage.ptr<unsigned short>(y);
      p[x] = h;

      image_corners_est->HSVtoRGB(h/256, 100, 100, &r, &g, &b);
      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
      counter++;
    }
  }
//  std::cout << counter << " points ok\n";






  //cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
 
  cv::Mat savelidar_image;
  savelidar_image= rectifyImage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/lidar_image/"+std::to_string(num)+".png", savelidar_image);
  vector<int>  compression_params;
  compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
  compression_params.push_back(0);

  cv::Mat savedepthimage;
  savedepthimage= depthimage(cv::Rect(0,4,640,352)); 
  cv::imwrite(package_path+"/data/depthmap/"+std::to_string(num)+".png", savedepthimage,compression_params);
  cv::waitKey(5);
}




std::deque<cv::Mat> img_buffer;
std::deque<pcl::PointCloud<pcl::PointXYZI>> cloud_buffer;


void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 3) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  pcl::PointCloud<pcl::PointXYZI> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

  cloud_buffer.push_back(cloud);
  if (cloud_buffer.size() > 3) {
    cloud_buffer.pop_front();
  }
}




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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

   
                                                                        
  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );


  ros::Subscriber sub_livox =nh.subscribe<sensor_msgs::PointCloud2>(lidar_topic, 10, LiDARHandler);
  ros::Subscriber sub_image = nh.subscribe<sensor_msgs::Image>(image_topic, 20, ImageHandler);
 
  ros::Rate loop_rate(10);



  while (ros::ok()) {
    if (img_buffer.size() > 0 && cloud_buffer.size() > 0) {
      cv::Mat tmp_image = img_buffer.back();
      pcl::PointCloud<pcl::PointXYZI> tmp_cloud = cloud_buffer.back();
      img_buffer.clear();

      processData(tmp_image, tmp_cloud.makeShared());

    }

    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;





}
Flying Stone
关注
  • 6
    点赞
  • 踩
  • 40
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
matlab编写程序对ros bag包抽帧，提取点云和图像
shuimanting520的博客
07-21 575
如下为代码ReadLidarAndImageDataFromRosbag.mlx的内容。
ROSBAG使用(二)：使用python提取bag中的图像和点云
12-21
1.1 载入 ROS提供了解析 bag 的python API，载入一个 bag 文件可以： import rosbag bag_file = 'test.bag' bag = rosbag.Bag(bag_file, "r") 1.2 读取信息 info = bag.get_type_and_topic_info() print(info) 可以得到类似如下的信息： TypesAndTopicsTuple( msg_types={ 'sensor_msgs/PointCloud2': '1158d486dd51d683ce2f1be655c3c181', 'sensor_msg
从rosbag中提取图片和数据
weixin_43977894的博客
09-05 8490
目录从rosbag中提取图片和数据1 提取图片1.1 用launch文件提取图片1.1.1 安装工具（一般不需要）1.1.2 直接创建roslaunch文件1.1.3 运行launch文件1.1.4 转移提取的文件1.1.5 批量修改文件名1.2 用Python提取图片2 提取数据为csv或txt格式2.1 命令行提取2.1.1 查看topic信息2.1.2 转换为csv格式2.2 使用Python一次性从rosbag中提取多个csv文件3 关于同步 从rosbag中提取图片和数据 本文只针对rosbag中
bag转换为image图片和激光雷达点云pcd
xc21217xc的博客
01-15 387
bag转为image和pcd
MATLAB读取.bag文件转.mat文件
weixin_43848689的博客
09-22 814
bag文件转化.mat文件
如何用matlab提取ROSBAG中的path信息与图片信息
wbzhang233的博客
10-09 629
1.提取图片信息参考博客 从ROS bag文件中提取图像 2.提取其他类型信息参考博客 利用MATLAB对Rosbag中数据的进行提取（特别是当bag中存在图片信息的时候） 另附我对提取路径定制的matlab代码： filepath=fullfile('20191009glass/','20191009glass3.bag'); bag=rosbag(filepath); % ba...
机器人操作系统ROS（五）：rosbag与读取图片与点云数据
博学而笃志，切问而近思。
01-07 3949
机器人操作系统ROS（五）：rosbag基本应用与读取图片与点云数据
解析rosbag中的bag文件，转为图片和点云
weixin_42135969的博客
10-12 625
进入rosbag所在的文件目录下，新建终端，回放rosbag包。报错 ：E: 无法定位软件包 mjepgtools。3、解析.bag文件到.pcd点云数据文件。2、解析.bag文件到.jpg文件。新建终端，执行 roscore。1、查看rosbag的信息。
ROS：bag数据包内容提取——雷达点云数据和imu数据
m0_48748418的博客
07-27 8641
从bag数据包中提取雷达点云数据和imu数据
ROS bag包提取数据
晓晨的博客
05-10 2334
日常使用中有时需要提取bag包中的某个topic数据为新的bag包，或者裁剪其中的一段时间数据。这些功能均可以通过命令实现。可以看到，其中包含ouster的激光雷达数据（/os_cloud_node_1/points）,内部IMU数据（/os_cloud_node_1/imu）以及外部IMU数据（/imu/data），总时长为202s。
kitti2bag:将KITTI数据集轻松转换为ROS bag文件！
05-02
kitti2bag 大家好！ 我正在寻找更多可以将此软件包带入新高度的人。 如果您想提供帮助，可以通过电子邮件与我联系。 我会为每一个新闻贡献感到高兴！ 将数据集轻松转换为ROS bag文件！ 合作 这个包引起了更多人的浓厚兴趣，这比我一开始就想的要多。 看到这个我真的很高兴。 我看到了许多PR和问题，但是我的日常工作不允许我进一步推进此存储库。 为了使这个程序包繁荣起来，我向社区开放。 我很高兴将您添加为该存储库的合作者。 只需给我发送电子邮件。 顺便说一下。 为了确保我们保持回购的质量，您需要至少一位合作者的PR批准。 您可以使用与他人交流。 待办事项 帮我使此功能更丰富和完整。 只需分叉此仓库，实现新功能（在这种情况下非常简单）并发出。 功能请求列表： 制作汽车的 ，因此ROS本身可以轻松完成帧之间的转换。 处理小径 支持未同步+未修正的版本 通过提供文档 提供简单的GU
rosbag包转kitti数据集
06-21
在目前智能汽车的数据采集（激光雷达数据和视觉数据）的过程中，我们通常在ROS系统中完成数据的记录。因此，我们直接得到的数据是以.bag文件格式保存的。但是，大多数现有感知网络框架的数据格式与 KITTI 数据集的数据格式一致。在 KITTI 数据集中，图像文件保存为 .png 格式，PointCloud 文件保存为 .bin 文件。所以我们需要完成从.bag 文件到.png 和.bin 文件的转换
bag2mat：一个ros包，用于将rosbag文件中的数据提取并转换为Matlab mat文件
02-04
此pkg现在是一部分，并移至rosbag_toolkit / bag2mat，请在签出新版本。 bag2mat 提取一个rosbag文件，并根据在Dictionary和yaml配置文件中指定的设置将其转换为matlab mat文件。 字典：字典文件指定应将每种消息类型的哪些变量移至matlab数组。 例如sensor_msgs/NavSatFix: t.to_sec(), m.latitude, m.longitude, m.altitude有关完整示例sensor_msgs/NavSatFix: t.to_sec(), m.latitude, m.longitude, m.altitud
rosbag.js:用于JavaScript的ROS bag文件阅读器:handbag:
05-17
rosbag rosbag是一个与node.js和浏览器兼容的模块，用于读取二进制数据文件。 安装 npm install rosbag 或者 yarn add rosbag 然后，根据您的环境，您可以import {open} from 'rosbag'或require('rosbag') import {open} from 'rosbag' 。 如果您不是在node.js中运行代码，也不是使用像webpack这样的包管理器为浏览器构建代码，则可以将脚本直接导入到页面中： < script src =" node_modules/rosbag/dist/web/index.js " > </ script > < script > // use rosbag.open() here... </ script > 快速开始 与rosbag交互的最常见方法是读取特定主题集的
2024-2030全球及中国控制膨胀合金箔行业研究及十五五规划分析报告.docx
04-18
2024-2030全球及中国控制膨胀合金箔行业研究及十五五规划分析报告
通信历年真题选择题汇总1.(DOC).doc
04-18
通信历年真题选择题汇总1.(DOC).doc
电子通信设计资料电子万年历设计与制作论文资料
04-18
电子通信设计资料电子万年历设计与制作论文资料提取方式是百度网盘分享地址
JSP-SSM健身俱乐部客户关系管理系统可升级SpringBoot源码.7z
最新发布
04-18
前台框架基于Bootstrap这一HTML5响应式框架，能够自适应不同终端设备的屏幕大小，为用户提供良好的浏览体验。开发环境兼容myEclipse、Eclipse、Idea等多种工具，配合mysql数据库，实现数据的存储与管理。后台则采用SSM（SpringMVC + Spring + Mybatis）框架，保证系统的稳定与高效运行。 系统主要包括会员信息管理、员工信息管理、设备信息管理以及退出模块。会员信息管理模块详细记录了会员的基本信息、健身目标、消费记录以及健身习惯等，同时设有会员投诉管理模块，用于收集和处理会员的意见与建议。员工信息管理模块则涵盖了员工的基本信息、工资发放情况等，帮助俱乐部进行人事管理。设备信息管理模块则负责建立器械档案，跟踪维修情况，并合理安排器械摆放位置，以延长器械使用寿命。 数据库设计方面，会员表记录了会员的各项基本信息，包括姓名、性别、职业等；部门表与员工表则分别用于记录俱乐部的组织机构和员工信息；会员消费表记录了会员的消费详情；员工工资表则用于记录员工的工资发放情况；留言表用于收集会员的留言及回复；设备类别表与设备表则详细记录了会所内器械的分类与具
2024-2030全球及中国超级殷钢 32-5行业研究及十五五规划分析报告.docx
04-18
2024-2030全球及中国超级殷钢 32-5行业研究及十五五规划分析报告
写.bin转txt格式点云转换代码
08-09
### 回答1： 我可以回答这个问题。将二进制格式的点云转换为文本格式的点云可以使用以下代码： ```c #include <stdio.h> #include <stdlib.h> typedef struct { float x; float y; float z; } Point; int main(int argc, char *argv[]) { if (argc != 3) { printf("Usage: %s input.bin output.txt\n", argv[]); return 1; } FILE *input = fopen(argv[1], "rb"); if (input == NULL) { printf("Failed to open input file: %s\n", argv[1]); return 1; } FILE *output = fopen(argv[2], "w"); if (output == NULL) { printf("Failed to open output file: %s\n", argv[2]); fclose(input); return 1; } Point point; while (fread(&point, sizeof(Point), 1, input) == 1) { fprintf(output, "%f %f %f\n", point.x, point.y, point.z); } fclose(input); fclose(output); return ; } ``` 这个程序将从名为 `input.bin` 的二进制文件中读取点云数据，并将其写入名为 `output.txt` 的文本文件中。每个点在文本文件中占一行，由三个浮点数表示其 x、y 和 z 坐标。 ### 回答2： 将二进制格式的点云文件(.bin)转换为文本格式的点云文件(.txt)可以通过以下代码实现： ```cpp #include <iostream> #include <fstream> #include <vector> struct PointCloud { float x; float y; float z; float intensity; }; int main() { std::string inputPath = "input.bin"; // 输入的二进制点云文件路径 std::string outputPath = "output.txt"; // 输出的文本点云文件路径 std::ifstream inputFile(inputPath, std::ios::binary); if (!inputFile.is_open()) { std::cout << "无法打开输入文件！" << std::endl; return 0; } inputFile.seekg(0, std::ios::end); int fileSize = inputFile.tellg(); // 获取文件大小 inputFile.seekg(0, std::ios::beg); int pointCount = fileSize / sizeof(PointCloud); // 计算点云中点的数量 std::vector<PointCloud> pointCloudData(pointCount); inputFile.read((char*)&pointCloudData[0], fileSize); // 读取二进制文件数据 inputFile.close(); std::ofstream outputFile(outputPath); if (!outputFile.is_open()) { std::cout << "无法创建输出文件！" << std::endl; return 0; } for (int i = 0; i < pointCount; i++) { outputFile << pointCloudData[i].x << " " << pointCloudData[i].y << " " << pointCloudData[i].z << " " << pointCloudData[i].intensity << "\n"; } outputFile.close(); std::cout << "转换完成！" << std::endl; return 0; } ``` 这段代码使用C++编写，首先通过指定输入的二进制点云文件路径和输出的文本点云文件路径。然后打开输入的二进制文件并获取文件大小。接着根据文件大小计算点云中点的数量，并动态分配内存保存点云数据。 接下来，从输入文件中读取二进制数据，并将其存储在vector中。最后，打开输出文件，迭代读取vector中的数据，并按照指定的格式写入到输出文件中。完成后，关闭文件并输出转换完成的消息。 需要注意的是，这段代码假定二进制文件中的数据结构是`struct PointCloud`，包含了x、y、z坐标和强度信息。如有需要，可以根据实际情况进行修改。 ### 回答3： 要将二进制（.bin）格式的点云转换为文本（.txt）格式，可以使用以下代码： ```python import numpy as np def convert_bin_to_txt(bin_file, txt_file): # 读取二进制文件 point_cloud = np.fromfile(bin_file, dtype=np.float32).reshape(-1, 4) # 保存为文本文件 np.savetxt(txt_file, point_cloud, fmt='%.6f', delimiter=' ') print("转换完成") # 示例用法 convert_bin_to_txt("input.bin", "output.txt") ``` 以上代码首先导入了必要的库，其中`numpy`用于数据处理和数组操作。然后定义了一个`convert_bin_to_txt`函数，该函数用于将二进制文件转换为文本文件。函数中的`bin_file`和`txt_file`参数分别为输入的二进制文件和输出的文本文件。 在函数中，我们使用`np.fromfile`函数从二进制文件中读取数据。使用`dtype=np.float32`指定数据类型，并使用`reshape`函数将数据转换为4列的形式。这是因为通常点云数据的每个点都包含3个坐标（x、y、z）和1个颜色（r、g、b），因此总共有4列。 接下来，我们使用`np.savetxt`函数将点云数据保存为文本文件。我们指定了保存格式为`%.6f`，这表示小数点后保留6位有效数字。我们还设置了分隔符为一个空格。 最后，在示例用法中，我们调用了`convert_bin_to_txt`函数，将输入的二进制文件路径和输出的文本文件路径作为参数传递给函数。完成转换后会打印出"转换完成"的提示。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值