本文主要是介绍通俗易懂理解RANSAC算法,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
RANSAC(Random Sample Consensus),翻译为随机抽样一致算法。
算法思路:从所有观测中随机找到几个尽可能少的点去拟合模型,拟合后依次计算模型和所有观测数据的残差,当残差小于给定的阈值时,就将其判断为内点,大于给定的阈值时,就判断为外点,并统计内点的数量,然后再次随机选取几个点拟合模型迭代。如果本次拟合内点数量大于先前的模型,就将旧模型迭代为新的模型。
做过AMCL算法的同学是不是秒懂了,感觉就是一个低配版的粒子滤波。
直接上代码,以我正在做的项目中的一环进行代码分享(三维平面的拟合)。
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <limits>
#include <random>
#include <vector>#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
using namespace std;
using namespace cv;
using namespace Eigen;struct Point3D {double x, y, z;
};double distance(const Point3D& p1, const Point3D& p2) {return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2) +std::pow(p1.z - p2.z, 2));
}Point3D ransacPlaneFitting(const std::vector<Point3D>& points, int max_iterations,double distance_threshold) {int n = points.size();// 生成随机数std::random_device rd; // 用于随机数引擎获得随机种子std::mt19937 gen(rd()); // 以rd()为种子的标准mersenne_twister_enginestd::uniform_int_distribution<> dis(0, n - 1); // 限制随机数范围Point3D best_plane{0, 0, 0};int best_inliers = 0;// 一直迭代for (int i = 0; i < max_iterations; ++i) {int idx1 = dis(gen);int idx2 = dis(gen);int idx3 = dis(gen);Point3D p1 = points[idx1];Point3D p2 = points[idx2];Point3D p3 = points[idx3];double a = (p2.y - p1.y) * (p3.z - p1.z) - (p2.z - p1.z) * (p3.y - p1.y);double b = (p2.z - p1.z) * (p3.x - p1.x) - (p2.x - p1.x) * (p3.z - p1.z);double c = (p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x);double d = -a * p1.x - b * p1.y - c * p1.z;int inliers = 0;double m_dist = std::sqrt(a * a + b * b + c * c);for (const auto& point : points) {double dist = std::abs(a * point.x + b * point.y + c * point.z + d) / m_dist;if (dist < distance_threshold) {++inliers;}}if (inliers > best_inliers) {best_inliers = inliers;best_plane = {a, b, c};}}return best_plane;
}int main() {int png_size = 4;// TODO: 需要输入一个三维点集std::vector<Point3D> points;int max_iterations = 1000;double distance_threshold = 0.05;Point3D plane = ransacPlaneFitting(points, max_iterations, distance_threshold);// 输出平面的 ABC值std::cout << "Best plane: (" << plane.x << ", " << plane.y << ", " << plane.z << ")"return 0;
}
这篇关于通俗易懂理解RANSAC算法的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!