标定系列(三):lidar-gnss标定

文章An Extrinsic Calibration Method between LiDAR and GNSS/INS for Autonomous Driving,https://arxiv.org/pdf/2209.07694

代码 https://github.com/OpenCalib/LiDAR2INS

讲述的是lidar和gnss/ins之间的标定,当然作者用的是几何的方法来进行标定。

首先是问题陈述

1.生成全局坐标系的点云就通过这个变换来生成,TiL就是计算出来点在雷达坐标系中的坐标;

2.TIL就是雷达坐标系到INS的两者之间的相对变换参数,这个参数就是本篇文章所要求解的参数;3.还有一个参数就是TWI,就是把激光雷达点变换到INS当前坐标系后,还需要根据INS自身的位置和姿态,求解激光雷达点在世界坐标系下的位置。

该文章把标定分为了三个部分、分别是粗略的标定、标定优化和z轴矫正。

一、粗略的标定(使用滑动窗口内点到面距离最小来进行的标定)

作者用的滑动窗口,所以就是把1-n帧数据变换到第一帧所在的坐标系下。然后使用BALM中的王个划分方法,也就是八叉树划分方法,把点云划分到体素中,在每一个体素中进行平面拟合,然后在优化的时候就是直接优化的点到平面的距离最小。当然,在优化的过程中是没有优化平面参数的,只优化旋转和平移。

点到平面的距离可以表示为:

可以这样去想象,如果lidar和ins之间的相对位姿特别准,那么如果激光雷达扫描了一个平面,那么所生成的点云应该就基本上上在一个平面上,所以可以用平面这种结构来进行数据的约束和参数的优化。

现在看代码:

在每一个具有平面的体素内构建ceres优化程序。

可以看优化函数,就是首先把激光雷达点变换到ins坐标系下,然后在根据从ins获取的旋转和位置,变换到世界坐标系下,然后使用点到面的距离进行优化。

复制代码
struct BalmVoxelEnergy1 {
  BalmVoxelEnergy1(const Eigen::Vector3d &current_laser_point,
                   const Eigen::Vector3d &point_average,
                   const Eigen::Vector3d &normal,
                   const Eigen::Matrix4d &imu_tran)
      : current_laser_point_(current_laser_point),
        point_average_(point_average), normal_(normal), imu_tran_(imu_tran) {}

  ~BalmVoxelEnergy1() {}

  template <typename T>
  bool operator()(const T *const q, const T *const t, T *residual) const {
    T cur_p[3];
    cur_p[0] = T(current_laser_point_(0));  //当前激光点
    cur_p[1] = T(current_laser_point_(1));
    cur_p[2] = T(current_laser_point_(2));
    T world_p[3];
    ceres::QuaternionRotatePoint(q, cur_p, world_p); //变换到ins坐标系下
    // ceres::AngleAxisRotatePoint(q, cur_p, world_p);
    world_p[0] += t[0];
    world_p[1] += t[1];
    // world_p[2] += t[2];
    T tran_p[3];

    // n * (p - p_aver)  再变换到世界坐标系下
    tran_p[0] = imu_tran_(0, 0) * world_p[0] + imu_tran_(0, 1) * world_p[1] +
                imu_tran_(0, 2) * world_p[2] + imu_tran_(0, 3) -
                point_average_[0];
    tran_p[1] = imu_tran_(1, 0) * world_p[0] + imu_tran_(1, 1) * world_p[1] +
                imu_tran_(1, 2) * world_p[2] + imu_tran_(1, 3) -
                point_average_[1];
    tran_p[2] = imu_tran_(2, 0) * world_p[0] + imu_tran_(2, 1) * world_p[1] +
                imu_tran_(2, 2) * world_p[2] + imu_tran_(2, 3) -
                point_average_[2];

    residual[0] = normal_[0] * tran_p[0] + normal_[1] * tran_p[1] +
                  normal_[2] * tran_p[2];

    return true;
  }

  static ceres::CostFunction *Create(const Eigen::Vector3d &current_laser_point,
                                     const Eigen::Vector3d &point_average,
                                     const Eigen::Vector3d &normal,
                                     const Eigen::Matrix4d &imu_tran) {
    auto cost_function =
        new ceres::AutoDiffCostFunction<BalmVoxelEnergy1, 1, 4, 3>(
            new BalmVoxelEnergy1(current_laser_point, point_average, normal,
                                 imu_tran));
    return cost_function;
  }

  const Eigen::Vector3d current_laser_point_;
  const Eigen::Vector3d point_average_;
  const Eigen::Vector3d normal_;
  const Eigen::Matrix4d imu_tran_;
};

