a上大分法得给

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <cmath>
#include <pcl/kdtree/kdtree_flann.h>

// 定义经纬度点结构体
struct Point {
    double lat;
    double lon;
};

// 读取 CSV 文件并将经纬度数据存储到 A 列表中
std::vector<Point> read_csv(std::string filename) {
    std::vector<Point> A;
    std::ifstream file(filename);
    std::string line;
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string lat_str, lon_str;
        std::getline(ss, lat_str, ',');
        std::getline(ss, lon_str, ',');
        double lat = std::stod(lat_str);
        double lon = std::stod(lon_str);
        A.push_back({lat, lon});
    }
    return A;
}

// 计算两个经纬度之间的距离（单位：千米）
double distance(Point p1, Point p2) {
    const double R = 6371;  // 地球半径（单位：千米）
    double lat1 = p1.lat * M_PI / 180.0;
    double lon1 = p1.lon * M_PI / 180.0;
    double lat2 = p2.lat * M_PI / 180.0;
    double lon2 = p2.lon * M_PI / 180.0;
    double dlon = lon2 - lon1;
    double dlat = lat2 - lat1;
    double a = std::pow(std::sin(dlat / 2), 2) + std::cos(lat1) * std::cos(lat2) * std::pow(std::sin(dlon / 2), 2);
    double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
    return R * c;
}

// 打印出 A 列表中在阈值距离内的所有 b 中的元素
void print_within_threshold(const std::vector<Point>& A, const std::map<int, std::vector<Point>>& b, double threshold_distance) {
    pcl::KdTreeFLANN<Point> kdtree;
    kdtree.setInputCloud(makePointCloud(b));

    for (const Point& a : A) {
        std::vector<int> indices;
        std::vector<float> distances;
        Point search_point{a.lat, a.lon};
        kdtree.radiusSearch(search_point, threshold_distance, indices, distances);

        for (int index : indices) {
            const std::vector<Point>& b_list = b.at(index);
            for (const Point& point : b_list) {
                std::cout << "Element " << index << " in b is within threshold distance of " << a.lat << "," << a.lon << std::endl;
            }
        }
    }
}

int main() {
    // 读取 CSV 文件并将经纬度数据存储到 A 列表中
    std::vector<Point> A = read_csv("file.csv");

    // 定义 b 列表，元素为 id 和 list，list 中也是经纬度
    std::map<int, std::vector<Point>> b;

    // 打印出 A 列表中在阈值距离内的所有 b 中的元素
    double threshold_distance = 10; // 阈值距离（单位：千米）
    print_within_threshold(A, b, threshold_distance);

    return 0;
}