同时可视化原始中心点和经过坐标转换后的中心点

std::vector centroids_unknown_motion_underk;

std::vector measurements_centroids_unknown_motion_k
                = transformLandmarks(centroids_unknown_motion_k, weights_pose);
// 数据填充

    // k时刻经过转换到k-1时刻坐标系下的中心点位置和
    // k-1时刻坐标系下的中心点位置的----可视化
    // measurements_centroids_unknown_motion_k 和
    // centroids_unknown_motion_underk

    pcl::visualization::PCLVisualizer viewer("Centroid Visualization");
    viewer.setBackgroundColor(0.05, 0.05, 0.05); // 设置背景颜色
    viewer.addCoordinateSystem(30.0); // 添加坐标系
    viewer.initCameraParameters(); // 初始化相机参数

    double sphere_radius = 0.1; // 球体的半径
    double text_scale = 0.1; // 文本标签的大小

    // 可视化 centroids_unknown_motion_k（红色）
    for (size_t i = 0; i < centroids_unknown_motion_underk.size(); ++i) {
        Eigen::Vector2d point = centroids_unknown_motion_underk[i];
        std::string sphere_id = "centroids_k_" + std::to_string(i);
        viewer.addSphere(pcl::PointXYZ(point[0], point[1], 0), sphere_radius, 1.0, 0.0, 0.0, sphere_id);

        std::stringstream ss;
        ss << std::fixed << std::setprecision(2) << "K: (" << point[0] << ", " << point[1] << ")";
        viewer.addText3D(ss.str(), pcl::PointXYZ(point[0], point[1], 0), text_scale, 1.0, 0.0, 0.0, sphere_id + "_text");
    }

    // 可视化 measurements_centroids_unknown_motion_k（蓝色）
    for (size_t i = 0; i < measurements_centroids_unknown_motion_k.size(); ++i) {
        Eigen::Vector2d point = measurements_centroids_unknown_motion_k[i];
        std::string sphere_id = "measurements_k_" + std::to_string(i);
        viewer.addSphere(pcl::PointXYZ(point[0], point[1], 0), sphere_radius, 0.0, 0.0, 1.0, sphere_id);

        std::stringstream ss;
        ss << std::fixed << std::setprecision(2) << "M: (" << point[0] << ", " << point[1] << ")";
        viewer.addText3D(ss.str(), pcl::PointXYZ(point[0], point[1], 0), text_scale, 0.0, 0.0, 1.0, sphere_id + "_text");
    }

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }