相机与激光雷达标定1 LCCT

1. 算法整体流程

这篇文章梳理下《Fast Extrinsic Calibration of a Laser Rangefinder to a Camera》算法流程。整体流程如下：

2. 预处理

lcct分两阶段对外参进行优化，这里棋盘格点云分割采用cloudcompare进行手动分割，得到棋盘格点云以及对应的棋盘格图片。

2.1 棋盘格在相机坐标系下的法向量与距离

角点检测
以棋盘格建立世界坐标系，pnp获取棋盘格外参 \(R、t\)
将棋盘格角点通过内外参投影到相机坐标系下

2.1.1 法向量计算

相机坐标系下的法向量 \(n_c\) 与世界坐标系 \(Z_w\) 轴方向相关，即 \(n_c=R⋅[0,0,1]^T\)（\(R\) 的第三列）

2.1.2 距离计算

因为平面方程为 \(n_c·{X_c} + d=0\)
其中：

\(X_c\)可取相机坐标系下任意坐标
距离\(d = -n_c·t\),\(t\)为平移向量

2.1.3 实现过程

实现过程如下(已从像素坐标转到相机坐标)：

cv::Mat rvec, tvec;
cv::solvePnP(obj_pts, image_pts[i], cv::Mat::eye(3, 3, CV_32F), cv::Mat(), rvec,
             tvec, false, cv::SOLVEPNP_ITERATIVE);

rvec.convertTo(rvec, CV_32F);
tvec.convertTo(tvec, CV_32F);

rvecs.push_back(rvec.clone());
tvecs.push_back(tvec.clone());
cv::Mat r;
cv::Rodrigues(rvec, r);
r.convertTo(r, CV_32F);
normal_vector_dist[i] << r.ptr<float>(0)[2], r.ptr<float>(1)[2],
    r.ptr<float>(2)[2], tvec.dot(cv::Mat(r.col(2)));

2.2 棋盘格在雷达坐标系下的法向量与距离

利用分割好的点云拟合点云平面，具体参考平面拟合

cpp实现

int LcctLidarCalib::FitPlaneTLS(const std::vector<cv::Point3f> &obj_pts,
                                Eigen::Vector4f &coeff) {
  int ret = 0;
  if (obj_pts.size() < 4) {
    return -1;
  }
  float x_sum = 0.0f, y_sum = 0.0f, z_sum = 0.0f;
  for (const auto pt : obj_pts) {
    x_sum += pt.x;
    y_sum += pt.y;
    z_sum += pt.z;
  }
  x_sum /= obj_pts.size();
  y_sum /= obj_pts.size();
  z_sum /= obj_pts.size();
  // 拟合一个平面，返回Mat  ax + by +cz - d = 0   : abcd
  cv::Mat A = cv::Mat(obj_pts.size(), 3, CV_32FC1);

  for (int i = 0; i < obj_pts.size(); i++) {
    A.at<float>(i, 0) = obj_pts[i].x - x_sum;
    A.at<float>(i, 1) = obj_pts[i].y - y_sum;
    A.at<float>(i, 2) = obj_pts[i].z - z_sum;
  }
  A = A.t() * A;
  cv::Mat S, U, Vt;
  cv::SVDecomp(A, S, U, Vt);
  cv::Mat theta = Vt.row(2).clone();
  cv::Mat centroid = (cv::Mat_<float>(3, 1) << x_sum, y_sum, z_sum);
  cv::Mat alpha = theta * centroid;
  float d = alpha.ptr<float>(0)[0];
  if (d < 0) {
    theta *= -1;
    d *= -1;
  }
  for (int i = 0; i < 3; ++i) {
    coeff[i] = theta.ptr<float>(0)[i];
  }
  coeff[3] = d;
  return ret;
}

注意：

d的方向

3. Stage1 参数初始化

在第二章中都得到了棋盘格在相机坐标系下与激光雷达坐标下的法向量与距离，基于这个输入分步计算初始化旋转矩阵与平移向量

3.1 输入

数据输入：

相机坐标系下标定板的法向量和相机中心距离标定板的距离；
激光雷达坐标系下标定板的法向量和激光雷达中心距离标定板的距离

3.2 初始平移向量计算

输入已知相机原点与棋盘格平面的距离，假设在相机坐标系下的位置t，那么这个位置距离棋盘格平面的距离为\(\alpha_t=\alpha_c-\theta_c^T t\) (\(\theta_c\)为归一化后的结果).

假定相机到标定板的距离，加上相机到激光雷达的距离之和（都为相对标定板，且本文叙述时激光雷达在标定板后面d>0） \(\alpha_c-\theta_c^T t\)，与激光雷达到标定板的距离之差应为0，得到平移向量t，此向量为相机坐标系下激光雷达的位置；

所以最终有关于平移向量t（lidar在camera下的位置）优化目标函数：\(\alpha_{c} - \theta_c^T*t\)得到雷达到棋盘格平面的距离:

