激光雷达和相机联合标定
时间：2023-06-01 07:07:00
前言
最近做了激光和相机的联合校准，分别使用c艹 pcl与python open3d写了写linux/windows记录相应的联合标定软件。
绪论
该方法以标定板为基础，目的是寻求传感器之间的外参。
小编的代码主要做了一件事，那就是选择每个传感器对应的关键点，比如激光中标定板角3d图像中标定板角的坐标和像素坐标。
整个过程分为以下四部分：
1. 提取标定板点云
2. 提取标定板点云和图像角点
- 解决外参矩阵
- 重投影验证标定结果
ps：open3d的api查起来比较麻烦，有很多pcl或eigen在open3d手鲁了一遍
1. 提取标定板点云
1.1 初筛
采用直通滤波初步限制点云范围(甚至根据航向角限制)opencv滑动条和pcl、open3d库，我们可以在限制范围后实时调整和显示点云。
然后用统计滤波器去除一些离群点
分别给下面python和c 的核心代码
python的
调用部分
# 直通滤波 create_trackbar() while 1: xyz = get_trackbar() point_cloud_copy = cloud_passthrough(point_cloud_numpy, xyz) pcd.points = o3d.utility.Vector3dVector(point_cloud_copy) vis.update_geometry(pcd) #open3d is terrible 这句话可有可无 vis.poll_events() vis.update_renderer() key = cv2.waitKey(1) if key == ord('q'): vis.remove_geometry(pcd) break # 统计滤波 out, _ = pcd.remove_statistical_outlier(20, 1) # parms: (统计时考虑的临近点数是否为离群点的阀值) out.paint_uniform_color([1, 1, 1]) vis.add_geometry(out) while 1: vis.update_geometry(out) vis.poll_events() vis.update_renderer() key = cv2.waitKey(1)
if key == ord('q'):
vis.remove_geometry(out)
break
函数实现
def cloud_passthrough(cloud_np, xyz):
x_min = xyz[0]
x_max = xyz[1]
y_min = xyz[2]
y_max = xyz[3]
z_min = xyz[4]
z_max = xyz[5]
cloud_np = cloud_np[cloud_np[:, 0] < x_max]
cloud_np = cloud_np[cloud_np[:, 0] > x_min]
cloud_np = cloud_np[cloud_np[:, 1] < y_max]
cloud_np = cloud_np[cloud_np[:, 1] > y_min]
cloud_np = cloud_np[cloud_np[:, 2] < z_max]
cloud_np = cloud_np[cloud_np[:, 2] > z_min]
return cloud_np
c++的
while(!viewer1->wasStopped())
{
// 判断标定板点云是否直通滤波完毕
if(chose_bd)
{
chose_bd = 0;
break;
}
// 每次都对原始点云进行直通滤波操作
pcl::copyPointCloud(*cloud, *cloud_show);
// 更新直通滤波参数
xyzMutex.lock();
x0 = x_min;
x1 = x_max;
y0 = y_min;
y1 = y_max;
z0 = z_min;
z1 = z_max;
xyzMutex.unlock();
// 进行直通滤波
cloudPassThrough(cloud_show, "y", y0, y1);
cloudPassThrough(cloud_show, "x", x0, x1);
cloudPassThrough(cloud_show, "z", z0, z1);
// 更新可视化viewer
viewer1->removePointCloud("test");
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_show, "z");
viewer1->addPointCloud(cloud_show,fildColor, "test");
boost::this_thread::sleep(boost::posix_time::microseconds(1000));
viewer1->spinOnce(100);
}
// 统计滤波 去除离群点
cloudStatisticalOutlierRemoval(cloud_show);
1.2 拟合平面并投影
初筛完标定板点云后，仍有离群点和波动存在，我们通过拟合平面，并且将原始点云投影于该平面的方法，让标定板点云在空间处于统一平面。
python的
调用部分
# 拟合平面
inlier_cloud = copy.copy(out)
plane_model, inliers = get_plane(inlier_cloud)
[a, b, c, d] = plane_model
print(f"Plane equation: {
a:.2f}x + {
b:.2f}y + {
c:.2f}z + {
d:.2f} = 0")
inlier_cloud = inlier_cloud.select_by_index(inliers)
inlier_cloud.paint_uniform_color([0, 1, 0])
vis.add_geometry(inlier_cloud)
while 1:
vis.update_geometry(inlier_cloud)
vis.poll_events()
vis.update_renderer()
key = cv2.waitKey(1)
if key == ord('q'):
vis.remove_geometry(inlier_cloud)
break
# 投影滤波
project_cloud = copy.copy(inlier_cloud)
project_cloud.points = project_filter(project_cloud, plane_model)
vis.add_geometry(project_cloud)
while 1:
vis.update_geometry(project_cloud)
vis.poll_events()
vis.update_renderer()
key = cv2.waitKey(1)
if key == ord('q'):
#vis.remove_geometry(inlier_cloud)
break
函数实现部分
def get_plane(pcd):
# parms:
# distance_threshold
# ransac_n
# num_iterations
plane_model, inliers = pcd.segment_plane(0.09, 10, 1000)
# dis应该在0.04-0.05，但是标定板中间凹陷
return plane_model, inliers
def project_filter(cloud, plane_model):
cloud = np.asarray(cloud.points)
[a, b, c, d] = plane_model
dis = a*a+b*b+c*c
x = copy.deepcopy(cloud[:, 0])
y = copy.deepcopy(cloud[:, 1])
z = copy.deepcopy(cloud[:, 2])
cloud[:, 0] = ((b*b+c*c)*x - a*(b*y + c*z + d))/dis
cloud[:, 1] = ((a*a+c*c)*y - b*(a*x + c*z + d))/dis
cloud[:, 2] = ((b*b+a*a)*z - c*(a*x + b*z + d))/dis
return o3d.utility.Vector3dVector(cloud)
c++de
//-------------------------- 模型估计 --------------------------
cout << "->正在估计平面..." << endl;
pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(new pcl::SampleConsensusModelPlane<PointT>(cloud_show)); //选择拟合点云与几何模型
pcl::RandomSampleConsensus<PointT> ransac(model_plane); //创建随机采样一致性对象
ransac.setDistanceThreshold(0.1); //设置距离阈值，与平面距离小于0.01的点作为内点
ransac.computeModel(); //执行模型估计
PointCloudT::Ptr cloud_plane(new PointCloudT);
//---------- 根据索引提取内点 ----------
// 方法1
vector<int> inliers; //存储内点索引的向量
ransac.getInliers(inliers); //提取内点对应的索引
pcl::copyPointCloud<PointT>(*cloud_show, inliers, *cloud_plane);
// 输出模型参数Ax+By+Cz+D=0
Eigen::VectorXf coefficient;
ransac.getModelCoefficients(coefficient);
cout << "平面方程为：\n"
<< coefficient[0] << "x + "
<< coefficient[1] << "y + "
<< coefficient[2] << "z + "
<< coefficient[3] << " = 0"
<< endl;
//----------------------- 可视化拟合结果 -----------------------
viewer1->addPointCloud<pcl::PointXYZ>(cloud_show, "cloud"); //添加原始点云
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud"); //颜色
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); //点的大小
viewer1->addPointCloud<pcl::PointXYZ>(cloud_plane, "plane"); //添加平面点云
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "plane"); //颜色
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane"); //点的大
// 创建投影器
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
// 由ax+by+cz+d=0设置投影面参数
coefficients->values.resize(4);
coefficients->values[0] = coefficient[0];
coefficients->values[1] = coefficient[1];
coefficients->values[2] = coefficient[2];
coefficients->values[3] = coefficient[3];
// 创建投影滤波对象
pcl::ProjectInliers<pcl::PointXYZ>proj;
// 设置对象对应的投影模型
proj.setModelType(pcl::SACMODEL_PLANE);
// 设置输入点云
proj.setInputCloud(cloud_plane);
// 设置模型对应的系数
proj.setModelCoefficients(coefficients);
// 执行投影滤波
PointCloudT::Ptr cloud_filtered(new PointCloudT);
proj.filter(*cloud_filtered);
做到这一步，我们搞个效果视频看一下，不然太抽象了
calibration
2. 标定板点云和图像角点提取
我们想通过3d to 2d，来简化点云聚类、拟合、最终寻找到角点的过程。
2.1 点云降维
我们先将投影得到同一平面上的点云进行平移、旋转，将其移动至xoy面，
经此操作，3d点云z坐标均为0，我们可以对点云进行图像层面操作。
c++的
// 将标定板点云旋转平移至xoy面
PointCloudT::Ptr cloud_hhh(new PointCloudT);
PointCloudT::Ptr cloud_rotate(new PointCloudT);
while(!viewer1->wasStopped())
{
// 判断投影是否完成
if(chose_bd)
{
viewer1->removeAllPointClouds();
PointCloudT::Ptr cloud_hhh(new PointCloudT);
PointCloudT::Ptr cloud_rotate(new PointCloudT);
// 计算过原点平面法线方程
float A = coefficient[0];
float B = coefficient[1];
float C = coefficient[2];
float D = coefficient[3];
float Z0 = -D/C;
float x0 = cloud_filtered->points[0].x;
float y0 = cloud_filtered->points[0].y;
float z0 = cloud_filtered->points[0].z;
// 将点云平面平移至原点所在平面
for (int i=0; i<cloud_filtered->points.size(); i++)
{
float x = cloud_filtered->points[i].x-x0;
float y = cloud_filtered->points[i].y-y0;
float z = cloud_filtered->points[i].z-z0;
PointT t0 = {
x, y, z};
cloud_hhh->points.push_back(t0);
}
// 法线可视化
PointT a,b;
a.x = -10*A;
a.y = -10*B;
a.z = -10*C;
b.x = 10*A;
b.y = 10*B;
b.z = 10*C;
viewer1->addLine<pcl::PointXYZ>(a, b, 255, 255, 255, "line1");
PointCloudT::Ptr linePoint(new PointCloudT);
linePoint->points.push_back(a);
linePoint->points.push_back(b);
// 将点云绕Y轴旋转
Eigen::Affine3f transform_2=Eigen::Affine3f::Identity();
float theta = - atan(A/C);
cout << theta*180/M_PI << endl;
transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitY()));
printf ("\n Method #2: using an Affine3f \n");
std::cout << transform_2.matrix() << std::endl;
pcl::transformPointCloud(*cloud_hhh, *cloud_rotate, transform_2);
// 绘制旋转后平面法线
pcl::transformPointCloud(*linePoint, *linePoint, transform_2);
a = linePoint->points[0];
b = linePoint->points[1];
viewer1->addLine<pcl::PointXYZ>(a, b, 255, 0, 0, "line2");
// 将点云绕X轴旋转
float new_B = b.y/10;
float new_C = b.z/10;
Eigen::Affine3f transform_1=Eigen::Affine3f::Identity();
float theta1 = atan(new_B/new_C);
cout << theta1*180/M_PI << endl;
transform_1.rotate(Eigen::AngleAxisf(theta1, Eigen::Vector3f::UnitX()));
std::cout << transform_1.matrix() << std::endl;
pcl::transformPointCloud(*cloud_rotate, *cloud_rotate, transform_1);
// 绘制旋转后平面法线
pcl::transformPointCloud(*linePoint, *linePoint, transform_1);
a = linePoint->points[0];
b = linePoint->points[1];
viewer1->addLine<pcl::PointXYZ>(a, b, 0, 255, 0, "line3");
python的
调用部分
# 平移旋转至xoy面
trans_cloud = copy.copy(project_cloud)
trans_cloud.points, euler, xyz0 = transform(trans_cloud, plane_model)
vis.add_geometry(trans_cloud)
while 1:
vis.update_geometry(trans_cloud)
vis.poll_events()
vis.update_re 
