autoware lidar_localizer代码review（一）

最新推荐文章于 2024-10-07 06:31:57 发布

钟领军shell

最新推荐文章于 2024-10-07 06:31:57 发布

阅读量526

点赞数 4

文章标签：人工智能计算机视觉

本文链接：https://blog.csdn.net/weixin_39895621/article/details/141896590

版权

代码来自autoware.ai perception下的ndt_matching，代码包含了多种传感器，这里作简化，只关注odom和lidar
主要关注以下问题：

如何读取map，对map作了哪些处理
map-odom的初始位姿态怎么得到
如何利用odom给ndt作为初始值
总结

1. 如何读取map，对map作了哪些处理

在代码内搜索关键词setInputTarget，发现进入ndt匹配的是map_ptr，以此为线索看。
可以看到map是来自外部节点发布，在本节点内调用map_callback做到的。然后作了初始化ndt匹配器的操作。

static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
  // if (map_loaded == 0)
  if (points_map_num != input->width)
  {
    std::cout << "Update points_map." << std::endl;

    points_map_num = input->width;

    // Convert the data type(from sensor_msgs to pcl).
    pcl::fromROSMsg(*input, map);

    if (_use_local_transform == true)
    {
      tf2_ros::Buffer tf_buffer;
      tf2_ros::TransformListener tf_listener(tf_buffer);
      geometry_msgs::TransformStamped local_transform_msg;
      try
      {
        local_transform_msg = tf_buffer.lookupTransform("map", "world", ros::Time::now(), ros::Duration(3.0));
      }
      catch (tf2::TransformException& ex)
      {
        ROS_ERROR("%s", ex.what());
      }

      tf2::fromMsg(local_transform_msg, local_transform);
      pcl::transformPointCloud(map, map, tf2::transformToEigen(local_transform_msg).matrix().inverse().cast<float>());
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));

    // Setting point cloud to be aligned to.
    if (_method_type == MethodType::PCL_GENERIC)
    {
      pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      new_ndt.setResolution(ndt_res);
      new_ndt.setInputTarget(map_ptr);
      new_ndt.setMaximumIterations(max_iter);
      new_ndt.setStepSize(step_size);
      new_ndt.setTransformationEpsilon(trans_eps);

      new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());

      pthread_mutex_lock(&mutex);
      ndt = new_ndt;
      pthread_mutex_unlock(&mutex);
    }
    else if (_method_type == MethodType::PCL_ANH)
    {
      cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_anh_ndt;
      new_anh_ndt.setResolution(ndt_res);
      new_anh_ndt.setInputTarget(map_ptr);
      new_anh_ndt.setMaximumIterations(max_iter);
      new_anh_ndt.setStepSize(step_size);
      new_anh_ndt.setTransformationEpsilon(trans_eps);

      pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
      pcl::PointXYZ dummy_point;
      dummy_scan_ptr->push_back(dummy_point);
      new_anh_ndt.setInputSource(dummy_scan_ptr);

      new_anh_ndt.align(Eigen::Matrix4f::Identity());

      pthread_mutex_lock(&mutex);
      anh_ndt = new_anh_ndt;
      pthread_mutex_unlock(&mutex);
    }
#ifdef CUDA_FOUND
    else if (_method_type == MethodType::PCL_ANH_GPU)
    {
      std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
          std::make_shared<gpu::GNormalDistributionsTransform>();
      new_anh_gpu_ndt_ptr->setResolution(ndt_res);
      new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
      new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
      new_anh_gpu_ndt_ptr->setStepSize(step_size);
      new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);

      pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
      pcl::PointXYZ dummy_point;
      dummy_scan_ptr->push_back(dummy_point);
      new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);

      new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());

      pthread_mutex_lock(&mutex);
      anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
      pthread_mutex_unlock(&mutex);
    }
