Method AddPoints

cpp/map_closures/VoxelMap.cpp:88–104

}

void VoxelMap::AddPoints(const Vector3dVector &points) {
    std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) {
        const Voxel voxel = ToVoxelCoordinates(point, voxel_size_);
        const auto [it, inserted] = map_.try_emplace(voxel, VoxelBlock());
        if (!inserted) {
            const VoxelBlock &voxel_block = it->second;
            if (voxel_block.size() == max_points_per_normal_computation ||
                std::any_of(voxel_block.cbegin(), voxel_block.cend(),
                            [&](const Eigen::Vector3d &voxel_point) {
                                return (voxel_point - point).norm() < map_resolution_;
                            })) {
                return;
            }
        }
        it->second.emplace_back(point);
    });
}

Vector3dVector VoxelMap::Pointcloud() const {
    Vector3dVector points;