C++ 代码实现 RANSAC 算法拟合点云数据中的多条直线

下面是一个使用 RANSAC 算法拟合点云数据中多条直线的 C++ 代码示例：

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

struct Point {
    float x;
    float y;
};

float distance(Point p1, Point p2) {
    return std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2));
}

std::vector<std::vector<Point>> ransacLineFitting(std::vector<Point> points, int numIterations, float distanceThreshold, int minPointsForLine) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<int> dist(0, points.size() - 1);

    std::vector<std::vector<Point>> lines;

    for (int i = 0; i < numIterations; i++) {
        int index1 = dist(gen);
        int index2 = dist(gen);

        while (index2 == index1) {
            index2 = dist(gen);
        }

        Point p1 = points[index1];
        Point p2 = points[index2];

        std::vector<Point> linePoints;
        linePoints.push_back(p1);
        linePoints.push_back(p2);

        for (int j = 0; j < points.size(); j++) {
            if (j != index1 && j != index2) {
                if (distance(points[j], p1) < distanceThreshold || distance(points[j], p2) < distanceThreshold) {
                    linePoints.push_back(points[j]);
                }
            }
        }

        if (linePoints.size() >= minPointsForLine) {
            lines.push_back(linePoints);
        }
    }

    return lines;
}

int main() {
    std::vector<Point> points = { {1.0, 2.0}, {2.0, 3.0}, {3.0, 4.0}, {4.0, 5.0}, {5.0, 6.0}, {6.0, 7.0} };

    std::vector<std::vector<Point>> lines = ransacLineFitting(points, 100, 0.5, 3);

    for (int i = 0; i < lines.size(); i++) {
        std::cout << "Line " << i + 1 << ":\n";
        for (int j = 0; j < lines[i].size(); j++) {
            std::cout << "(" << lines[i][j].x << ", " << lines[i][j].y << ")\n";
        }
        std::cout << std::endl;
    }

    return 0;
}

这个示例代码使用Point结构来表示二维点的坐标。distance函数用于计算两个点之间的欧氏距离。

ransacLineFitting函数接受点云数据、迭代次数、距离阈值和用于拟合直线的最小点数。在每次迭代中，它随机选择两个点作为直线的起点和终点，然后计算其他所有点到这条直线的距离。如果距离小于阈值的点的数量大于等于最小点数，则将这些点添加到一个线段中，并将该线段存储在结果中。

在主函数中，我们创建一个点云数据并调用ransacLineFitting函数来拟合直线。最后，我们打印出拟合得到的每条直线的点坐标。