#endif
#ifdef USE_PCL_OPENMP
    else if (_method_type == MethodType::PCL_OPENMP)
    {
      pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_omp_ndt;
      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      new_omp_ndt.setResolution(ndt_res);
      new_omp_ndt.setInputTarget(map_ptr);
      new_omp_ndt.setMaximumIterations(max_iter);
      new_omp_ndt.setStepSize(step_size);
      new_omp_ndt.setTransformationEpsilon(trans_eps);

      new_omp_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());

      pthread_mutex_lock(&mutex);
      omp_ndt = new_omp_ndt;
      pthread_mutex_unlock(&mutex);
    }
#endif
    map_loaded = 1;
  }
}

2.map-odom的初始位姿态怎么得到

这实际上是由人为指定的,关注到initial_pose这个变量，在main函数一开始是0，后续则用param_callback传入参数指定。

3.如何利用odom给ndt作为初始值

整体的思路是从initial_pose开始，一直记录odom在一段时间内的变化量，不断叠加得到当下base在map坐标系下的位姿。
关键的变量是previous_pose，他表示上一个时刻odom估计的base的位置（map系下）的坐标所以这里需要探究他如何产生，以及如何被使用

首先看他如何使用

static void odom_calc(ros::Time current_time)
{
  static ros::Time previous_time = current_time;
  double diff_time = (current_time - previous_time).toSec();

  double diff_odom_roll = odom.twist.twist.angular.x * diff_time;
  double diff_odom_pitch = odom.twist.twist.angular.y * diff_time;
  double diff_odom_yaw = odom.twist.twist.angular.z * diff_time;

  current_pose_odom.roll += diff_odom_roll;
  current_pose_odom.pitch += diff_odom_pitch;
  current_pose_odom.yaw += diff_odom_yaw;

  double diff_distance = odom.twist.twist.linear.x * diff_time;
  offset_odom_x += diff_distance * cos(-current_pose_odom.pitch) * cos(current_pose_odom.yaw);
  offset_odom_y += diff_distance * cos(-current_pose_odom.pitch) * sin(current_pose_odom.yaw);
  offset_odom_z += diff_distance * sin(-current_pose_odom.pitch);

  offset_odom_roll += diff_odom_roll;
  offset_odom_pitch += diff_odom_pitch;
  offset_odom_yaw += diff_odom_yaw;

  predict_pose_odom.x = previous_pose.x + offset_odom_x;
  predict_pose_odom.y = previous_pose.y + offset_odom_y;
  predict_pose_odom.z = previous_pose.z + offset_odom_z;
  predict_pose_odom.roll = previous_pose.roll + offset_odom_roll;
  predict_pose_odom.pitch = previous_pose.pitch + offset_odom_pitch;
  predict_pose_odom.yaw = previous_pose.yaw + offset_odom_yaw;

  previous_time = current_time;
}

可以看到，首先通过计算两个odom之间的time（这里有个点一开始没看懂，认为current_time和previous_time一直都是相等的，但static的特点就是只初始化一次）。然后计算两帧odom之间的位姿变化，叠加到predict_pose_odom，因为previous_pose是一直从initial_pose(地图起点，实际上是人为给定)，因此实际上就是map下的base位姿预测。
2. 再看previous_pose如何更新

    // Update ndt_pose
    ndt_pose.x = t2(0, 3);
    ndt_pose.y = t2(1, 3);
    ndt_pose.z = t2(2, 3);
    mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);

    // Calculate the difference between ndt_pose and predict_pose
    predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
                              (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
                              (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));

    if (predict_pose_error <= PREDICT_POSE_THRESHOLD)
    {
      use_predict_pose = 0;
    }
    else
    {
      use_predict_pose = 1;
    }
    use_predict_pose = 0;

    if (use_predict_pose == 0)
    {
      current_pose.x = ndt_pose.x;
      current_pose.y = ndt_pose.y;
      current_pose.z = ndt_pose.z;
      current_pose.roll = ndt_pose.roll;
      current_pose.pitch = ndt_pose.pitch;
      current_pose.yaw = ndt_pose.yaw;
    }
    previous_pose.x = current_pose.x;
    previous_pose.y = current_pose.y;
    previous_pose.z = current_pose.z;
    previous_pose.roll = current_pose.roll;
    previous_pose.pitch = current_pose.pitch;
    previous_pose.yaw = current_pose.yaw;