C++ 三维点云数据 经过每个点最小路径 源代码

以下是基于Dijkstra算法的C++代码，用于计算三维点云数据中每个点的最小路径。

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

using namespace std;

struct Point3D {
    double x, y, z;
};

struct Edge {
    int to;
    double weight;
};

typedef vector<vector<Edge>> Graph;

const double INF = 1e9;

double distance(const Point3D& a, const Point3D& b) {
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    double dz = a.z - b.z;
    return sqrt(dx * dx + dy * dy + dz * dz);
}

void dijkstra(const Graph& graph, int start, vector<double>& dist) {
    int n = graph.size();
    dist.assign(n, INF);
    dist[start] = 0;
    priority_queue<pair<double, int>, vector<pair<double, int>>, greater<pair<double, int>>> pq;
    pq.push({ 0, start });
    while (!pq.empty()) {
        auto [d, v] = pq.top();
        pq.pop();
        if (dist[v] < d) continue;
        for (auto& e : graph[v]) {
            if (dist[e.to] > dist[v] + e.weight) {
                dist[e.to] = dist[v] + e.weight;
                pq.push({ dist[e.to], e.to });
            }
        }
    }
}

vector<vector<double>> minPath(const vector<Point3D>& points) {
    int n = points.size();
    Graph graph(n);
    for (int i = 0; i < n; i++) {
        for (int j = i + 1; j < n; j++) {
            double d = distance(points[i], points[j]);
            graph[i].push_back({ j, d });
            graph[j].push_back({ i, d });
        }
    }
    vector<vector<double>> result(n, vector<double>(n));
    for (int i = 0; i < n; i++) {
        vector<double> dist;
        dijkstra(graph, i, dist);
        for (int j = 0; j < n; j++) {
            result[i][j] = dist[j];
        }
    }
    return result;
}

int main() {
    vector<Point3D> points = { {0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3} };
    vector<vector<double>> result = minPath(points);
    for (int i = 0; i < result.size(); i++) {
        for (int j = 0; j < result[i].size(); j++) {
            cout << result[i][j] << " ";
        }
        cout << endl;
    }
    return 0;
}

在上面的代码中，我们首先定义了一个结构体 `Point3D` 来表示三维空间中的点，然后定义了一个结构体 `Edge` 来表示图中的边。接下来，我们定义了一个类型别名 `Graph` 来表示邻接表表示的图。

在 `distance` 函数中，我们计算两个点之间的欧几里得距离。

在 `dijkstra` 函数中，我们使用Dijkstra算法来计算从起点到各个点的最短路径。我们首先初始化距离数组 `dist` 为无穷大，然后将起点加入优先队列。在每次循环中，我们从优先队列中取出一个距离最小的点，并遍历该点的所有邻接点。如果从起点到该邻接点的距离小于当前记录的距离，则更新距离并将该邻接点加入优先队列。

在 `minPath` 函数中，我们首先构建三维点云数据的邻接表表示的图。然后，对于每个点，我们使用Dijkstra算法计算从该点到其他点的最短路径，并将结果存储在一个二维数组中返回。

在主函数中，我们构造了一个简单的三维点云数据，并调用 `minPath` 函数计算每个点之间的最短路径，并输出结果。

请注意，上面的代码只是一个简单的示例，实际应用中可能需要进行更多的优化和改进。