The problem of modifying the coordinate system of point cloud image loaded by ndtmatching function in autoware

autoware in ndtmatching Loading point cloud image coordinate system correction problem

autoware Midpoint cloud and vector （ Hyperfine ） Maps are map system
In our mapping practice, the point cloud map created is inconsistent with the site
If the alignment correction method is adopted, create a new system, such as map3d To accommodate some clouds
Then modify the release map3d To world The transformation of makes Align the point cloud with the site

The coordinate system is shown in the figure ：
Pictured , The red box on the ground is the site boundary , Light blue marks the wall
The coordinate system of the loaded point cloud is map3d, After adjustment map3d To world（map） Transformation of systems , The point cloud has coincided with the site .
But at this time ndt_matching, After giving the initial position, the positioning will fail .
To solve this problem , It needs to be changed ndtmatching Processing of point cloud when loading it , Namely the map3d Point cloud of goes to map Next .

Main code ：
if (input->header.frame_id == “map3d”)
{
tf::TransformListener local_transform_listener;
try
{
ros::Time now = ros::Time(0);
local_transform_listener.waitForTransform(“/map”, “/map3d”, now, ros::Duration(10.0));
local_transform_listener.lookupTransform(“/map”, “map3d”, now, local_transform_map_map3d);
}
catch (tf::TransformException& ex)
{
ROS_ERROR(“%s”, ex.what());
}
pcl_ros::transformPointCloud(map, map, local_transform_map_map3d);
}

Add the positioning effect after conversion

map_callback Callback function of point cloud topic

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);
    //zzz20220722----------------------------begin
    if (input->header.frame_id == "map3d")
    {
    
      tf::TransformListener local_transform_listener;
      try
      {
    
        ros::Time now = ros::Time(0);
        local_transform_listener.waitForTransform("/map", "/map3d", now, ros::Duration(10.0));
        local_transform_listener.lookupTransform("/map", "map3d", now, local_transform_map_map3d);
      }
      catch (tf::TransformException& ex)
      {
    
        ROS_ERROR("%s", ex.what());
      }
      pcl_ros::transformPointCloud(map, map, local_transform_map_map3d);
    }
    //zzz20220722----------------------------end
    if (_use_local_transform == true)
    {
    
      tf::TransformListener local_transform_listener;
      try
      {
    
        ros::Time now = ros::Time(0);
        local_transform_listener.waitForTransform("/map", "/world", now, ros::Duration(10.0));
        local_transform_listener.lookupTransform("/map", "world", now, local_transform);
      }
      catch (tf::TransformException& ex)
      {
    
        ROS_ERROR("%s", ex.what());
      }

      pcl_ros::transformPointCloud(map, map, local_transform.inverse());
    }

    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;
  }
}