std::vector<Eigen::Vector2d> centroids_unknown_motion_underk;
std::vector<Eigen::Vector2d> centroids_unknown_motion_k;
// 进行数字填充 

 

    pcl::visualization::PCLVisualizer viewer("Centroid Visualization");
    int id = 0;

    // 添加 XY 坐标系
    double coordinate_system_scale = 2.0; // 根据需要调整尺度
    viewer.addCoordinateSystem(coordinate_system_scale, "coordinate_system", 0);

    double sphere_radius = 1; // 球体半径

    // 随机数生成器
    std::random_device rd;  // 随机数种子
    std::mt19937 gen(rd()); // 随机数生成器
    std::uniform_real_distribution<> dis(0, 1); // 分布范围

    // 为匹配的中心点对添加球体
    for (size_t i = 0; i < matched_indices.size(); ++i) {
        // 生成随机颜色
        float color_r = dis(gen);
        float color_g = dis(gen);
        float color_b = dis(gen);

        // 获取匹配对的索引
        auto[k_idx, underk_idx] = matched_indices[i];

        // 获取匹配点
        Eigen::Vector2d point_k = centroids_unknown_motion_k[k_idx];
        Eigen::Vector2d point_underk = centroids_unknown_motion_underk[underk_idx];

        // 为 k 时刻的点添加球体
        viewer.addSphere(pcl::PointXYZ(point_k[0], point_k[1], 0), sphere_radius, color_r, color_g, color_b, "matched_k_" + std::to_string(i));

        // 为 underk 时刻的点添加球体
        viewer.addSphere(pcl::PointXYZ(point_underk[0], point_underk[1], 0), sphere_radius, color_r, color_g, color_b, "matched_underk_" + std::to_string(i));
    }

// 启动可视化
    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }