理論
平面的一般定義
在三維空間中，一個平面可以由兩個要素唯一確定：
法向量 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;}}
}
)
)
)
)
)
