三维点拟合平面ransac c++

理论

平面的一般定义
在三维空间中，一个平面可以由两个要素唯一确定：

法向量 n=(a,b,c)：垂直于平面的方向

平面上一点

平面上任意一点 p=(x,y,z) 满足：
$$(p-p0)\ast n=0$$ 即$$a(x-x0)+b(y-y0)+c(z-z0)+d=0$$

Step 1：从 3 点拟合平面
设 3 个点为 p1, p2, p3

计算平面上的一点和法向量：
$$\begin{gathered} v1=p2-p1 \\ v2=p3-p1 \\ n=v1xv2 \end{gathered}$$

平面点
p0 = p1
平面方程：
$$(p-p0)\ast n=0$$

Step 2：点到平面的距离
对于任意点 pi ，其到平面距离为：

$$d_i=\frac{|(p_i-p0)\ast n|}{||n||}$$

或者直接转为标准平面方程 ax+by+cz+d=0 形式：
$$d_i=\frac{|a{x}_i+b{y}_i+c{z}_i+d|}{\sqrt{a^2+b^2+c^2}}$$

Step 3：判断 inlier
设阈值 ,统计内点个数

Code

//三维点拟合平面
void testransac3DPlane(std::vector<Eigen::Vector3d> point3ds, int iterator, int& bestliner, Eigen::Vector3d& bestn, Eigen::Vector3d& bestp0) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dis(0, point3ds.size() - 1);
    bestliner = 0;
    double thdis = 10.;
    for (int i = 0; i < iterator; i++) {
        int id1 = dis(gen);
        int id2 = dis(gen);
        int id3 = dis(gen);

        if (id1 == id2 || id1 == id3 || id2 == id3)
            continue;

        Eigen::Vector3d point1 = point3ds[id1];
        Eigen::Vector3d point2 = point3ds[id2];
        Eigen::Vector3d point3 = point3ds[id3];

        //法向量
        Eigen::Vector3d v1 = point2 - point1;
        Eigen::Vector3d v2 = point3 - point1;
        Eigen::Vector3d n = v1.cross(v2).normalized();

        int liner = 0;
        for (auto& point : point3ds) {
            //计算距离
            Eigen::Vector3d newpoint = point - point1;
            double dist = abs(newpoint.dot(n)) / n.norm();

            if (dist < thdis) {
                liner;
            }
        }

        if (liner > bestliner) {
            bestliner = liner;
            bestn = n;
            bestp0 = point1;
        }
    }
}