map_manager\map_manager.hpp 有一個小bug

[lonlonago]

void AddOriginMap(CloudPtr origin_map) {
    pcl::VoxelGrid<PointType> map_filter;
    map_filter.setLeafSize(global_map_filter_leaf_size, global_map_filter_leaf_size,
                           global_map_filter_leaf_size);
    map_filter.setInputCloud(origin_map);
    map_filter.filter(*origin_map);
    ROS_INFO("\033[1;35m Origin map points num after downsample: %lu \033[0m", origin_map->points.size());
    PubOriginMap(origin_map);

    for (auto &point : origin_map->points) {
        Voxel voxel(point.x, point.y, point.z, block_size);
        VoxelHashMap::iterator search = voxel_hash_map.find(voxel);
        if (search != voxel_hash_map.end()) {
            VoxelBlock &voxel_block = search.value();
            voxel_block.AddOriginPoint(Point3f(point.x, point.y, point.z));
        } else {
            VoxelBlock voxel_block(block_size, block_resolution, temporary_map_points_threshold);
            voxel_block.AddOriginPoint(Point3f(point.x, point.y, point.z));
            voxel_hash_map[voxel] = std::move(voxel_block);
        }
    }

    for (auto it = voxel_hash_map.begin(); it != voxel_hash_map.end(); ++it) {
        auto &voxel_block = it.value();
        voxel_block.InitOriginOctree();
    }

    std::cout << "Global map loaded. Total voxels: " << voxel_hash_map.size() << std::endl;
}

這個初始PCD地圖加載的部分缺少了 voxel_block.is_origin 標簽的設置，不知道是漏掉了還是沒有影響所以不用加，不是很確定

[M-Evanovic]

在AddOriginPoint中我们把先验地图也直接添加到static_map_points中，所以近邻搜索时可以直接从static_map_points中搜索到先验地图点。如果加上is_origin=true的话可以优先只搜索先验地图点，加不加取决于先验地图的可信度。
添加的话可以把
VoxelBlock voxel_block(block_size, block_resolution, temporary_map_points_threshold);
voxel_block.AddOriginPoint(Point3f(point.x, point.y, point.z));
voxel_hash_map[voxel] = std::move(voxel_block);
改为
VoxelBlock voxel_block(block_size, block_resolution, temporary_map_points_threshold);
voxel_block.AddOriginPoint(Point3f(point.x, point.y, point.z));
voxel_block.is_origin = true;
voxel_hash_map[voxel] = std::move(voxel_block);

[lonlonago]

懂了，因为加不加实际的效果是没有太大差别的，所有没有加