二、标定优化(使用占据体素数量最小来进行的标定)

体素占据最小来标定,这个比较好理解,因为参数越精确,分层现象越不可能,所以使用体素占据来优化参数。具体看论文描述。

接着看代码是怎么实现体素占据的,实际上不是构建的优化函数来优化的,实际上相对于是不断的尝试试出来的,这里面的iter*direction[j]和xyz_resolution和rpy_resolution是一些参数,这些参数构成了一个deta_transform,附加在粗参数基础上,然后一个参数一个参数的去实验,看看哪一个参数可以减少体素占用数量。最后,就把它作为最优参数。

复制代码
ar[2] = iter * direction[j] * rpy_resolution;
      std::cout << varName[2] << ": " << var[2] << std::endl;
      size_t cnt = ComputeVoxelOccupancy(var);
      if (cnt < min_voxel_occupancy * (1 - 1e-4)) {
        min_voxel_occupancy = cnt;
        bestVal[2] = var[2];
        std::cout << "points decrease to: " << min_voxel_occupancy << std::endl;

      } else {
        std::cout << "points increase to: " << cnt << std::endl;
        break;
      }
    }
  }
  var[2] = bestVal[2];
  // tx
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {

      var[3] = iter * direction[j] * xyz_resolution;
      std::cout << varName[3] << ": " << var[3] << std::endl;
      size_t cnt = ComputeVoxelOccupancy(var);
      if (cnt < min_voxel_occupancy * (1 - 1e-4)) {
        min_voxel_occupancy = cnt;
        bestVal[3] = var[3];
        std::cout << "points decrease to: " << min_voxel_occupancy << std::endl;

      } else {
        std::cout << "points increase to: " << cnt << std::endl;
        break;
      }
    }
  }
  var[3] = bestVal[3];
  // ty
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {

      var[4] = iter * direction[j] * xyz_resolution;
      std::cout << varName[4] << ": " << var[4] << std::endl;
      size_t cnt = ComputeVoxelOccupancy(var);
      if (cnt < min_voxel_occupancy * (1 - 1e-4)) {
        min_voxel_occupancy = cnt;
        bestVal[4] = var[4];
        std::cout << "points decrease to: " << min_voxel_occupancy << std::endl;

      } else {
        std::cout << "points increase to: " << cnt << std::endl;
        break;
      }
    }
  }
  var[4] = bestVal[4];
  for (size_t i = 0; i < 6; i++) {
    curr_best_extrinsic_[i] = bestVal[i];
  }
  std::cout << "roll: " << bestVal[0] << ", pitch: " << bestVal[1]
            << ", yaw: " << bestVal[2] << ", tx: " << bestVal[3]
            << ", ty: " << bestVal[4] << ", tz: " << bestVal[5] << std::endl;
  std::cout << "points: " << min_voxel_occupancy << std::endl;

  // calib result
  Eigen::Matrix4d deltaT = TransformUtil::GetDeltaT(bestVal);
  transform = lidar2imu_initial_ * deltaT;
  transform = transform.inverse().eval();

  return true;
}

三、z轴矫正(使用基准标志来进行z偏移标定)

这个不知道是否有必要,如果在粗标定中,如果体素中提取的平面不是都平行的,在xyz三个方向上都可以构成有效的约束的话,那么是不是就不用进行第三步了。

这一步骤的论文中的意思看下图:

就是用一些设备比如全站仪和rtk打一些地面点,然后册出来一些标志中心的点坐标,在激光雷达扫描到这些坐标的时候,直接使用这些坐标来矫正z方向的偏移。这一步相对来说高理解,但是感觉也是有点奇怪,如果打一些标志,那么只用来约束z-axis吗?

相关推荐
罗迪尼亚的熔岩2 个月前
kuka, fanuc, abb机器人和移动相机的标定
机器人·工业相机·海康威视·标定·visonmaster
charlee444 个月前
探究高空视频全景AR技术的实现原理
ar·全景·视频·相机·标定
光谷码农5 个月前
C#+halcon机器视觉九点标定算法
c#·halcon·机器视觉·标定
资深设备全生命周期管理7 个月前
相机不动,机构动作----Hands Eyes
标定
时凯凯凯凯8 个月前
T265相机双目鱼眼+imu联合标定(全记录)
数码相机·realsense·标定·t265
点PY8 个月前
camera和lidar外参标定
camera·标定·lidar
Begin,again9 个月前
【无标题】
python·计算机视觉·标定·结构光