apollo 定位localization 代码NDT算法模块解析（二）

ndt_localization.cc

---

---

前言

这里开始正是进入NDT算法。
主要内容：
倆个入口函数，分别为odomCallback()和LidarCallback()；
由LidarCallback（）->Update（）函数；
lidar_locator_ndt 中的Update()函数为点云匹配算法入口，相当于算法main()函数；

一、OdometryCallback

把组合导航输出的定位结果当里程计用,并用来做瞬时定位，即当前定位结果=上一时刻激光定位的结果+时间间隔内里程计变化；
保存组合导航定位结果到链表list中,便于激光数据输入时查找和插值，主要根据时间戳进行判断，因为odometry的输出频率较高，一般是100—200hz,而激光点云数据的频率较低，一般是10-20Hz,10hz的比较普遍。
最终得到高频率的实时定位结果。

double odometry_time = odometry_msg->header().timestamp_sec(); //获取时间戳
  static double pre_odometry_time = odometry_time;
  double time_delay = odometry_time - pre_odometry_time;
  if (time_delay > 0.1) { 
    AINFO << "Odometry message loss more than 10ms, the pre time and cur time: "
          << pre_odometry_time << ", " << odometry_time;
  } else if (time_delay < 0.0) {
    AINFO << "Odometry message's time is earlier than last one, "
          << "the pre time and cur time: " << pre_odometry_time << ", "
          << odometry_time;
  }
  pre_odometry_time = odometry_time;
  Eigen::Affine3d odometry_pose = Eigen::Affine3d::Identity();
  if (odometry_msg->has_localization()) {
    odometry_pose.translation()[0] =
        odometry_msg->localization().position().x();
    odometry_pose.translation()[1] =
        odometry_msg->localization().position().y();
    odometry_pose.translation()[2] =
        odometry_msg->localization().position().z();
    Eigen::Quaterniond tmp_quat(
        odometry_msg->localization().orientation().qw(),
        odometry_msg->localization().orientation().qx(),
        odometry_msg->localization().orientation().qy(),
        odometry_msg->localization().orientation().qz());
    odometry_pose.linear() = tmp_quat.toRotationMatrix();
  }
  if (ZeroOdometry(odometry_pose)) {
    AINFO << "Detect Zero Odometry";
    return;
  }
//保存组合导航输出的定位结果当作里程计来使用
  TimeStampPose timestamp_pose;
  timestamp_pose.timestamp = odometry_time;
  timestamp_pose.pose = odometry_pose;
  {
    std::lock_guard<std::mutex> lock(odometry_buffer_mutex_);
    if (odometry_buffer_size_ < max_odometry_buffer_size_) {
      odometry_buffer_.push_back(timestamp_pose);
      odometry_buffer_size_++;
    } else {
      odometry_buffer_.pop_front();
      odometry_buffer_.push_back(timestamp_pose);
    }
  }

  if (ndt_debug_log_flag_) {
    AINFO << "NDTLocalization Debug Log: odometry msg: "
          << std::setprecision(15) << "time: " << odometry_time << ", "
          << "x: " << odometry_msg->localization().position().x() << ", "
          << "y: " << odometry_msg->localization().position().y() << ", "
          << "z: " << odometry_msg->localization().position().z() << ", "
          << "qx: " << odometry_msg->localization().orientation().qx() << ", "
          << "qy: " << odometry_msg->localization().orientation().qy() << ", "
          << "qz: " << odometry_msg->localization().orientation().qz() << ", "
          << "qw: " << odometry_msg->localization().orientation().qw();
  }
//上一时刻激光点云的位置加上间隔时间内组合导航的相对位移作为当前时刻激光点云定位的预测值

  Eigen::Affine3d localization_pose = Eigen::Affine3d::Identity();
  if (lidar_locator_.IsInitialized()) {
    localization_pose =
        pose_buffer_.UpdateOdometryPose(odometry_time, odometry_pose);
  }
  ComposeLocalizationEstimate(localization_pose, odometry_msg,
                              &localization_result_);
  drivers::gnss::InsStat odometry_status;
  FindNearestOdometryStatus(odometry_time, &odometry_status);
  ComposeLocalizationStatus(odometry_status, &localization_status_);
  is_service_started_ = true;

二、LidarCallback（）

获取激光当前时间戳对应的组合导航数据；
将激光数据传入算法入口程序lidar_locator_ndt；

void NDTLocalization::LidarCallback(
    const std::shared_ptr<drivers::PointCloud>& lidar_msg) {
  static unsigned int frame_idx = 0;
  LidarFrame lidar_frame;
  //保存了点云坐标，强度，时间戳
  LidarMsgTransfer(lidar_msg, &lidar_frame);

  double time_stamp = lidar_frame.measurement_time;
  Eigen::Affine3d odometry_pose = Eigen::Affine3d::Identity();
  //获取对应时刻的odom（组合导航） 位置
  if (!QueryPoseFromBuffer(time_stamp, &odometry_pose)) {
    if (!QueryPoseFromTF(time_stamp, &odometry_pose)) {
      AERROR << "Can not query forecast pose";
      return;
    }
    AINFO << "Query pose from TF";
  } else {
    AINFO << "Query pose from buffer";
  }
  //判度是否完成初始化
  if (!lidar_locator_.IsInitialized()) {
    lidar_locator_.Init(odometry_pose, resolution_id_, zone_id_);
    return;
  }
  //NDT算法入口函数，更新，匹配，核心的东西都在这里面
  lidar_locator_.Update(frame_idx++, odometry_pose, lidar_frame);
  //获取点云匹配定位结果
  lidar_pose_ = lidar_locator_.GetPose();
  //保存点云定位与组合导航定位结果
  pose_buffer_.UpdateLidarPose(time_stamp, lidar_pose_, odometry_pose);
  ComposeLidarResult(time_stamp, lidar_pose_, &lidar_localization_result_);
  //获取匹配度，根据匹配度来判断定位结果是否可用
  ndt_score_ = lidar_locator_.GetFitnessScore();
  if (ndt_score_ > warnning_ndt_score_) {
    bad_score_count_++;
  } else {
    bad_score_count_ = 0;
  }
  if (ndt_debug_log_flag_) {
    Eigen::Quaterniond tmp_quat(lidar_pose_.linear());
    AINFO << "NDTLocalization Debug Log: lidar pose: " << std::setprecision(15)
          << "time: " << time_stamp << ", "
          << "x: " << lidar_pose_.translation()[0] << ", "
          << "y: " << lidar_pose_.translation()[1] << ", "
          << "z: " << lidar_pose_.translation()[2] << ", "
          << "qx: " << tmp_quat.x() << ", "
          << "qy: " << tmp_quat.y() << ", "
          << "qz: " << tmp_quat.z() << ", "
          << "qw: " << tmp_quat.w();

    Eigen::Quaterniond qbn(odometry_pose.linear());
    AINFO << "NDTLocalization Debug Log: odometry for lidar pose: "
          << std::setprecision(15) << "time: " << time_stamp << ", "
          << "x: " << odometry_pose.translation()[0] << ", "
          << "y: " << odometry_pose.translation()[1] << ", "
          << "z: " << odometry_pose.translation()[2] << ", "
          << "qx: " << qbn.x() << ", "
          << "qy: " << qbn.y() << ", "
          << "qz: " << qbn.z() << ", "
          << "qw: " << qbn.w();
  }
}

1 ，QueryPoseFromBuffer(double time, Eigen::Affine3d* pose)

获取激光点云时间戳时刻的odom位置，包括坐标（x,y,z）和角度（roll,pitch,yaw）

 CHECK_NOTNULL(pose);

  TimeStampPose pre_pose;
  TimeStampPose next_pose;
  {
    std::lock_guard<std::mutex> lock(odometry_buffer_mutex_);
    //获取最后时刻的odom 数据
    TimeStampPose timestamp_pose = odometry_buffer_.back();
    // check abnormal timestamp 
    //检查时间戳，确保激光数据是在odom数据前的，即是过去的值
    
    if (time > timestamp_pose.timestamp) {
      AERROR << "query time is newer than latest odometry time, it doesn't "
                "make sense!";
      return false;
    }
    // search from reverse direction
    //确保激光时间点前后都有odom
    auto iter = odometry_buffer_.crbegin();
    for (; iter != odometry_buffer_.crend(); ++iter) {
      if (iter->timestamp < time) {
        break;
      }
    }
    if (iter == odometry_buffer_.crend()) {
      AINFO << "Cannot find matching pose from odometry buffer";
      return false;
    }
    pre_pose = *iter;
    next_pose = *(--iter);
  }
  // interpolation
  //根据时间差值求比例，等比例求解位置和角度
  double v1 =
      (next_pose.timestamp - time) / (next_pose.timestamp - pre_pose.timestamp);
  double v2 =
      (time - pre_pose.timestamp) / (next_pose.timestamp - pre_pose.timestamp);
  pose->translation() =
      pre_pose.pose.translation() * v1 + next_pose.pose.translation() * v2;

  Eigen::Quaterniond pre_quat(pre_pose.pose.linear());
  //四元素转欧拉角
  common::math::EulerAnglesZXYd pre_euler(pre_quat.w(), pre_quat.x(),
                                          pre_quat.y(), pre_quat.z());

  Eigen::Quaterniond next_quat(next_pose.pose.linear());
  common::math::EulerAnglesZXYd next_euler(next_quat.w(), next_quat.x(),
                                           next_quat.y(), next_quat.z());

  double tmp_euler[3] = {};
  //等比例求解角度
  tmp_euler[0] = pre_euler.pitch() * v1 + next_euler.pitch() * v2;
  tmp_euler[1] = pre_euler.roll() * v1 + next_euler.roll() * v2;
  tmp_euler[2] = pre_euler.yaw() * v1 + next_euler.yaw() * v2;
  //欧拉角转四元素
  common::math::EulerAnglesZXYd euler(tmp_euler[1], tmp_euler[0], tmp_euler[2]);
  
  Eigen::Quaterniond quat = euler.ToQuaternion();
  quat.normalize();
  pose->linear() = quat.toRotationMatrix();//转化为旋转矩阵
  return true;
}

---

总结

下一章为lidar_locator_ndt.cc

版权申明：转载请注明出处，严禁用于商业用途。