激光雷达和相机联合标定保姆级教程_ros2单目相机和激光雷达联合标定

作者：酷酷是懒虫 | 2024-07-13 17:40:03

踩

ros2单目相机和激光雷达联合标定

1、标定准备

（1）标定板选择，棋盘格标定板，一定要长方形，否则会导致后续标定时标定工具无法判断方向。且标定板尽可能大，雷达扫描的会更准确。

（2）准备rosbag包，

首先，需要开雷达驱动和相机驱动，然后检查雷达话题和相机话题，我的是rslidar_points和usb_cam/image_raw，然后使用录制指令,需要将雷达和相机话题更换为你自己的。

rosbag record /rslidar_points /usb_cam/image_raw

需要注意激光雷达和相机的采样频率不一样，导致点云和图像不能帧间匹配，网上有很多方法，1、使用GPS对时间进行硬同步。2、可以使用抽帧的方法进行时间软同步。

解决时间同步的问题很简单，我用了一个很笨的办法，我使用rosbag实现“拍照”的效果。即对每一次标定板的姿态录一个bag包，取点云和图像的第一帧，从而达到点云和图像匹配的效果。

对标定板每个姿态录了包之后用下面的代码将rosbag转换为img和pcd的格式，从而实现了一对点云-图像对，重复上述操作，。就能得到n对点云-图像对。

代码使用教程：创建文件夹，在文件夹下创建extract_bag.py文件和data文件夹，将下面的代码填充进extract_bag.py；在data文件夹下面创建images和pointclouds文件夹，用来保存提取的点云图像。需要修改的地方有话题，bag路径，提取bag里的点云和图像的保存路径。


  #! /usr/bin/python
# coding=utf-8
 
import os
import rosbag
import cv2
from cv_bridge import CvBridge
from tqdm import tqdm
 
class ExtractBagData(object):
 
    def __init__(self, bagfile_path, camera_topic, pointcloud_topic, root):
        self.bagfile_path = bagfile_path
        self.camera_topic = camera_topic
        self.pointcloud_topic = pointcloud_topic
        self.root = root
        self.image_dir = os.path.join(root, "images")
        self.pointcloud_dir = os.path.join(root, "pointcloud")
 
        #创建提取图片和点云的目录 ./root/images  root/pointcloud
        if not os.path.exists(self.image_dir):
            os.makedirs(self.image_dir)
        if not os.path.exists(self.pointcloud_dir):
            os.makedirs(self.pointcloud_dir)
 
    def extract_camera_topic(self):
        bag = rosbag.Bag(self.bagfile_path, "r")
        bridge = CvBridge()
        bag_data_imgs = bag.read_messages(self.camera_topic)
 
        index = 0
 
        pbar = tqdm(bag_data_imgs)
        for topic, msg, t in pbar: # type: ignore
            pbar.set_description("Processing extract image id: %s" % (index+1))
            cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
            # 如果你需要使用时间戳对提取的图片命名，可以使用msg.header.stamp.to_sec()获取时间戳
            timestr = "%.6f" %  msg.header.stamp.to_sec()
            cv2.imwrite(os.path.join(self.image_dir, str(timestr) + ".jpg"), cv_image) # type: ignore
            index += 1
 
    def extract_pointcloud_topic(self):
        '''
        # 提取点云数据为pcd后缀文件,默认提取以为时间戳命名
        # 提取命令:rosrun pcl_ros bag_to_pcd result.bag /velodyne_points ./pointcloud
        # 提取点云以时间戳命令:1616554905.476288682.pcd
        :return:
        '''
        cmd = "rosrun pcl_ros bag_to_pcd %s /rslidar_points %s" % (self.bagfile_path, self.pointcloud_dir)
        os.system(cmd)
 
        # 再读取提取的pcd点云数据，把文件名修改为按照顺序索引名
        #pcd_files_list = os.listdir(self.pointcloud_dir)
        # 因为提取的pcd是以时间戳命令的，但是存放到列表中并不是按照时间戳从小到大排列，这里对时间戳进行重新排序
        #pcd_files_list_sorted = sorted(pcd_files_list)
        # print(zip(pcd_files_list, pcd_files_list_sorted))
 
        #index = 0
        #pbar = tqdm(pcd_files_list_sorted)
        #for pcd_file in pbar:
        #    pbar.set_description("Processing extract poindcloud id: %s" % (index + 1))
        #    os.rename(os.path.join(self.pointcloud_dir, pcd_file),
        #              os.path.join(self.pointcloud_dir, str(index) + ".pcd"))
        #    print("pcd_file name: ", pcd_file)
        #    index += 5
 
if __name__ == '__main__':
    #需要处理的bag包的路径
    bagfile_path = '/media/happy406/046dcd53-ee66-4d3b-b556-5f9559acab84/0508/55.bag'
    camera_topic = "/usb_cam/image_raw"
    pointcloud_topic = "/rslidar_points"
    #将bag转换为pcd和jpg格式的保存路径
    extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic,  "/media/happy406/046dcd53-ee66-4d3b-b556-5f9559acab84/0508")
    extract_bag.extract_camera_topic()
    extract_bag.extract_pointcloud_topic()