激光雷达和相机同步调整（rosbag数据没对齐）

最新推荐文章于 2025-04-21 09:53:55 发布
最新推荐文章于 2025-04-21 09:53:55 发布
阅读量1.9k 收藏 28
点赞数 4
分类专栏： 激光雷达/相机
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/weixin_42840360/article/details/119875840
版权

更改时间戳的代码

#!/usr/bin/env python3

import rospy
import rosbag
import sys

if sys.getdefaultencoding() != 'utf-8':
    reload(sys)
    sys.setdefaultencoding('utf-8')
bag_name = '2021-06-09-15-16-30.bag' #被修改的bag名
out_bag_name = 'change_2021-06-09-15-16-30.bag' #修改后的bag名
dst_dir = '/home/stone/projects/ws/config/' #使用路径

with rosbag.Bag(dst_dir+out_bag_name, 'w') as outbag:
    stamp = None
    #topic:就是发布的topic msg:该topic在当前时间点下的message t:消息记录时间(非header)
    ##read_messages内可以指定的某个topic
    for topic, msg, t in rosbag.Bag(dst_dir+bag_name).read_messages():
       
       
        if topic == '/livox/imu':
            stamp = msg.header.stamp
            msg.header.stamp=stamp
            outbag.write(topic, msg, stamp)
           # print("imu")
            #print(stamp)
        #针对没有header的文件，使用上面帧数最高的topic(即:/gps)的时间戳
        ##因为read_messages是逐时间读取，所以该方法可以使用
        elif topic == '/livox/lidar' and stamp is not None: 
            stamp = msg.header.stamp
            msg.header.stamp=stamp
            outbag.write(topic, msg, stamp)
        #    continue
        # #针对格式为Header的topic
        elif topic == '/a_image' and stamp is not None:
            #print("image")
            #print(msg.header.stamp)
            stamp = msg.header.stamp-rospy.Duration(1623123042.253242879)-rospy.Duration(0.80)
            msg.header.stamp=stamp
            print(msg.header.stamp)
            print(stamp)
            outbag.write(topic, msg, stamp)

        #     outbag.write(topic, msg, msg.stamp)
        #     continue
        # #针对一般topic
        #outbag.write(topic, msg, msg.header.stamp)

print("finished")

bag2depth.cpp


#include "bag2depth/getparam.hpp"


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

void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
 // std::cout<<std::setprecision(20)<< "Image recived at "<<img_msg->header.stamp.toSec()<<std::endl;
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 10) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  //"1623123042.829017311"

 // std::cout<<std::setprecision(20)<<"LiDAR recived at "<<lidar_msg->header.stamp.toSec()<<std::endl;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

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

}


