This repository is used for automatic calibration between high resolution LiDAR and camera in targetless scenes.

Overview

livox_camera_calib

livox_camera_calib is a robust, high accuracy extrinsic calibration tool between high resolution LiDAR (e.g. Livox) and camera in targetless environment. Our algorithm can run in both indoor and outdoor scenes, and only requires edge information in the scene. If the scene is suitable, we can achieve pixel-level accuracy similar to or even beyond the target based method.

An example of a outdoor calibration scenario. We color the point cloud with the calibrated extrinsic and compare with actual image. A and C are locally enlarged views of the point cloud. B and D are parts of the camera image corresponding to point cloud in A and C.

Related paper

Related paper available on arxiv:
Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environments

Related video

Related video: https://youtu.be/sp6PZTL2StY

1. Prerequisites

1.1 Ubuntu and ROS

Ubuntu 64-bit 16.04 or 18.04. ROS Kinetic or Melodic. ROS Installation and its additional ROS pacakge:

    sudo apt-get install ros-XXX-cv-bridge ros-xxx-pcl-conversions

1.2 Eigen

Follow Eigen Installation

1.3 Ceres Solver

Follow Ceres Installation.

1.4 PCL

Follow PCL Installation. (Our code is tested with PCL1.7)

2. Build

Clone the repository and catkin_make:

cd ~/catkin_ws/src
git clone https://github.com/hku-mars/livox_camera_calib.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash

3. Run our example

Download Our recorded rosbag to your local path, and then change the path in calib.launch to your data path. Then directly run

roslaunch livox_camera_calib calib.launch

4. Run on your own sensor set

4.1 Record data

Record the point cloud and image msg to rosbag (15s or more for avia LiDAR). Then change the topic name in config_outdoor.yaml file

# Topic name in rosbag
PointCloudTopic: "/livox/lidar"
ImageTopic: "/camera/color/image_raw"

4.2 Modify some params

Modify the camera matrix and distortion coeffs in camera.yaml
Modify the initial extrinsic in config_outdoor.yaml if needed.

