理论
平面的一般定义
在三维空间中，一个平面可以由两个要素唯一确定：
法向量 n=(a,b,c)：垂直于平面的方向
平面上一点
平面上任意一点 p=(x,y,z) 满足：
( p − p 0 ) ∗ n = 0 (p - p0) * n = 0 (p−p0)∗n=0 即 a ( x − x 0 ) + b ( y − y 0 ) + c ( z − z 0 ) + d = 0 a(x - x0) + b(y-y0) + c(z - z0) + d = 0 a(x−x0)+b(y−y0)+c(z−z0)+d=0
Step 1：从 3 点拟合平面
设 3 个点为 p1, p2, p3
计算平面上的一点和法向量：
v 1 = p 2 − p 1 v 2 = p 3 − p 1 n = v 1 x v 2 v1 = p2 -p1\\ v2 = p3 - p1 \\ n = v1 x v2 v1=p2−p1v2=p3−p1n=v1xv2
平面点
p0 = p1
平面方程：
( p − p 0 ) ∗ n = 0 (p - p0) * n = 0 (p−p0)∗n=0
Step 2：点到平面的距离
对于任意点 pi ，其到平面距离为：
d i = ∣ ( p i − p 0 ) ∗ n ∣ ∣ ∣ n ∣ ∣ d_i = \frac{| (p_i - p0) * n | }{|| n || } di=∣∣n∣∣∣(pi−p0)∗n∣
或者直接转为标准平面方程 ax+by+cz+d=0 形式：
d i = ∣ a x i + b y i + c z i + d ∣ a 2 + b 2 + c 2 d_i = \frac{| ax_i + by_i+cz_i + d | }{\sqrt{a^2 + b^2 + c^2}} di=a2+b2+c2∣axi+byi+czi+d∣
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;}}
}
