一、手动标定
代码工程:GitHub - Livox-SDK/livox_camera_lidar_calibration: Calibrate the extrinsic parameters between Livox LiDAR and camera
这是Livox提供的手动校准Livox雷达和相机之间外参的方法,并在Mid-40,Horizon和Tele-15上进行了验证。其中包含了计算相机内参,获得标定数据,优化计算外参和雷达相机融合应用相关的代码。本方案中使用了标定板角点作为标定目标物,由于Livox雷达非重复性扫描的特点,点云的密度较大,比较易于找到雷达点云中角点的准确位置。相机雷达的标定和融合也可以得到不错的结果。
1.1 环境配置
我的基础环境为 ubuntu20.04 ros neotic pcl 1.10 提前还需要装好 Eigen Ceres
# install Livox_SDK
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
sudo ./third_party/apr/apr_build.sh
cd build && cmake ..
make
sudo make install
# install livox_ros_driver
cd ..
git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src
cd ws_livox
catkin_make
# install camera/lidar calibration package
cd src
git clone https://github.com/Livox-SDK/livox_camera_lidar_calibration.git
cd ..
catkin_make
source devel/setup.bash
编译过程中遇到的问题:
/usr/include/pcl-1.10/pcl/point_types.h: In function ‘const pcl::CPPFSignature& pcl::common::operator-=(pcl::CPPFSignature&, const float&)’:
/usr/include/pcl-1.10/pcl/point_types.h:574:1: error: ‘minusscalar’ is not a member of ‘pcl::traits’
574 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/pcl-1.10/pcl/point_types.h:574:1: error: ‘minusscalar’ is not a member of ‘pcl::traits’
574 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/pcl-1.10/pcl/point_types.h:574:1: error: ‘minusscalar’ is not a member of ‘pcl::traits’
574 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/pcl-1.10/pcl/point_types.h:574:1: error: ‘minusscalar’ is not a member of ‘pcl::traits’
574 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature,
解决办法:修改编译文件,将和c++11的位置进行修改
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3")
cmake_minimum_required(VERSION 2.8.3)
project(camera_lidar_calibration)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
#set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
pcl_conversions
pcl_ros
cv_bridge
)
find_package(PCL 1.10 REQUIRED)
find_package(OpenCV)
find_package(Threads)
find_package(Ceres REQUIRED)
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTOUIC ON)
set(CMAKE_AUTORCC ON)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
catkin_package(
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(cameraCalib src/cameraCalib.cpp)
target_link_libraries(cameraCalib ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(pcdTransfer src/pcdTransfer.cpp src/common.h)
target_link_libraries(pcdTransfer ${catkin_LIBRARIES} ${PCL_LIBRARIES})
add_executable(cornerPhoto src/corner_photo.cpp src/common.h)
target_link_libraries(cornerPhoto ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(getExt1 src/cam_lid_external1.cpp src/common.h)
target_link_libraries(getExt1 ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES})
add_executable(getExt2 src/cam_lid_external2.cpp src/common.h)
target_link_libraries(getExt2 ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES})
add_executable(projectCloud src/projectCloud.cpp src/common.h)
target_link_libraries(projectCloud ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(colorLidar src/color_lidar_display.cpp src/common.h)
target_link_libraries(colorLidar ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES})
作者使用的硬件设备为:
1.2 执行标定
1)相机内参标定
将拍摄了标定板的相机的图像,放在data/camera/photos下,然后在data/camera/in.txt中写入所有需要使用的照片名称。可以使用以下的代码实现:
"""
#-*-coding:utf-8-*-
# @author: wangyu a beginner programmer, striving to be the strongest.
# @date: 2022/7/7 20:25
"""
import os
# 文件夹路径
img_path = r'/home/nvidia/calibration_ws/src/livox_camera_lidar_calibration/data/camera/photos/'
# txt 保存路径
save_txt_path = r'./in.txt'
# 读取文件夹中的所有文件
imgs = os.listdir(img_path)
# 图片名列表
names = []
# 过滤:只保留png结尾的图片
for img in imgs:
if img.endswith(".png"):
names.append(img)
txt = open(save_txt_path,'w')
for name in names:
#name = name[:-4] # 去掉后缀名.png
txt.write(name + '\n') # 逐行写入图片名,'\n'表示换行
txt.close()
作者使用的标定板的样子为:
我使用的标定板, 内点是行向是9点和6点,边长是20mm
修改 对应的launch文件,如下进行修改:
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<param name="camera_in_path" value="/home/nvidia/calibration_ws/src/livox_camera_lidar_calibration/data/camera/in.txt" /> <!-- the file to contain all the photos -->
<param name="camera_folder_path" value="/home/nvidia/calibration_ws/src/livox_camera_lidar_calibration/data/camera/photos/" /> <!-- the file to contain all the photos -->
<param name="result_path" value="/home/nvidia/calibration_ws/src/livox_camera_lidar_calibration/data/camera/result.txt" /> <!-- the file to save the intrinsic data -->
<param name="row_number" type="int" value="9" /> <!-- the number of block on the row -->
<param name="col_number" type="int" value="6" /> <!-- the number of block on the column -->
<param name="width" type="int" value="20" /> <!-- the width of each block on the chessboard(mm) -->
<param name="height" type="int" value="20" /> <!-- the height of each block on the chessboard(mm)-->
<node pkg="camera_lidar_calibration" name="cameraCalib" type="cameraCalib" output="screen"></node>
</launch>
执行下面的语句进行标定:
roslaunch camera_lidar_calibration cameraCalib.launch
标定的结果会保存在data/camera/result.txt中,包括重投影误差,内参矩阵和畸变纠正参数。以上文件夹需要创建。
最终得到的结果如图所示:
2)准备图像中的角点坐标
首先需要把步骤2得到的内参和畸变纠正参数以下图的格式保存在data/parameters/intrinsic.txt文件下 [注 4]。distortion下面对应5个畸变纠正参数,按顺序是k1和k2 (RadialDistortion),p1和p2 (TangentialDistortion),最后一个是k3,一般默认是0
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<param name="intrinsic_path" value="/home/nvidia/calibration_ws/src/livox_camera_lidar_calibration/data/parameters/intrinsic.txt" /> <!-- intrinsic file -->
<param name="input_photo_path" value="/home/nvidia/LVI_ws/src/livox_camera_calib/data/frame4.png" /> <!-- photo to find the corner -->
<param name="ouput_path" value="/home/nvidia/calibration_ws/src/livox_camera_lidar_calibration/data/parameters/corner_photo.txt" /> <!-- file to save the photo corner -->
<node pkg="camera_lidar_calibration" name="cornerPhoto" type="cornerPhoto" output="screen"></node>
</launch>
2)相机和激光的手动标定
查看点云的方式
roslaunch livox_ros_driver livox_lidar_rviz.launch
需要录制rosbag时输入另一个命令
roslaunch livox_ros_driver livox_lidar_msg.launch
需要注意:保存的rosbag数据格式是customMsg,后续程序中处理rosbag是对应的“livox custom msg format”格式。