三维点拟合平面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;}}
}