int num=0;
void processData( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat image)  
{
  num+=1;
  cv::Mat img_undistort;
  cv::resize(image, image, cv::Size( m_image_size.width,m_image_size.height));  
  cv::undistort(image, img_undistort, camK, distort_param, camK);
  createpath("/home/stone/projects/ws/output/image/");
  cv::imwrite("/home/stone/projects/ws/output/image/"+std::to_string(num)+".png", img_undistort);
  cv::Mat depthimage=cv::Mat::zeros( m_image_size.height,m_image_size.width,CV_16UC1); 
  pcl::PointXYZ ptmp;
  double max_depth_value=0;
  double min_depth_value=260;
  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 ( spaceToPlane(Pw, Pcam, distance_valid) ) {
        int x = Pcam[0];
        int y = Pcam[1];
        unsigned char r, g, b;
        double depth_value;
        depth_value=sqrt(ptmp.x*ptmp.x+ptmp.y*ptmp.y+ptmp.z*ptmp.z);
        if(depth_value>max_depth_value)
        {max_depth_value=depth_value;}
        if(depth_value<min_depth_value)
        {min_depth_value=depth_value;}
        double h = (depth_value-zmin)/(zmax -zmin)*65535;  //对深度进行归一化
        unsigned short *p = depthimage.ptr<unsigned short>(y);
        p[x] = h;
        HSVtoRGB(h/256, 100, 100, &r, &g, &b);
        cv::circle(img_undistort, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
        }
    }
    std::cout<<"max_depth_value="<<max_depth_value<<std::endl;
    std::cout<<"min_depth_value="<<min_depth_value<<std::endl;
    createpath("/home/stone/projects/ws/output/lidar_image/");
    cv::imwrite("/home/stone/projects/ws/output/lidar_image/"+std::to_string(num)+".png", img_undistort);

    std::vector<int>  compression_params;
    compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
    compression_params.push_back(0);

    createpath("/home/stone/projects/ws/output/depthmap/");
    cv::imwrite("/home/stone/projects/ws/output/depthmap/"+std::to_string(num)+".png",depthimage,compression_params);
}



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

  ros::init(argc,argv,"bag2depth");
  ros::NodeHandle nh;
  
  double T;
  std::string yaml_path, image_topic, lidar_topic;   //相机内参，topic名字
  std::string extrinsic_file;                        //外参
  ros::NodeHandle nh_private("~");
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/home/stone/projects/ws/config/pointgrey1.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "/home/stone/projects/ws/config/cam_matrix_b.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/a_image");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/livox/lidar");
  nh_private.param<double>("integral_time",T, 1);
  nh_private.param<double>("distance_valid", distance_valid,260);
  nh_private.param<double>("zmin", zmin,0);
  nh_private.param<double>("zmax", zmax,260);
  std::cout << "load T_lidar2cam from"<<  extrinsic_file<< std::endl;
  Load_extrinsic(extrinsic_file);
  Load_intrinsic(yaml_path);

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

  // ros::Publisher lidar_pub = nh.advertise<sensor_msgs::PointCloud2>("fixed_lidar", 1000);

  ros::Rate loop_rate(1/T);
  int num_cloud=0;
    while (ros::ok()) {
      if(img_buffer.size() > 0 && cloud_buffer.size() > 0) 
      {
        pcl::PointCloud<pcl::PointXYZ> integralcloud;
        for(int i=0;  i<cloud_buffer.size();i++)
        {
          pcl::PointCloud<pcl::PointXYZ> tmp_cloud = cloud_buffer.back();
          cloud_buffer.pop_back();
          integralcloud+= tmp_cloud;
        }
        //保存积分点云
        if(integralcloud.width!=0)
        { num_cloud+=1;
            std::cout<<num_cloud<<std::endl;
            std::string folderPath="/home/stone/projects/ws/output/integral_lidar"+std::to_string(T)+"/";
            createpath(folderPath);
            pcl::io::savePCDFileASCII (folderPath+"lidar"+std::to_string(num_cloud)+".pcd", integralcloud);
        }
        cv::Mat tmp_image = img_buffer.back();

        cloud_buffer.clear();
        img_buffer.clear();
        processData(integralcloud.makeShared(),tmp_image);
      }
      ros::spinOnce();
      loop_rate.sleep();

     
    }

  return 0;


}

message_filter同步


#include "bag2depth/getparam.hpp"




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

void ImageHandler(const sensor_msgs::Image::ConstPtr& img_msg) {
 // std::cout<<std::setprecision(20)<< "Image recived at "<<img_msg->header.stamp.toSec()<<std::endl;
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  if (!img.empty()) {
    img_buffer.push_back(img);
    if (img_buffer.size() > 10) {
      img_buffer.pop_front();
    }
  }
}

void LiDARHandler(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg) {
  //"1623123042.829017311"

 // std::cout<<std::setprecision(20)<<"LiDAR recived at "<<lidar_msg->header.stamp.toSec()<<std::endl;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

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

}


int num=0;
void processData( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat image)  
{
  num+=1;
  cv::Mat img_undistort;
  cv::resize(image, image, cv::Size( m_image_size.width,m_image_size.height));  
  cv::undistort(image, img_undistort, camK, distort_param, camK);
  createpath("/home/stone/projects/ws/output/image/");
  cv::imwrite("/home/stone/projects/ws/output/image/"+std::to_string(num)+".png", img_undistort);
  cv::Mat depthimage=cv::Mat::zeros( m_image_size.height,m_image_size.width,CV_16UC1); 
  pcl::PointXYZ ptmp;
  double max_depth_value=0;
  double min_depth_value=260;
  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 ( spaceToPlane(Pw, Pcam, distance_valid) ) {
        int x = Pcam[0];
        int y = Pcam[1];
        unsigned char r, g, b;
        double depth_value;
        depth_value=sqrt(ptmp.x*ptmp.x+ptmp.y*ptmp.y+ptmp.z*ptmp.z);
        if(depth_value>max_depth_value)
        {max_depth_value=depth_value;}
        if(depth_value<min_depth_value)
        {min_depth_value=depth_value;}
        double h = (depth_value-zmin)/(zmax -zmin)*65535;  //对深度进行归一化
        unsigned short *p = depthimage.ptr<unsigned short>(y);
        p[x] = h;
        HSVtoRGB(h/256, 100, 100, &r, &g, &b);
        cv::circle(img_undistort, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
        }
    }
    std::cout<<"max_depth_value="<<max_depth_value<<std::endl;
    std::cout<<"min_depth_value="<<min_depth_value<<std::endl;
    createpath("/home/stone/projects/ws/output/lidar_image/");
    cv::imwrite("/home/stone/projects/ws/output/lidar_image/"+std::to_string(num)+".png", img_undistort);

    std::vector<int>  compression_params;
    compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
    compression_params.push_back(0);

    createpath("/home/stone/projects/ws/output/depthmap/");
    cv::imwrite("/home/stone/projects/ws/output/depthmap/"+std::to_string(num)+".png",depthimage,compression_params);
}

