【chatgpt代码系列】RANSAC拟合多段直线

Q1：关于RANSAC 空间多段的直线拟合，请用c++给个例子

以下是一个简单的C++示例程序，用于使用RANSAC算法在3D点云数据中拟合多段直线：

#include <iostream>
#include <vector>
#include <random>
#include <chrono>

using namespace std;

// 定义3D点的结构体
struct Point3D {
    double x, y, z;
};

// 定义直线的结构体
struct Line3D {
    Point3D start, end;
};

// RANSAC算法拟合直线函数
// 参数：
//   input_points：输入的点云数据
//   num_points：点云数据的个数
//   max_dist：最大距离阈值
//   min_points：最小拟合点数
//   num_iter：迭代次数
// 返回值：
//   拟合出的多条直线
vector<Line3D> ransac_line_fitting(Point3D* input_points, int num_points, double max_dist, int min_points, int num_iter) {
    // 随机数生成器
    default_random_engine generator(chrono::system_clock::now().time_since_epoch().count());
    uniform_int_distribution<int> distribution(0, num_points - 1);

    vector<Line3D> lines; //保存拟合出的多段直线
    vector<int> inlier_indices(num_points); //保存当前内点的索引

    for (int i = 0; i < num_iter; i++) {
        // 随机选择min_points个点作为样本点
        vector<Point3D> sample_points(min_points);
        for (int j = 0; j < min_points; j++) {
            sample_points[j] = input_points[distribution(generator)];
        }

        // 拟合直线
        Point3D start = sample_points[0];
        Point3D end = sample_points[1];
        double length = sqrt((end.x - start.x) * (end.x - start.x) + (end.y - start.y) * (end.y - start.y) + (end.z - start.z) * (end.z - start.z));

        for (int j = 0; j < num_points; j++) {
            if (find(sample_points.begin(), sample_points.end(), input_points[j]) != sample_points.end()) {
                continue;
            }
            double dist = abs((end.y - start.y) * (input_points[j].x - start.x) - (end.x - start.x) * (input_points[j].y - start.y)
                              + end.x * start.y - end.y * start.x) / length;
            if (dist <= max_dist) {
                inlier_indices[j] = 1;
            }
        }

        // 如果内点个数小于min_points，放弃当前迭代
        int inlier_count = count(inlier_indices.begin(), inlier_indices.end(), 1);
        if (inlier_count < min_points) {
            continue;
        }

        // 使用内点重新拟合直线
        Point3D new_start = { 0, 0, 0 };
        Point3D new_end = { 0, 0, 0 };
        for (int j = 0; j < num_points; j++) {
            if (inlier_indices[j] == 1) {
                new_start.x += input_points[j].x;
                new_start.y += input_points[j].y;
                new_start.z += input_points[j].z;
                new_end.x += input_points[j].x;
                new_end.y += input_points[j].y;
                new_end.z += input_points[j].z;
            }
        }
        new_start.x /= inlier_count;
        new_start.y /= inlier_count;
        new_start.z /= inlier_count;
        new_end.x /= inlier_count;
        new_end.y /= inlier_count;
        new_end.z /= inlier_count;
        lines.push_back(Line3D{new_start, new_end});

        // 将所有内点的标记重置为0
        fill(inlier_indices.begin(), inlier_indices.end(), 0);
    }

    return lines;
}

int main() {
    // 生成测试数据（一条直线分成了三段）
    Point3D points[] = {{0, 0, 0}, {1, 0, 1}, {2, 0, 2}, {3, 0, 3}, {4, 1, 3}, {5, 2, 3}, {6, 3, 3}, {7, 4, 3}, {8, 5, 3}};
    int num_points = sizeof(points) / sizeof(Point3D);

    // 进行直线拟合
    vector<Line3D> lines = ransac_line_fitting(points, num_points, 0.1, 2, 100);

    // 打印结果
    cout << "拟合出了" << lines.size() << "条直线：" << endl;
    for (size_t i = 0; i < lines.size(); i++) {
        cout << "Line " << i + 1 << ": (" << lines[i].start.x << ", " << lines[i].start.y << ", " << lines[i].start.z << ") -> ("
             << lines[i].end.x << ", " << lines[i].end.y << ", " << lines[i].end.z << ")" << endl;
    }

    return 0;
}