\[\min_{t} \sum_{i}{(\alpha_{l,i} - (\alpha_{c,i} - \theta_{c,i}^Tt))^2} \]

整理为矩阵形式：

\[t_1=arg\min_t|| \theta_c^T t - (\alpha_c - \alpha_l) ||_F^2 \]

得到最终的解为：

\[t_1=(\theta_c\theta_c^T)^{-1}\theta_c(\alpha_c - \alpha_l) \]

3.3 初始旋转矩阵的计算

激光雷达坐标系下的标定板法向量，经过旋转矩阵R旋转，与相机坐标系下标定板的法向量的夹角应为0，余弦值为1(最大)；

\[R_1=arg\max_R\sum_{i}{\theta_{c,i}^T(R\theta_{l,i})} \]

R为旋转矩阵，有\(R^TR=I\)，且\(det(R)=1\)，标量的最大化，转换为矩阵优化形式有：

\[R_1=\max_{R}trace(\theta^T_cR\theta_l) \]

进一步转换为Orthogonal Procrustes Problem (OPP)问题，闭式解如下：

\[R_1=VU^T\\ \theta_l\theta_c^T=USV^T \]

证明过程见论文最后部分

3.4 实现

int LcctLidarCalib::Stage1ForInitExtrinsic(
    const std::vector<Eigen::Vector4f> &normal_vec_cam,
    const std::vector<Eigen::Vector4f> &normal_vec_lidar, cv::Mat &R, cv::Mat &t) {
  int ret = 0;

  const int plane_num = (int)normal_vec_cam.size();
  if (normal_vec_cam.size() != normal_vec_lidar.size() || plane_num < 5) {
    LOG(ERROR) << "expect normal_vec_cam.size() == normal_vec_lidar.size()";
    return -1;
  }
  cv::Mat cam_theta(plane_num, 3, CV_32F);
  cv::Mat cam_alpha(plane_num, 1, CV_32F);

  cv::Mat lidar_theta(plane_num, 3, CV_32F);
  cv::Mat lidar_alpha(plane_num, 1, CV_32F);

  for (int i = 0; i < plane_num; ++i) {
    for (int j = 0; j < 3; ++j) {
      cam_theta.ptr<float>(i)[j] = normal_vec_cam[i][j];
      lidar_theta.ptr<float>(i)[j] = normal_vec_lidar[i][j];
    }
    cam_alpha.ptr<float>(i)[0] = normal_vec_cam[i][3];
    lidar_alpha.ptr<float>(i)[0] = normal_vec_lidar[i][3];
  }

  // t_1 = (\theta_c \theta_c^T)^{-1}\theta_c (\alpha_c - \alpha_l)
  cam_theta = cam_theta.t();
  lidar_theta = lidar_theta.t();
  t = (cam_theta * cam_theta.t()).inv() * cam_theta * (cam_alpha - lidar_alpha);
  cv::Mat trace_left = lidar_theta * cam_theta.t();

  cv::Mat s, u, vt;
  cv::SVD::compute(trace_left, s, u, vt);
  R = vt.t() * u.t();
  return ret;
}

4. Stage2 参数优化

将激光雷达坐标系下的激光雷达点，经过R，t变换得到相机坐标系下的点，若该点在相机坐标系下的标定板中，则点到标定板的距离应为0，文章利用相机原点到变换后点的法向量，与相机坐标系下标定板的法向量相乘，得到投影距离，与相机原点到标定板的距离做差值，进行优化，得到最终的优化解。

4.1 内点选择

根据拟合的点云平面计算各点到平面的距离，排除误差大于设置阈值的点

关于阈值设置，可选择距离中值作为阈值进行过滤

4.2 优化目标函数

输入：一系列的删选后激光雷达棋盘格点云坐标，i表示第i张图片

\[ x_{l,i}=[x_{l,i}^{(0)} \space x_{l,i}^{(1)}\dots x_{l,i}^{(m)}] \]

激光雷达坐标点经过外参Rt变换到相机后，与原始相机坐标点的差值最小化，优化函数如下：

\[ arg\min_{R,t}{\sum_{i=1}^{n}\frac{1} {m(i)}\sum_{j=1}^{m(i)}{(\theta_{c,i}^T(Rx_{l,i}^{(j)} + t)} - \alpha_{c,i})^2} \]

4.3 标定效果

2025-09-07_12-50

选择的棋盘格就是普通相机标定的靶标，这篇文章没有提到如何设置参数指标去衡量标定结果的好坏，例如相机标定重投影误差多少像素表示结果可行。从上图可以看到上下边缘处有不少点在棋盘格外面，可能是点云噪声引起。

在设计标定靶的时候可以参考opencalib中的标靶样式，通过拟合空心区域得到pnp结果，从而计算重投影误差进行度量。

posted @ 2025-09-07 12:47 wangnb 阅读(60) 评论(0) 收藏举报

刷新页面返回顶部

wangnb