slam入门1：单目相机与2D激光联合标定——优化调试总结

接上一篇博客： 博客链接

参考开源实现一和2004年论文（Extrinsic Calibration of a Camera and Laser Range Finder (improves camera calibration)）

调试优化总结：

1、标定时使用单个二维码（AprilTag Family 36h11），标定分别测试了6组, 10组, 12组, 15组标定板位姿，利用15组位姿时，标定效果最好（理论上越多越好，但是没有测试更多姿态），将算法标定结果与结构外参对比，旋转角相差1°以内，平移向量相差1~3cm（有点大）。

2、在获取标定板上的激光点时，首先截取激光点前方90度范围（应保证标定板在激光前方90度范围内），减少激光点数量，然后利用RANSAC拟合直线，求出标定板的直线点（即直线内点），最后为了消除与直线内点在同一条直线上的离群点（或噪点），通过从激光点中间点向两边遍历，求相邻激光点距离，若距离大于一个阈值，则移除远离中间点的离群点。

代码如下：

void FilterLaserPoints(
  const Eigen::Matrix3Xd& laser_points_origin,
  Eigen::Matrix3Xd& laser_points_filtered) {

  // filter noise:
  // from middle column to both ends to find noise point and remove it
  unsigned int forward_col = laser_points_filtered.cols() / 2;
  unsigned int backward_col = laser_points_filtered.cols() / 2;
  while(backward_col < laser_points_filtered.cols() - 1 || forward_col > 0) {
    // if distance of both points > 5cm, abandon the outliner
    if(backward_col < laser_points_filtered.cols() - 1) {
      double backward_dis = (laser_points_filtered.col(backward_col) -
        laser_points_filtered.col(backward_col + 1)).norm();
      if(backward_dis > 0.05) {
        Utilities::RemoveColumn(laser_points_filtered, backward_col + 1);
      } else {
        ++backward_col;
      }
    }
    if(forward_col > 0) {
      double forward_dis = (laser_points_filtered.col(forward_col) -
        laser_points_filtered.col(forward_col - 1)).norm();
      if (forward_dis > 0.05) {
        Utilities::RemoveColumn(laser_points_filtered, forward_col - 1);
        --backward_col;
      }
      --forward_col;
    }
  } // end while loop
}

3、算法改进：

（1）利用单个标定板上多个二维码标定（如下图），可以提高相机位姿计算精度，从而提高标定板平面方程计算精度；

（2）原04年论文方法是将标定板上的所有激光点到平面的距离作为优化对象，实现时可以改为标定板上激光线的两端点到平面的距离作为优化对象（效果更好），进一步优化可以将下图中的标定板的黑色边缘线检测出来，然后将激光线两端点到黑色边缘线的距离作为优化对象（理论上效果会更好，参见论文：An Algorithm for Extrinsic Parameters Calibration of a Camera and a Laser Range Finder Using Line Features）。

 

4、新想法：

（1）接着上面3中第（2）条讨论，进一步优化可以将图中的标定板的黑色边缘线检测出来，然后将激光线两端点到黑色边缘线的距离作为优化对象（理论上效果会更好，参见论文：An Algorithm for Extrinsic Parameters Calibration of a Camera and a Laser Range Finder Using Line Features）。怎么实现呢？

1）首先二维码坐标系是定义好的，所以4条边缘线的直线方程也是可以得到的（比如标定板最下面那条直线方向向量为

[1, 0, 0]），那么通过二维码坐标系与相机坐标系的变换可以将4条直线的方程转到相机坐标系下（类似将标定板平面方程从二维码坐标系转到相机坐标系，见博客），然后就可以求激光两端点到标定板边缘线的距离，进行优化。

2）那么怎么判断激光点与哪两条边缘线相交呢？首先激光线打在标定板平面时，与边缘线只有两个交点，所以只需分别计算激光两端点到四条边缘线的距离，对每个端点取最小的距离即可。