这个示例程序将生成一些3D点云数据并使用RANSAC算法拟合多段直线，其中点云数据分成了三段直线。该程序使用了C++11的一些新特性，如uniform_int_distribution、lambda表达式和auto类型推导。

Q2：关于RANSAC在二维平面的多段的直线拟合，请用c++给个例子

好的，以下是关于RANSAC在二维平面的多段直线拟合的C++实现示例：

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

// 定义点的结构体
struct Point {
    double x;
    double y;
};

// 定义直线的结构体
struct Line {
    double a;
    double b;
    double c;
};

// 计算两点之间的距离
double distance(Point p1, Point p2) {
    return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2));
}

// 计算直线的一般式方程
Line lineEquation(Point p1, Point p2) {
    Line l;
    l.a = p1.y - p2.y;
    l.b = p2.x - p1.x;
    l.c = p1.x * p2.y - p2.x * p1.y;
    return l;
}

// 计算点到直线距离
double pointToLineDistance(Line l, Point p) {
    return std::abs(l.a * p.x + l.b * p.y + l.c) / std::sqrt(std::pow(l.a, 2) + std::pow(l.b, 2));
}

// 随机获取n个不同的索引
std::vector<int> randomIndex(int n, int k) {
    std::vector<int> index(n);
    for (int i = 0; i < n; i++) {
        index[i] = i;
    }
    std::random_shuffle(index.begin(), index.end());
    return std::vector<int>(index.begin(), index.begin() + k);
}

// RANSAC算法
std::vector<Line> ransac(std::vector<Point> points, double threshold, int iterations, int k) {
    std::vector<Line> lines;
    int n = points.size();
    int maxInliers = 0;
    for (int i = 0; i < iterations; i++) {
        std::vector<int> index = randomIndex(n, k);
        std::vector<Point> samplePoints(k);
        for (int j = 0; j < k; j++) {
            samplePoints[j] = points[index[j]];
        }
        Line l = lineEquation(samplePoints[0], samplePoints[1]);
        std::vector<Point> inliers;
        for (int j = 0; j < n; j++) {
            if (j == index[0] || j == index[1]) {
                continue;
            }
            double d = pointToLineDistance(l, points[j]);
            if (d < threshold) {
                inliers.push_back(points[j]);
            }
        }
        if (inliers.size() > maxInliers) {
            maxInliers = inliers.size();
            Line bestLine = l;
            while (inliers.size() > k) {
                std::vector<int> index = randomIndex(inliers.size(), k);
                std::vector<Point> samplePoints(k);
                for (int j = 0; j < k; j++) {
                    samplePoints[j] = inliers[index[j]];
                }
                bestLine = lineEquation(samplePoints[0], samplePoints[1]);
                std::vector<Point> newInliers;
                for (Point p : inliers) {
                    double d = pointToLineDistance(bestLine, p);
                    if (d < threshold) {
                        newInliers.push_back(p);
                    }
                }
                inliers = newInliers;
            }
            lines.push_back(bestLine);
        }
    }
    return lines;
}

// 测试
int main() {
    std::vector<Point> points = {{1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}, {6, 7}, {7, 8}, {9, 10}, {10, 11}, {11, 12}};
    double threshold = 0.5;
    int iterations = 1000;
    int k = 2;
    std::vector<Line> lines = ransac(points, threshold, iterations, k);
    for (Line l : lines) {
        std::cout << "line: " << l.a << "x + " << l.b << "y + " << l.c << " = 0" << std::endl;
    }
    return 0;
}

上述代码演示了如何使用RANSAC算法分段拟合多条直线。其中定义了Point结构体表示点，Line结构体表示直线；distance函数计算两点之间的距离，lineEquation函数计算直线的一般式方程，pointToLineDistance函数计算点到直线的距离；randomIndex函数随机获取n个不同的索引；ransac函数实现了RANSAC算法，其中threshold表示阈值，iterations表示迭代次数，k表示选择点的个数。在main函数中，我们定义了一组测试数据进行测试。