一、相机与激光雷达之间的标定
1.MATLAB2020b
(1)获取方式:百度网盘链接提取码:zadk
(2)安装教程:详细说明
2.详细步骤
官方教程:仔细瞅瞅
(1)搭建实验平台,实验操作系统Ubuntu16.04+ROS(kinetic)+velodyne(16线)+basler+棋盘格(10*7 41mm)。(ps:标定板没有白边,没有白边!)
(2)Ubuntu下打开激光雷达和相机,教程见前文,并录制数据bag。(ps:一定要激光雷达和相机的频率的一致!一定一定!)
(3)拍摄单目相机棋盘格图像,用MATLAB工具箱标定,获取相机的内参。(ps:传统的相机标定方法,标定板要留白边!)
(4)将数据bag中的激光雷达数据和相机数据分割出来。
代码如下:
bag→.pcd
#include <pcl_ros/point_cloud.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <iostream>
using namespace std;
int num = 1;
void lidar_callback(const sensor_msgs::PointCloud2ConstPtr &in_cloud_msg) {
// 1.Msg to pointcloud
pcl::PointCloud<pcl::PointXYZI> rawCloudIn;
pcl::fromROSMsg(*in_cloud_msg, rawCloudIn);
string pcdpath = "";//保存.pcd文件路径
std::string str=std::to_string(num);
//pcdpath += std::to_string(count++);
if(str.size()==1)
{
pcdpath+="000"+str+".pcd";
}
else if(str.size()==2)
{
pcdpath+="00"+str+".pcd";
}
else if(str.size()==3)
{
pcdpath+="0"+str+".pcd";
}
else
{
pcdpath+=str+".pcd";
}
num++;
pcl::io::savePCDFileASCII(pcdpath, rawCloudIn);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "GroundPlaneFit");
ros::NodeHandle nh;
ros::Subscriber lidar_sub = nh.subscribe("/velodyne_points", 1, lidar_callback); //订阅激光雷达点云
ros::spin();
return 0;
}
bag→.jpg
import roslib
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge