相机和雷达外参联合标定
內容: 關于雷達和相機外參聯合標定的踩坑紀錄。
Date: 2023/03/19
硬件:
- 上位機: Jetson ORIN (Ubuntu 20.04, ROS noetic)
- 雷達: Ouster 32線
- 相機: Intel D435
一、 標定方案
目前流行的 雷達+相機 標定方案有五種：Autoware, apollo, lidar_camera_calibration, but_velodyne。
Ubuntu20.04安裝autoware我看bug比較多，因此我follow的是: https://github.com/ankitdhall/lidar_camera_calibration
1.1 opencv
這一步花了我一天時間，坑比較多，需要安裝opencv_contrib才能進行進一步的操作,僅僅安裝opencv是不行的!
我安的版本是3.4.10。
參考:
- https://blog.csdn.net/m0_73694897/article/details/129175515?spm=1001.2014.3001.5506
- https://blog.csdn.net/weixin_34910922/article/details/125248118?spm=1001.2014.3001.5506
二、 時間強行同步
直接實時或者直接播包的話就會發生: 只有mono8這一個窗口，沒有其他窗口彈出!
- Only Mono8 window visible · Issue #94 · ankitdhall/lidar_camera_calibration
- https://github.com/ankitdhall/lidar_camera_calibration/issues/65
- https://github.com/ankitdhall/lidar_camera_calibration/issues/53
因為車是靜止的，如果時間戳不完全一致，標定的程序是無法彈出點云窗口的，這個bug困擾了我很久，在issues中也沒找到也沒有人能給出解決方案，于是我把時間戳都強行設置到1970年，解決。
#!/usr/bin/env python3import rospy import rosbag import sysif sys.getdefaultencoding() != 'utf-8':reload(sys)sys.setdefaultencoding('utf-8') bag_name = 'subset_2023-03-19-15-52-03.bag' #被修改的bag名 out_bag_name = 'change.bag' #修改后的bag名 dst_dir = '/aigo/' #使用路徑with rosbag.Bag(dst_dir+out_bag_name, 'w') as outbag:stamp = None#topic:就是發布的topic msg:該topic在當前時間點下的message t:消息記錄時間(非header)##read_messages內可以指定的某個topicfor topic, msg, t in rosbag.Bag(dst_dir+bag_name).read_messages():if topic == '/ouster/points':stamp = msg.header.stamp# print("imu")#print(stamp)outbag.write(topic, msg, stamp)#針對沒有header的文件，使用上面幀數最高的topic(即:/gps)的時間戳##因為read_messages是逐時間讀取，所以該方法可以使用elif topic == '/camera/color/image_raw' and stamp is not None: #print("lidar")#print(msg.header.stamp)outbag.write(topic, msg, stamp)# continue# #針對格式為Header的topicelif topic == '/camera/depth/image_rect_raw' and stamp is not None:#print("image")#print(msg.header.stamp)outbag.write(topic, msg, stamp)# outbag.write(topic, msg, msg.stamp)# continue# #針對一般topic#outbag.write(topic, msg, msg.header.stamp)print("finished")- 前:
- 后:
三、 標定
我用的雷達是32線的，我修改了/home/nvidia/wuke/a2b_ws/src/lidar_camera_calibration/include/lidar_camera_calibration/PreprocessUtils.h:
// line 200 // std::vector <std::vector<myPointXYZRID *>> rings(16); std::vector <std::vector<myPointXYZRID *>> rings(32);- 播包:
- 標定:
四、 標定結果
-------------------------------------------------------------------- After 40 iterations -------------------------------------------------------------------- Average translation is: 0.00659326-0.241034-0.113246 Average rotation is:0.901867 0.00575031 0.431975 -0.0142371 0.999764 0.0164154-0.431778 -0.0209546 0.901736 Average transformation is: 0.901867 0.00575031 0.431975 0.00659326 -0.0142371 0.999764 0.0164154 -0.241034-0.431778 -0.0209546 0.901736 -0.1132460 0 0 1 Final rotation is:0.0174841 -0.999826 -0.006546440.0208512 0.00691063 -0.9997590.99963 0.0173434 0.0209684 Final ypr is: 0.873007 -1.543580.69106 Average RMSE is: 0.0291125 RMSE on average transformation is: 0.02988264.1 將雷達點云投影到相機坐標系
上面的位姿變換是從雷達坐標系到相機坐標系的變換。
我打算把雷達點云變換到相機坐標系下然后和深度圖對比，看重疊效果咋樣:
-
將D435的深度圖變成點云:
#include <ros/ros.h> #include <cv_bridge/cv_bridge.h> #include <sensor_msgs/Image.h> #include <sensor_msgs/CameraInfo.h> #include <sensor_msgs/PointCloud2.h> #include <pcl_conversions/pcl_conversions.h> #include <pcl/point_types.h> #include <pcl/PCLPointCloud2.h> #include <pcl/conversions.h> #include <iostream> using namespace std;ros::Publisher pub_point_cloud2;bool is_K_empty = 1; double K[9]; // [fx 0 cx] // K = [ 0 fy cy] // [ 0 0 1]void img_callback(const sensor_msgs::ImageConstPtr &img_msg) {// Step1: 讀取深度圖//ROS_INFO("image format: %s %dx%d", img_msg->encoding.c_str(), img_msg->height, img_msg->width);int height = img_msg->height;int width = img_msg->width;// 通過指針強制轉換，讀取為16UC1數據，單位是mmunsigned short *depth_data = (unsigned short*)&img_msg->data[0];// Step2: 深度圖轉點云sensor_msgs::PointCloud2 point_cloud2;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);for(int uy=0; uy<height; uy++){for(int ux=0; ux<width; ux++){float x, y, z;z = *(depth_data + uy*width + ux) / 1000.0;if(z!=0){x = z * (ux - K[2]) / K[0];y = z * (uy - K[5]) / K[4];pcl::PointXYZ p(x, y, z);cloud->push_back(p);}}}// Step3: 發布點云pcl::toROSMsg(*cloud, point_cloud2);point_cloud2.header.frame_id = "world";pub_point_cloud2.publish(point_cloud2); }void camera_info_callback(const sensor_msgs::CameraInfoConstPtr &camera_info_msg) {// 讀取相機參數if(is_K_empty){for(int i=0; i<9; i++){K[i] = camera_info_msg->K[i];}is_K_empty = 0;} }int main(int argc, char **argv) {ros::init(argc, argv, "ros_tutorial_node");ros::NodeHandle n;// 訂閱D435i的深度圖，在其回調函數中把深度圖轉化為點云，并發布出來ros::Subscriber sub_img = n.subscribe("/camera/depth/image_rect_raw", 100, img_callback);// 訂閱D435i的深度相機參數ros::Subscriber sub_cmara_info = n.subscribe("/camera/depth/camera_info", 1, camera_info_callback);pub_point_cloud2 = n.advertise<sensor_msgs::PointCloud2>("/d435i_point_cloud", 1000);ROS_INFO("Runing ...");ros::spin();return 0; } -
發布tf變換消息:
rosrun tf2_ros static_transform_publisher 0 0 0 1.57 -1.57 0 world os_sensor -
result:
-
raw
-
after (紅色是雷達點云，白色是深度相機的投影)
-
Reference
[1] https://github.com/ankitdhall/lidar_camera_calibration/wiki/Welcome-to-%60lidar_camera_calibration%60-Wiki!
[2] 3D雷達與相機的標定方法詳細教程_lidar_camera_calibration_敢敢のwings的博客-CSDN博客
[3] https://blog.csdn.net/m0_73694897/article/details/129175515?spm=1001.2014.3001.5506
[4] https://blog.csdn.net/weixin_34910922/article/details/125248118?spm=1001.2014.3001.5506
[5] https://blog.csdn.net/weixin_42840360/article/details/119844648?spm=1001.2014.3001.5506
[6] https://blog.csdn.net/Jeff_zjf/article/details/124255916?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522167922032616800213053412%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=167922032616800213053412&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allfirst_rank_ecpm_v1~pc_rank_34-1-124255916-null-null.142
總結
以上是生活随笔為你收集整理的相机和雷达外参联合标定的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 2019年第十届蓝桥杯决赛(国赛) C+
- 下一篇： Pixel2Mesh-Tensorflo