该系列主要为对前期工作进行梳理，以后所进行的工作也会部分在此记录。

// voxel_filter过滤
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
voxel_grid_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
voxel_grid_filter.setInputCloud(base_map);
voxel_grid_filter.filter(*map_filtered);

struct Area
{
    std::string filename;
    double x_min;
    double y_min;
    double z_min;
    double x_max;
    double y_max;
    double z_max;
    sensor_msgs::PointCloud2 points;
};

pnh_.param<double>("margin", MARGIN, 100.0);  // 需要加载的区域的范围，比如velodyne16的有效距离是100m，则加载范围最好应>120m
pnh_.param<double>("update_interval", UPDATE_INTERVAL, 1000.0); // ms，更新频率
pnh_.param<std::string>("map_dir", MAP_DIR, "/map/"); // 存储点云文件的目录

ros::Subscriber current_sub = nh_.subscribe("/ndt/current_pose", 10, publish_current_pcd);
ros::Subscriber initial_sub = nh_.subscribe("initialpose", 1, publish_dragged_pcd);

sensor_msgs::PointCloud2 create_pcd(const geometry_msgs::Point &p) // 载入p±MARGIN区域内的点云
{
    AreaList wanted_AreaList = create_wantedAreaList(p);  // std::vector<Area>, 存储目标点云文件信息，此时并没有进行点云的io加载
    AreaList tmp_AreaList;

    bool cache_update = false;
    for (Area &area : wanted_AreaList) // cached_AreaList记录缓存的点云及其信息：由于两次发布之间大多数点云是重复的，因此使用cached_AreaList进行缓存，以减少IO次数
    {
        int index = is_in_cachedAreaList(area);
        if (index == -1)
        { // io
            Area in_area(area);
            pcl::io::loadPCDFile(MAP_DIR + area.filename, in_area.points);
            tmp_AreaList.push_back(in_area);
            cache_update = true;
        }
        else
        {
            Area in_area(cached_AreaList[index]);
            tmp_AreaList.push_back(in_area);
        }
    }
    if (cache_update)
    {
        cached_AreaList.clear();
        cached_AreaList = tmp_AreaList;  // 更新cached_AreaList
    }
    else
    {
        sensor_msgs::PointCloud2 msg_pcd;
        msg_pcd.width = 0;  // 若该次应发布的点云和上次的相同，则不发布
        return msg_pcd;
    }

    sensor_msgs::PointCloud2 msg_pcd;
    for (const Area &area : cached_AreaList)  // 构建需要更新发布的点云消息
    {
        if (msg_pcd.width == 0)
            msg_pcd = area.points;
        else
        {
            msg_pcd.width += area.points.width;
            msg_pcd.row_step += area.points.row_step;
            msg_pcd.data.insert(msg_pcd.data.end(), area.points.data.begin(), area.points.data.end());
        }
    }
    return msg_pcd;
}

void *thread_func(void *args)
{
    ros::NodeHandle nh_map;
    ros::CallbackQueue map_callback_queue;
    nh_map.setCallbackQueue(&map_callback_queue);

    sub_map_ = nh_map.subscribe<sensor_msgs::PointCloud2>("/points_map", 10, mapCB);
    ros::Rate ros_rate(10);
    while (nh_map.ok())
    {
        map_callback_queue.callAvailable(ros::WallDuration());
        ros_rate.sleep();
    }

    return nullptr;
}

void mapCB(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
    if (model_pc_num_ == msg->width)
    {
        // suppose it is same map.
        return;
    }
    if (!pose_init_)
    {
        ROS_WARN("initial pose not set, cannot update target_map");
        return;
    }
    model_pc_num_ = msg->width;
    pcl::fromROSMsg(*msg, model_pc_);
    PointCloudT::Ptr target_map_ptr(new PointCloudT(model_pc_)); // 存储用于更新ndt的target_map

    // set NDT target
    if (param_method_type_ == METHOD_CUDA)
    {
#ifdef CUDA_FOUND
        std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr = std::make_shared<gpu::GNormalDistributionsTransform>();
        new_anh_gpu_ndt_ptr->setResolution(param_ndt_resolution_);
        new_anh_gpu_ndt_ptr->setInputTarget(target_map_ptr);
        new_anh_gpu_ndt_ptr->setMaximumIterations(param_ndt_max_iterations_);
        new_anh_gpu_ndt_ptr->setStepSize(param_ndt_step_size_);
        new_anh_gpu_ndt_ptr->setTransformationEpsilon(param_ndt_epsilon_);

        PointCloudT::Ptr dummy_scan_ptr(new PointCloudT());
        PointT 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;  // 此步将更新主线程中的ndt实例
        pthread_mutex_unlock(&mutex);

#else
        ROS_ERROR("param method_type set to cuda, but cuda_found not defined!");
#endif
    }
    map_init_ = true;
}