深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第七章作业

第七章

如果在IEKF中引入点面ICP作为观测方程，请说明观测方程的具体处理方式，以及IEKF和纯激光ICP之间的关系。

• 将点面ICP替换掉增量式NDT，残差计算也相应地需要改为点面距离。

• 点面 ICP 的误差函数为：

• 其中， n 为单位法向量
• 观测方程的一阶泰勒展开为：

• 其中 v 为高斯分布，单次点面 ICP 服从一维高斯分布：

• 要取观测的最大似然估计，可以使用最小化负对数来求：

• 最小二乘问题的高斯牛顿解法为：

• 那么按照状态变量的定义顺序，第 j 个点的残差相对于六个估计状态的雅克比矩阵为：

• HTVH, HTVr 矩阵：

• 整个IESKF滤波器的解算有ICP联系起来了。在这种程度上，完全可以将紧耦合系统看出带IMU预测的高维ICP，并且这些预测分布还会被推导至下一时刻。

实现基于点面ICP的IEKF LIO系统，给出实现代码和实验展示（注意local map的处理方式和NDT有所不同）。

• 使用第七章 3d_icp 以及类 loam 的 local_map_ 思维：

// lio_iekf.h 中添加
// ICP
Icp3d icp_;
CloudPtr local_map_ = nullptr;
std::deque<CloudPtr> local_map_buffer_;

• 主要对 Align() 函数进行修改:

  • 处理第一帧

  /// 处理第一帧
  if (flg_first_scan_) {
      local_map_buffer_.emplace_back(current_scan_);
      icp_.SetTarget(current_scan_);
  
      local_map_ = current_scan_;
  
      flg_first_scan_ = false;
      return;
  }
  

  • 计算 HTVH, HTVr 矩阵，并进行优化

  icp_.SetSource(current_scan_filter);
  ieskf_.UpdateUsingCustomObserve([this](const SE3 &input_pose, Mat18d &HTVH, Vec18d &HTVr) {
      icp_.ComputeResidualAndJacobians(input_pose, HTVH, HTVr);
  });
  
  // 在第7章 icp_3d 中增加函数 ComputeResidualAndJacobians() 计算HTVH, HTVr 矩阵
  void Icp3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr) {
      std::vector<int> index(source_->points.size());
      for (int i = 0; i < index.size(); ++i) {
          index[i] = i;
      }
  
      std::vector<bool> effect_pts(index.size(), false);   // 有效点
      std::vector<Eigen::Matrix<double, 1, 18>> jacobians(index.size());   // 雅可比
      std::vector<double> errors(index.size());   // 残差 r
  
  
      std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&, this](int idx) {
          auto q = ToVec3d(source_->points[idx]);
          Vec3d qs = input_pose * q;  // 转换之后的q
          std::vector<int> nn;
          kdtree_->GetClosestPoint(ToPointType(qs), nn, 5);  // 这里取5个最近邻
          if (nn.size() > 3) {
              // convert to eigen
              std::vector<Vec3d> nn_eigen;
              for (int i = 0; i < nn.size(); ++i) {
                  nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
              }
  
              Vec4d n;   // 拟合平面的法向量和截矩 Ax + By + Cz + D = 0 
              if (!math::FitPlane(nn_eigen, n)) {
                  // 失败的不要
                  effect_pts[idx] = false;
                  return;
              }
  
              // dis = n * P + d 
              double dis = n.head<3>().dot(qs) + n[3];
              if (fabs(dis) > options_.max_plane_distance_) {
                  // 点离的太远了不要
                  effect_pts[idx] = false;
                  return;
              }
  
              effect_pts[idx] = true;
  
              // build residual
              Eigen::Matrix<double, 1, 18> J;
              // （7.7）
              J.block<1, 3>(0, 0) = n.head<3>().transpose();
              J.block<1, 3>(0, 6) = -n.head<3>().transpose() * input_pose.so3().matrix() * SO3::hat(q);
  
              jacobians[idx] = J;
              errors[idx] = dis;
          } else {
              effect_pts[idx] = false;
          }
      });
  
      // 累加Hessian和error,计算dx
      double total_res = 0;
      int effective_num = 0;
  
      const double info_ratio = 1.0;  // 每个点反馈的info因子  防止结果向ndt倾斜
  
      auto H_and_err = std::accumulate(
              index.begin(), index.end(), std::pair<Mat18d, Vec18d>(Mat18d::Zero(), Vec18d::Zero()),
              [&jacobians, &errors, &effect_pts, &total_res, &effective_num, &info_ratio](const std::pair<Mat18d, Vec18d>& pre,
                                                                          int idx) -> std::pair<Mat18d, Vec18d> {
                  if (!effect_pts[idx]) {
                      return pre;
                  } else {
                      total_res += errors[idx] * errors[idx];     // 累积误差全用正数
                      effective_num++;
                      // 视觉slam（6.41）
                      return std::pair<Mat18d, Vec18d>(pre.first + jacobians[idx].transpose() * jacobians[idx] * info_ratio,
                                                  pre.second - jacobians[idx].transpose() * errors[idx] * info_ratio);
                  }
              });
  
      HTVH = H_and_err.first;
      HTVr = H_and_err.second;
  }
  

  • 限制速度:

  // 在优化结束后进行速度限制
  // 在 UpdateUsingCustomObserve() 函数中
  void NormalizeVelocity() {
      /// 限制v的变化
      /// 一般是-y 方向速度
      Vec3d v_body = R_.inverse() * v_;
      if (v_body[1] > 0) {
          v_body[1] = 0;
      }
      v_body[2] = 0;
  
      if (v_body[1] < -2.0) {
          v_body[1] = -2.0;
      }
  
      if (v_body[0] > 0.1) {
          v_body[0] = 0.1;
      } else if (v_body[0] < -0.1) {
          v_body[0] = -0.1;
      }
  
      v_ = R_ * v_body;
  }
  

  • 判断关键帧，体素滤波，更新 local_map_ 以及 ui显示

  auto current_nav_state = ieskf_.GetNominalState();
  
  // 每一帧都配准，但只把关键帧放入地图中
  // 若运动了一定范围，则把点云放入地图中
  SE3 current_pose = ieskf_.GetNominalSE3();
  SE3 delta_pose = last_pose_.inverse() * current_pose;
  
  if (delta_pose.translation().norm() > 1.0 || delta_pose.so3().log().norm() > math::deg2rad(10.0)) {
      // 将地图合入NDT中
      CloudPtr current_scan_world(new PointCloudType);
      pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
  
      local_map_buffer_.emplace_back(current_scan_world);
  
      // 保持一定数量的 local map
      if (local_map_buffer_.size() > options_.num_kfs_in_local_map_) {
          local_map_buffer_.pop_front();
      }
  
      local_map_.reset(new PointCloudType);
  
      // 根据当前队列重建local map 
      for (auto& s : local_map_buffer_) {
          *local_map_ += *s;
      }
      
      local_map_ = VoxelCloud(local_map_, 0.5);
  
      icp_.SetTarget(local_map_);
  
      last_pose_ = current_pose;
  }
      // 放入UI
  if (ui_) {
      // 分别更新点云和定位
      ui_->UpdateScan(current_scan_, current_nav_state.GetSE3());  // 转成Lidar Pose传给UI
      ui_->UpdateNavState(current_nav_state);
  }
  
  frame_num_++;
  

• nclt 20130110.bag 结果：

• 刚开始很准确，经过长廊等退化场景的时候很容易发散，产生重影，后期效果更差。

• 调了一段时间参数，依旧会发散，不知道该怎么解决

• 前期还可以

• 退化场景和后期产生重影和发散