三维点拟合直线ransac c++
理论
直线公式：
l ( t ) = p 0 + t ∗ d l(t) = p0 + t *d l(t)=p0+t∗d
其中
- p0 : 直线上的某点，通过两个点确定
- d : 方向向量（归一化）
RANSAC 拟合步骤（3D）
随机选两个不同点
𝑝1,𝑝2
计算方向向量
d = p 2 − p 1 d = p2 - p1 d=p2−p1
遍历所有点，计算它们到直线的距离：
d i = ∣ ∣ ( p i − p 1 ) x d ∣ ∣ ∣ ∣ d ∣ ∣ d_i = \frac{|| (p_i - p1) x d||}{||d||} di=∣∣d∣∣∣∣(pi−p1)xd∣∣
判断哪些点为内点（距离小于阈值）
重复若干次，保存内点最多的模型
Code
//point3ds输入 iterations迭代次数 bestliner输出最佳的点数，bestdir bestp0直线的参数
void testransac3D(std::vector<Eigen::Vector3d> point3ds,int iterator, int& bestliner, Eigen::Vector3d& bestdir, 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);if (id1 == id2)continue;Eigen::Vector3d point1 = point3ds[id1];Eigen::Vector3d point2 = point3ds[id2];//方向向量Eigen::Vector3d dir = (point2 - point1).normalized();int liner = 0;for (auto& point : point3ds) {//计算距离Eigen::Vector3d newpoint = point - point1;double dist = (newpoint.cross(dir)).norm() / dir.norm();if (dist < thdis) {liner;}}if (liner > bestliner) {bestliner = liner;bestdir = dir;bestp0 = point1;}}
}