void callback(const sensor_msgs::PointCloud2::ConstPtr& lidar_msg,const sensor_msgs::Image::ConstPtr& img_msg)
{
  cv::Mat img = cv_bridge::toCvCopy(img_msg)->image;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*lidar_msg, cloud);

   processData(cloud.makeShared(),img);

}

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

  ros::init(argc,argv,"bag2depth");
  ros::NodeHandle nh;
  
  double T;
  std::string yaml_path, image_topic, lidar_topic;   //相机内参，topic名字
  std::string extrinsic_file;                        //外参
  ros::NodeHandle nh_private("~");
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/home/stone/projects/ws/config/pointgrey1.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "/home/stone/projects/ws/config/cam_matrix_b.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/a_image");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/livox/lidar");
  nh_private.param<double>("integral_time",T, 1);
  nh_private.param<double>("distance_valid", distance_valid,260);
  nh_private.param<double>("zmin", zmin,0);
  nh_private.param<double>("zmax", zmax,260);
  std::cout << "load T_lidar2cam from"<<  extrinsic_file<< std::endl;
  Load_extrinsic(extrinsic_file);
  Load_intrinsic(yaml_path);



  
  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, lidar_topic, 1);
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, image_topic, 1);

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

  // ros::Rate loop_rate(1/T);
   ros::spin();


}
【实战项目开发技术分享】如何实现ROS数据同步
xiewf的博客
05-06 3534
ROS（Robot Operating System）是一个开源的机器人软件平台，它提供了一系列的工具和库，让机器人开发者可以方便地构建、测试和运行机器人应用。ROS的核心概念之一是话题（topic），它是一种基于发布/订阅模式的通信机制，让不同的节点（node）可以在分布式的网络中交换数据。话题的数据类型可以是任意的，例如图像、激光扫描、传感器数据等。话题的发布者（publisher）和订阅者（subscriber）不需要知道对方的存在，只需要通过一个统一的话题名称来进行匹配。
相机雷达时间同步（基于ROS）
qq_42227760的博客
04-10 7048
相机雷达时间同步（基于ROS）
解决rosbag时间戳未对齐的解决方法
热门推荐
泠山的博客
08-14 1万+
rosbag时间戳未对齐的解决方法 Reference: ROS.org: rosbag/Cookbook : http://wiki.ros.org/rosbag/Cookbook. rosbag在存储时间时，接收消息的时间和生成消息的时间可能会有很大的差别。例如：程序处理速度慢导致的消息延后; 消息在慢速播放时被记录;Synchronizer的msg传入过晚… 因此，使用每个topic内的header时间作为播放时的时间戳（rosbag record时间使用的是当前的电脑时间）不失为一种比较奈斯的方法：
如何测试雷达与相机是否时间同步？
AMOVLAB(阿木实验室)
04-21 947
由于相机和雷达在采样频率、启动延迟和数据传输等方面存在差异，若未进行有效的时间对齐，可能导致数据融合误差，影响系统性能。在STM32中将雷达和相机的硬件触发要求配置完成，并将对应的引脚连接雷达和相机的硬件触发口。以四目相机和Livox Mid-360激光雷达为例，为了对相机和雷达的数据进行融合，我们首先需要测试相机与雷达是否达到时间同步，因此我们需要测试两者的数据时间戳的差值保持在一个稳定的区间。在图（b）中两者的差值始终小于1ms，该实验结果表明雷达和相机的时间同步成功。，以实现高精度的环境感知和定位。
激光雷达LiDAR和相机的.bag数据解析与对齐
Summer_crown的博客
12-30 2506
LiDAR数据解析 Ubuntu18.04 系统,已安装ROS 查看bag信息,在相应文件夹下打开终端 rosbag info xxx.bag 获得对应的topics topics: /velodyne_points 940 msgs : sensor_msgs/PointCloud2 /zed2i/zed_node/left/image_rect_color 822 msgs : sensor_
修复ROSBAG话题时间戳不对
weixin_48916561的博客
02-18 690
修复ROSBAG话题时间戳
对rosbag中的多传感器数据进行同步并提取
weixin_43977894的博客
09-05 4536
对rosbag中的多传感器数据进行同步并提取 rosbag中记录的多传感器数据来自多个subscriber, 但是真正使用的过程中还需要对不同的传感器数据进行同步. ROS的message_filter 提供了两种同步方式: 精确同步和近似同步, 由于时间戳无法真正对齐, 本文采用近似同步. 关于message_filter 的介绍参见官网链接: http://wiki.ros.org/message_filters 1. 数据同步代码 1.1 查看.bag文件内容 rosbag info xx.ba
两个激光雷达融合功能包
01-16
"两个激光雷达融合功能包"是专为ROS设计的一个软件模块，其目的是将来自两个不同LiDAR传感器的数据进行整合，提高定位和建图的精度与可靠性。以下是对这个功能包的详细解释： 1. **激光雷达（LiDAR）**：Light ...
激光雷达相机联合标定 rosbag ros2
03-11
好的，我需要帮助用户找到在ROS2中进行激光雷达和相机联合标定的方法或教程，特别是涉及到rosbag的使用。让我先理清用户的具体需求。用户可能已经了解ROS1中的标定方法，现在想迁移到ROS2，或者他们可能刚开始接触...
激光雷达和相机事件对齐
03-09
### 激光雷达与相机的时间同步及数据对齐 #### 时间同步的重要性 为了确保激光雷达和相机的数据能够有效地组合在一起，时间同步至关重要。不同传感器采集的数据可能存在微秒级别的差异，这些差异如果不加以处理会...
激光雷达和相机标定
03-19
好的，用户想了解激光雷达和相机联合标定的方法和教程。首先，我需要回忆一下相关的知识。联合标定的主要目的是建立两个传感器之间的坐标转换关系，这样可以将激光雷达的三维点云与相机的二维图像融合。根据之前用户...
激光雷达-相机标定
最新发布
05-02
1. **时间同步**：需保证激光雷达和相机数据的时间戳对齐，可通过硬件触发或软件插值实现。 2. **特征匹配精度**：点云分辨率低时，平面提取易受噪声干扰，需结合滤波算法（如统计离群点移除）[^1]。 3. **非线性...
如何将两个rosbag包合并或者提取rosbag包中某些话题到一个rosbag里
01-20
代码叫做merge_bag.py 运行的时候 python merge_bag.py -v 1028msf.bag msf.bag  vinReNoOutlier.bag  就把msf.bag和vinReNoOutlier.bag完全合并在一起了，时间戳打的都是原来两个bag里原始的时间戳，而不是像一边rosbag play一边rosbag record一样录的是现在这个时间戳 python merge_bag.py -v –topics ‘/aft_mapped_to_init /gnss/data /imu/data vinReNoOutlier’ msf.bag 2019-10-28-
Hesai激光雷达使用PTP时间同步
晓晨的博客
06-03 4760
PTP是一个网络同步时钟的协议，全称是Precise Time Protocol，也称为IEEE 1588，其有IEEE1588v1和IEEE1588v2两个版本（其中v1版本是为工业自动化测量和控制系统开发的，适用于工业局域网应用。v2版本是在v1版本的基础上，专门针对通信网改进的，我们使用的是IEEE1588v2版本）。当硬件支持时，PTP 精度能达到亚微秒，在外界第三方PTP Master接入时连接方式如图 2中所示。
Autoware.ai开源自动驾驶系统学习日记（一）：使用Autoware录制激光雷达的rosbag数据包
R_ichun的博客
03-19 1817
本篇文章记录我使用autoware内置rosbag激光雷达数据包录制功能的录制方法，当然，如果只掌握了怎么录制，对于我而言是远远不够的，我将复习丢掉已久的ROS知识并且适度挖掘这其中的相关技术
rosbag中时间戳异常情况下，同步激光雷达和相机
路边的石头
08-21 1736
修改rosbag文件中对应topic的时间戳 #!/usr/bin/env python3 import rospy import rosbag import sys if sys.getdefaultencoding() != 'utf-8': reload(sys) sys.setdefaultencoding('utf-8') bag_name = '2021-06-09-15-16-30.bag' #被修改的bag名 out_bag_name = 'change_2021-0.
rosbag中多个话题的时间戳对齐（非实时处理）
qq_43493858的博客
11-02 3723
参考： 解决rosbag时间戳未对齐的解决方法_泠山的博客-优快云博客 结果图：
ros 同步两个消息
11-20 5745
由于最近经常用到 相机和激光雷达来采集数据，因此在这里就需要对两个不同的消息进行同步处理。 由于是之前录好的包，因此需要在播放的时候  ，需要进行 设置一个参数。   rosparam set use_sim_time true   降低播放速度：   rosbag play -r 0.1 stereo_camera.bag --clock --pause     ...
ROS 之订阅多个topic时间同步问题
xp_fangfei的博客
12-20 3483
message_filters的介绍： message_filters是一个用于roscpp和rospy的实用程序库。 它集合了许多的常用的消息“过滤”算法。消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。 举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。 Time Synchroniz
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值