Livox-Mid70与相机联合标定及数据集采集

---

1. 相机内参标定

相机内存标定可以使用Matlab的Calibration Tools，也可以使用官方文档中的相机标定方法进行标定
利用官方文档中的方法进行标定：
官方文档：https://github.com/Livox-SDK/livox_camera_lidar_calibration/blob/master/doc_resources/README_cn.md

运行usb_cam启动USB摄像头，采集图片

roslaunch usb_cam usb_cam-test.launch

按右键或Ctrl+s保存图片。

标定相机

roslaunch camera_lidar_calibration cameraCalib.launch

---

2. 按照官方标定文档进行标定

官方文档

3. 数据集采集

修改官方Livox驱动代码，使得时间戳i为ROS的时间戳，方便与照片对其。

1. 将ws_livox/src/livox_ros_driver/livox_ros_driver中的lddc.cpp的ros::Time(timestamp / 1000000000.0)改为ros::Time::now()。
2. livox_ros_driver.cpp

  ros::Time::init();
  while (ros::ok()) {
    lddc->DistributeLidarData();
    cout<<1111111111<<ros::Time::now();
  }

rosbag record -O dataset524.bag /livox/lidar /usb_cam/image_raw

rosrun pcl_ros bag_to_pcd /sd/dataset524.bag /livox/lidar /sd/dataset524/lidar/

python get_image.py

get_image.py

#coding:utf-8
 
import roslib;  
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
 
path='/sd/dataset524/images/' #存放图片的位置
class ImageCreator():
 
 
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/sd/dataset524-all.bag', 'r') as bag:   #要读取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/usb_cam/image_raw":  #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print e
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
                        cv2.imwrite(path+image_name, cv_image)  #保存；
 
 
if __name__ == '__main__':
 
    #rospy.init_node(PKG)
 
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

python align.py

align.py

# align_data.py
#coding:utf-8
 
import os
from shutil import copy

#初步解析后点云和图片的存放地址
Lpath='/sd/dataset524/lidar/'
Cpath='/sd/dataset524/images/'

# 存在对齐后的点云图片
New_L_path ='/sd/dataset524/align/lidar/'
New_C_path ='/sd/dataset524/align/images'

jpg_appendix='.jpg'
pcd_appendix='.pcd'


def file2txt(file_path,txt_path):
    names = os.listdir(file_path)
    image_ids = open(txt_path,'w')
    for name in names:
        image_ids.write('%s\n'%(name))
    image_ids.close()

def changename(filepath,appendix):
    flist=os.listdir(filepath)
    n=0
    for i in flist:
        oldname=filepath+flist[n]
        #取时间戳的前0-11位,重新命名
        newname=filepath+flist[n][:12]+appendix
        os.rename(oldname,newname)
        n+=1
        #print(oldname,'---->',newname)

def find_timestap(Lidar_path,Camera_path):
    Lidar_list = os.listdir(Lidar_path)
    Camera_list = os.listdir(Camera_path)
    Both_list=list()

    for n,name in enumerate(Lidar_list):
        Lidar_list[n]=name.rstrip('.pcd')
        n+=1

    for n,name in enumerate(Camera_list):
        Camera_list[n]=name.rstrip('.jpg')
        n+=1
    n=0
    # 利用图片的索引去点云中找,一般以帧数少的作为索引
    for i in Camera_list:
        current_time = Camera_list[n]
        if current_time in Lidar_list:
            Both_list.append(current_time)
            Lidar_list.remove(current_time)
        n+=1
    return Both_list

def copy_file(old_path,new_path,appendix,item_list):
    for name in item_list:
        name=name+appendix
        from_path=os.path.join(old_path,name)
        to_path=new_path
        copy(from_path,to_path)

#按照指定位数时间戳重新命名
changename(Lpath,pcd_appendix)
changename(Cpath,jpg_appendix)
#寻找时间戳对齐的点云和图片,返回所有文件名的list
Both_list=find_timestap(Lpath,Cpath)
#从原文件夹复制到对齐文件夹中
copy_file(Lpath,New_L_path,pcd_appendix,Both_list)
copy_file(Cpath,New_C_path,jpg_appendix,Both_list)