Issues
  • Issue with finding depth-continuous regions

    Issue with finding depth-continuous regions

    We were very impressed with the results seen running your sample datasets, but once we tried using our own data, the extrinsics calibration failed in 99% of tested scenarios. In every case, your tool miscalculates scene edges and shifts the image projection, in some cases, by as much as 50-60% off original estimates.

    For example, here is the projection of an ultra-HiRes image onto a cloud generated with the Livox Avia:

    image

    And here is the same cloud / image pair filtered through your calibration tool:

    image

    We provided the same exact intrinsics and initial extrinsics that were used to create the first projection, hoping that your tool would properly recognize scene edges and perfect / optimize the calibration. Instead, it identifies edges far away from their origin and attempts to shift the projection to match them. This results in a completely inaccurate calibration.

    Here is the planner cloud:

    image

    And the residuals seem to show that the lines are properly detected:

    image

    Nonetheless, the optimization shifts everything, producing the result I have posted above. If there are any settings you could recommend that might improve our result, please let me know. I presume that the reason your sample datasets work better is that they are based on scenes with very clean, broad lines, such as those found in artificial structures. We even tried indoor scenes with Aruco / checkerboard plates placed in various locations, and your tool does not recognize any of the lines in these plates. It completely ignores the plates and, as with the example above, "hunts" for edges far from the original extrinsic origin.

    Perhaps this tool is only applicable in industrial / city-centric settings where large buildings or artificial structures are present. We will attempt to re-test in a "cleaner" environment, but the fact that this tool cannot work with even standard Aruco / checkerboard patterns to properly orient itself makes it mostly unusable for the majority of contexts we would require.

    opened by VisionaryMind 13
  • os1 64 and hik vision camera caliberation.

    os1 64 and hik vision camera caliberation.

    Hi I have checked your git repository regarding lidar camera calibration. I am a research intern at my Home university NUST Pakistan. I have an os1 64 spinning lidar available at my lab. I need to calibrate the camera and lidar. Now kindly tell me the precise step I need to follow to get that calibration parameter. I don't know how to use Fast LIO for spinning lidar calibration. kindly outline the steps to follow to get extrinsic parameters with your package using os1 64 lidar. I would very thankful to you for this favor.I have bag file recorded with topic pointcloud2 and hikvision camera image. kindly help me in this regard

    opened by islamtalha01 6
  • Bad result of calibration

    Bad result of calibration

    Lidar: Livox Mid-100 Camera: Flir BlackFly Rosbag: https://cloud.tsinghua.edu.cn/f/09ceccd8e1c647f59650/?dl=1 Camera_config: https://cloud.tsinghua.edu.cn/f/2687d771b0ea421ea30d/?dl=1

    The calibration result seems promising after rough calibration, by collapsed after fine-tune optimization. rough_extrinsic optim_extrinsic

    opened by meng-zha 6
  • Process dies on LiDAR edge extraction

    Process dies on LiDAR edge extraction

    I have setup an environment with Ubuntu 18.04 and ROS Melodic. All dependencies have been installed, however, using your sample ROS bag files, I am not able to get past the Lidar edge extraction phase. Here is the terminal output:

    Camera Matrix: 
    [1364.45, 0, 958.327;
     0, 1366.46, 535.074;
     0, 0, 1]
    Distortion Coeffs: 
    [0.0958277;
     -0.198233;
     -0.000147133;
     -0.000430056;
     0]
    Init extrinsic: 
    [0, -1, 0, 0;
     0, 0, -1, 0;
     1, 0, 0, 0;
     0, 0, 0, 1]
    [ INFO] [1617231323.709287053]: Loading the rosbag /home/visionarymind/Documents/scans/livoxcalib/outdoor1.bag
    [ INFO] [1617231328.493049027]: Sucessfully load Point Cloud and Image
    [ INFO] [1617231328.493564736]: Building Voxel
    [ INFO] [1617231357.841487217]: Extracting Lidar Edge
    [lidar_camera_calib-2] process has died [pid 2586, exit code -11, cmd /home/visionarymind/vision_ws/devel/lib/livox_camera_calib/lidar_camera_calib /home/visionarymind/vision_ws/src/livox_camera_calib/config/camera.yaml /home/visionarymind/vision_ws/src/livox_camera_calib/config/config_outdoor.yaml /home/visionarymind/Documents/scans/livoxcalib/outdoor1.bag /home/visionarymind/vision_ws/src/livox_camera_calib/result __name:=lidar_camera_calib __log:=/home/visionarymind/.ros/log/2c3b8fe2-9274-11eb-9545-000c29df74c2/lidar_camera_calib-2.log].
    log file: /home/visionarymind/.ros/log/2c3b8fe2-9274-11eb-9545-000c29df74c2/lidar_camera_calib-2*.log
    

    Would you have any advice on how to correct this problem?

    opened by VisionaryMind 6
  • Rough Optimization looks good, then Optimization goes bad

    Rough Optimization looks good, then Optimization goes bad

    Hi there,

    Cool package, hoping to get this to work! I'm using an os1-64. When I launch the calib the Rough Optimization starts and is pretty good. However, when it moves onto the Optimization step, the point cloud is aligned very poorly. It seems when Optimization starts the cloud is rotated by some amount, maybe 90 degrees.

    Here are a couple photos. Please note that my camera images do not fit on my monitor so I can only show what I can see:

    Rough Optimization step: image

    Optimization Step: image

    Any thoughts or ideas would be appreciated. Thank you

    opened by pauldeee 4
  • pcl problem

    pcl problem

    Hi, thank you for your contribution to the society. When I run your code, I met a problem:

    [ INFO] [1626940489.756662588]: Loading the rosbag /home/catkin_ws/src/livox_camera_calib/data/merge.bag [ INFO] [1626940490.902931052]: Sucessfully load Point Cloud and Image [ INFO] [1626940490.903897494]: Building Voxel [ INFO] [1626940508.755300261]: Extracting Lidar Edge [ INFO] [1626940526.485055384]: Load all data! [ INFO] [1626940526.904265229]: Finish prepare! [pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!

    Do you know in which function this error happens? So I can check my own data. Thank you for your time.

    opened by Zhu-Liyuan 4
  • Question about vpnp cost function

    Question about vpnp cost function

    Dear author, thanks for your excellent work, but I am puzzled by the vpnp cost function, I did not understand why we need the line direction to compute the matrix V, and the physical meaning of this formula VRV.transpose

    1636686381(1)

    Look forward to your reply

    opened by RoseWillow 3
  • Issue while running on ros-kinetics based docker image on ubuntu machine18.04

    Issue while running on ros-kinetics based docker image on ubuntu machine18.04

    Hi, Because of lower version of PCL(1.17), i prepared a docker image(ros:kinetic-ros-core-xenial), with all dependencies. Also placed the respective files on path. While running i am getting the following error-

    [ INFO] [1629189514.066569998]: Loading the rosbag /opt/catkin_ws/src/livox_camera_calib/data/indoor1.bag process[rviz-3]: started with pid [381] process 381: D-Bus library appears to be incorrectly set up; failed to read machine uuid: UUID file '/etc/machine-id' should contain a hex string of length 32, not length 0, with no other text See the manual page for dbus-uuidgen to correct this issue. libGL error: No matching fbConfigs or visuals found libGL error: failed to load driver: swrast Could not initialize OpenGL for RasterGLSurface, reverting to RasterSurface. libGL error: No matching fbConfigs or visuals found libGL error: failed to load driver: swrast

    [rviz-3] process has died [pid 381, exit code -11, cmd /opt/ros/kinetic/lib/rviz/rviz -d /opt/catkin_ws/src/livox_camera_calib/rviz_cfg/calib.rviz __name:=rviz __log:=/root/.ros/log/81017cba-ff36-11eb-bf90-2c56dc38a7a2/rviz-3.log]. log file: /root/.ros/log/81017cba-ff36-11eb-bf90-2c56dc38a7a2/rviz-3*.log*

    [ INFO] [1629189515.516145283]: Sucessfully load Point Cloud and Image [ INFO] [1629189515.516546482]: Building Voxel [ INFO] [1629189539.422697927]: Extracting Lidar Edge [ERROR] [1629189544.520860218]: Can not load image from /opt/catkin_ws/src/livox_camera_calib/data/indoor1.bag [lidar_camera_calib-2] process has died [pid 374, exit code 255, cmd /opt/catkin_ws/devel/lib/livox_camera_calib/lidar_camera_calib /opt/catkin_ws/src/livox_camera_calib/config/camera.yaml /opt/catkin_ws/src/livox_camera_calib/config/config_outdoor.yaml /opt/catkin_ws/src/livox_camera_calib/data/indoor1.bag /opt/catkin_ws/src/livox_camera_calib/result __name:=lidar_camera_calib __log:=/root/.ros/log/81017cba-ff36-11eb-bf90-2c56dc38a7a2/lidar_camera_calib-2.log]. log file: /root/.ros/log/81017cba-ff36-11eb-bf90-2c56dc38a7a2/lidar_camera_calib-2*.log*

    I am not sure, why process has been died and telling indoor1.bag is not found but it has already started with the same.

    Any suggestions ? Thanks.

    opened by rahulsharma11 3
  • pnp_calib

    pnp_calib

    English version

    Thank you for the open source. This work is very perfect. However, I have the some question about the code. First, what's the vpnp. I don't know the Physical mean. Could you provide me reference? Second, why's the camera frame map into the pixel frame(image frame), you use the (u0-cx)/ fx. According to my knowledge, I would use (u0)/fx - cx. Thank you very much. image

    中文版 感謝您提供的開源程式碼,這個程式碼非常的棒 但是我有一些疑問是有關於code的部份

    1. 請問什麼是vpnp,我不太清楚其物理意義,請問能麻煩您提供我關於這個參考資料嘛?
    2. 在lidar_camera_calib.cpp裡面中,您透過相機內參投影到pixel frame,您使用的是(u0-cx)/ fx,您是先減掉像素中心在除以焦距長. 但照我知識中,應該是(u0)/fx - cx,先除以焦距長,在減掉像素的中心,請問我哪裡搞錯了嘛?
    opened by yanlong658 3
  • 关于vPnP的问题

    关于vPnP的问题

    非常感谢您的分享,有几个问题想请教下您:

    1. 下图这个公式可以解释下吗,代表什么含义?
    2. 代码里构建的残差公式是依据的下图这个公式,但是比这个公式多乘了一个V.transpose(),可以解释下吗?
    3. 方便的话,能否把这篇论文发我看看。地址: [email protected]

    再次感谢,期待您的回复

    image

    opened by codehory 2
  • Running with the latestest update fail

    Running with the latestest update fail

    Thank you for your works, it's a great idea to solve these kind of problems. I've run your newest code with the your indoor example , but it doesn't work. Screenshot from 2022-02-11 11-49-08 I'm pretty sure the config parameters are set to be correct. Could you please to check?

    opened by fuwei-li 2
  • 运行自己的数据错误。

    运行自己的数据错误。

    非常感谢作者所做的工作!我使用自己的数据集,我的传感器为livox-AVIA和灰点相机BFLY-PSG-23S63C。安装方式和论文中的一样。运行环境为ubuntu18.04,ros melodic ,pcl-1.8。在运行roslaunch multi_calib.launch时,终端报以下错误。 /home/xinyuc/图片/2022-08-02 17-55-46屏幕截图.png

    opened by chenxinyuzdshuai 2
  • 运行错误

    运行错误

    您好,我在使用kitti数据集时发生了错误,报错结果如下: OpenCV Error: Assertion failed (npoints >= 0 && (depth == 5 || depth == 6)) in projectPoints, file /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/calib3d/src/calibration.cpp, line 3246 terminate called after throwing an instance of 'cv::Exception' what(): /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/calib3d/src/calibration.cpp:3246: error: (-215) npoints >= 0 && (depth == 5 || depth == 6) in function projectPoints

    [lidar_camera_calib-2] process has died [pid 4744, exit code -6, cmd /home/zhengyang 0000000001.zip /Pixel_wise_Calibration/catkin_ws/devel/lib/livox_camera_calib/lidar_camera_calib __name:=lidar_camera_calib __log:=/home/zhengyang/.ros/log/b3f15002-f277-11ec-9ce8-7085c26a688b/lidar_camera_calib-2.log]. log file: /home/zhengyang/.ros/log/b3f15002-f277-11ec-9ce8-7085c26a688b/lidar_camera_calib-2*.log 我附上了我的Lidar文件。 谢谢!

    opened by zhangzh103 0
  • Empty argument `lidar_line_cloud_3d` passed to `buildVPnp()` cause a cv::Exception. 是因为点云密度太小没有提取到有效边缘吗?

    Empty argument `lidar_line_cloud_3d` passed to `buildVPnp()` cause a cv::Exception. 是因为点云密度太小没有提取到有效边缘吗?

    你好,我在运行程序的时候opencv抛出异常:

      what():  OpenCV(4.2.0) ../modules/core/src/matrix_expressions.cpp:23: error: (-5:Bad argument) Matrix operand is an empty matrix. in function 'checkOperandsExist'
    

    经过检查,错误发生在buildVPnp()(或者buildPnP(),当我把bool use_vpnp改为false。 在这个函数中,由于第五个参数lidar_line_cloud_3d的size为0,没有点能够进入std::vector<cv::Point3f> pts_3d,在进行投影时发生了错误。

    // src/include/lidar_camera_calib.hpp, 
    void Calibration::buildVPnp(
        const Vector6d &extrinsic_params, const int dis_threshold,
        const bool show_residual,
        const pcl::PointCloud<pcl::PointXYZ>::Ptr &cam_edge_cloud_2d,
        const pcl::PointCloud<pcl::PointXYZI>::Ptr &lidar_line_cloud_3d,
        std::vector<VPnPData> &pnp_list)
    {
      //....
      // project 3d-points into image view
      std::vector<cv::Point2d> pts_2d;
      // debug
      std::cout << "camera_matrix:" << camera_matrix << std::endl;
      std::cout << "distortion_coeff:" << distortion_coeff << std::endl;
      std::cout << "r_vec:" << r_vec << std::endl;
      std::cout << "t_vec:" << t_vec << std::endl;
      // here output 0 to screen.
      std::cout << "pts 3d size:" << pts_3d.size() << std::endl;
      // cv::Exception thrown here.
      cv::projectPoints(pts_3d, r_vec, t_vec, camera_matrix, distortion_coeff,
                        pts_2d);
      //...
    

    我没有足够的知识获得更多信息。请问这种情况是否因为我的点云密度太小或场景内没有足够的物体边缘而发生?我能否通过增加扫描时间(我使用非重复式扫描激光雷达livox)来避免这个问题?

    另外,ceres提示我许多接口已经被放弃或即将在2.2.0中被移除,全部都是警告,没有发生错误。

    cat /usr/local/include/ceres/version.h
    
    #define CERES_VERSION_MAJOR 2
    #define CERES_VERSION_MINOR 2
    #define CERES_VERSION_REVISION 0
    

    这里显示我使用的ceres版本为2.2.0,但LocalParameterization()并没有如warning信息所说的被移除。请问这些API变化是否会影响标定功能?

    opened by groundturtle 1
  • 雷达点云稀疏无法提取足够的边缘特征

    雷达点云稀疏无法提取足够的边缘特征

    今天尝试在仿真环境中进行标定,出现了以下的错误: OpenCV Error: Assertion failed (npoints >= 0 && (depth == CV_32F || depth == CV_64F)) in projectPoints, file /home/wjc/ws_opencv/opencv-3.4.16/modules/calib3d/src/calibration.cpp, line 3317 image 貌似是提取出来的边缘特征太少了,请问标定时对雷达或者拍摄的视角有什么建议吗。我仿真环境中雷达的点云比较稀疏。可以通过调节配置文件中的参数得到收敛的结果吗

    opened by SylaraAnh 2
Owner
HKU-Mars-Lab
Mechatronics and Robotic Systems (MaRS) Laboratory
HKU-Mars-Lab
ROS package to calibrate the extrinsic parameters between LiDAR and Camera.

lidar_camera_calibrator lidar_camera_calibrator is a semi-automatic, high-precision, feature-based camera and LIDAR extrinsic calibration tool. In gen

Networked Robotics and Sytems Lab 61 Aug 4, 2022
This repository uses a ROS node to subscribe to camera (hikvision) and lidar (livox) data. After the node merges the data, it publishes the colored point cloud and displays it in rviz.

fusion-lidar-camera-ROS 一、介绍 本仓库是一个ROS工作空间,其中ws_fusion_camera/src有一个工具包color_pc ws_fusion_camera │ README.md │ └───src │ └───package: c

hongyu wang 16 Jul 22, 2022
A generic and robust calibration toolbox for multi-camera systems

MC-Calib Toolbox described in the paper "MultiCamCalib: A Generic Calibration Toolbox for Multi-Camera Systems". Installation Requirements: Ceres, Boo

null 150 Jul 27, 2022
the implementations of 'A Flexible New Technique for Camera Calibration' and Bouguet's method

StereoCameraCalibration MonocularCameraCalibration/StereoCameraCalibration/StereoCameraRectification 1、Class "MonocularCameraCalibration" provides the

gtc1072 6 Apr 14, 2022
Harsh Badwaik 1 Dec 19, 2021
Windows 10 interface adjustment tool supports automatic switching of light and dark modes, automatic switching of themes and transparent setting of taskbar

win10_tools Windows 10 interface adjustment tool supports automatic switching of light and dark modes, automatic switching of themes and transparent s

Simon 1 Dec 3, 2021
calibrate a Livox LiDAR and a camera

Livox LiDAR-Camera Calibration This method is from the official method of Livox(https://github.com/Livox-SDK/livox_camera_lidar_calibration) It's just

null 9 Jul 23, 2022
A Gazebo based LiDAR-Camera Data Simulator.

Livox-LiDAR-Camera System Simulator A package to provide plug-in for Livox Series LiDAR. This work is inherited from EpsAvlc and LvFengchi's work: liv

zhijianglu 10 Apr 4, 2022
BAAF-Net - Semantic Segmentation for Real Point Cloud Scenes via Bilateral Augmentation and Adaptive Fusion (CVPR 2021)

Semantic Segmentation for Real Point Cloud Scenes via Bilateral Augmentation and Adaptive Fusion (CVPR 2021) This repository is for BAAF-Net introduce

null 84 Jul 17, 2022
Livox-Mapping - An all-in-one and ready-to-use LiDAR-inertial odometry system for Livox LiDAR

Livox-Mapping This repository implements an all-in-one and ready-to-use LiDAR-inertial odometry system for Livox LiDAR. The system is developed based

null 201 Jul 31, 2022
Visual-inertial-wheel fusion odometry, better performance in scenes with drastic changes in light

VIW-Fusion An visual-inertial-wheel fusion odometry VIW-Fusion is an optimization-based viusla-inertial-wheel fusion odometry, which is developed as a

庄庭达 189 Aug 6, 2022
A Robust LiDAR-Inertial Odometry for Livox LiDAR

LIO-Livox (A Robust LiDAR-Inertial Odometry for Livox LiDAR) This respository implements a robust LiDAR-inertial odometry system for Livox LiDAR. The

livox 300 Aug 2, 2022
Lidar-with-velocity - Lidar with Velocity: Motion Distortion Correction of Point Clouds from Oscillating Scanning Lidars

Lidar with Velocity A robust camera and Lidar fusion based velocity estimator to undistort the pointcloud. This repository is a barebones implementati

ISEE Research Group 136 Aug 8, 2022
Repository for building and operating REVOLVER: An automatic protein purification system for gravity columns. Developed at the University of Toronto.

REVOLVER: An automated protein purification system This repository contains all the hardware and firmware files to build and operate REVOLVER: an auto

Laboratory for Metabolic Systems Engineering 3 Jun 14, 2022
This project is used for lidar point cloud undistortion.

livox_cloud_undistortion This project is used for lidar point cloud undistortion. During the recording process, the lidar point cloud has naturally th

livox 51 Aug 9, 2022
Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras

Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras The pre-print version of our paper is available here. The pre-release code has

HKU-Mars-Lab 201 Aug 7, 2022
rlua -- High level bindings between Rust and Lua

rlua -- High level bindings between Rust and Lua

Amethyst Foundation 1.3k Aug 3, 2022
A real-time, direct and tightly-coupled LiDAR-Inertial SLAM for high velocities with spinning LiDARs

LIMO-Velo [Alpha] ?? [16 February 2022] ?? The project is on alpha stage, so be sure to open Issues and Discussions and give all the feedback you can!

Andreu Huguet 121 Aug 12, 2022
This repo contains source code of our paper presented in IROS2021 "Single-Shot is Enough: Panoramic Infrastructure Based Calibration of Multiple Cameras and 3D LiDARs"

Single-Shot is Enough: Panoramic Infrastructure Based Calibration of Multiple Cameras and 3D LiDARs Updates [2021/09/01] first commit, source code of

Alibaba 62 Jul 23, 2022