点云找最近邻点C++代码

以下是使用KD树数据结构查找最近邻点C的C++代码：

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>

struct Point {
    double x, y, z;
    Point(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {};
};

struct Node {
    Point point;
    Node* left;
    Node* right;
    Node(Point p) : point(p), left(nullptr), right(nullptr) {};
};

class KDTree {
public:
    KDTree(std::vector<Point>& points) {
        root = buildTree(points, 0);
    }

    Point findNearest(Point& target) {
        nearest = root->point;
        nearestDist = dist(root->point, target);
        searchTree(root, target);
        return nearest;
    }

private:
    Node* root;
    Point nearest;
    double nearestDist;

    Node* buildTree(std::vector<Point>& points, int depth) {
        if (points.empty()) {
            return nullptr;
        }
        int axis = depth % 3;
        if (points.size() == 1) {
            return new Node(points[0]);
        }
        auto mid = points.begin() + points.size() / 2;
        std::nth_element(points.begin(), mid, points.end(), [axis](const Point& a, const Point& b) {
            if (axis == 0) {
                return a.x < b.x;
            }
            else if (axis == 1) {
                return a.y < b.y;
            }
            else {
                return a.z < b.z;
            }
        });
        auto node = new Node(*mid);
        node->left = buildTree(std::vector<Point>(points.begin(), mid), depth + 1);
        node->right = buildTree(std::vector<Point>(mid + 1, points.end()), depth + 1);
        return node;
    }

    void searchTree(Node* node, Point& target) {
        if (node == nullptr) {
            return;
        }
        double d = dist(node->point, target);
        if (d < nearestDist) {
            nearestDist = d;
            nearest = node->point;
        }
        int axis = depth(node) % 3;
        if (axis == 0) {
            if (target.x < node->point.x) {
                searchTree(node->left, target);
                if (target.x + nearestDist >= node->point.x) {
                    searchTree(node->right, target);
                }
            }
            else {
                searchTree(node->right, target);
                if (target.x - nearestDist <= node->point.x) {
                    searchTree(node->left, target);
                }
            }
        }
        else if (axis == 1) {
            if (target.y < node->point.y) {
                searchTree(node->left, target);
                if (target.y + nearestDist >= node->point.y) {
                    searchTree(node->right, target);
                }
            }
            else {
                searchTree(node->right, target);
                if (target.y - nearestDist <= node->point.y) {
                    searchTree(node->left, target);
                }
            }
        }
        else {
            if (target.z < node->point.z) {
                searchTree(node->left, target);
                if (target.z + nearestDist >= node->point.z) {
                    searchTree(node->right, target);
                }
            }
            else {
                searchTree(node->right, target);
                if (target.z - nearestDist <= node->point.z) {
                    searchTree(node->left, target);
                }
            }
        }
    }

    int depth(Node* node) {
        if (node == nullptr) {
            return -1;
        }
        return 1 + std::max(depth(node->left), depth(node->right));
    }

    double dist(Point& a, Point& b) {
        return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2) + std::pow(a.z - b.z, 2));
    }
};

int main() {
    std::vector<Point> points = {Point(1, 2, 3), Point(4, 5, 6), Point(7, 8, 9)};
    KDTree tree(points);
    Point target(3, 4, 5);
    Point nearest = tree.findNearest(target);
    std::cout << "Nearest point to target: (" << nearest.x << ", " << nearest.y << ", " << nearest.z << ")" << std::endl;
    return 0;
}