行业知识
利用C++实现RANSAC拟合多条直线并提出符合要求的直线,标准库和手写(不使用任何库、链表方式)两种方法
2026-11-17 23:57:17
来源:互联网转载
**背���:**2D/3D激光雷达扫描的点云数据,拟合直线做分析,实现总共有三种方法:
(1)PCL点云库实现
(2)利用标准库手写
(3)不使用任何库,链表方式实现
使用手写实现的主要目的是因为程序可能会在性能一般的单片机(不支持库)上跑。
第一种方式可看本人激光雷达处理系列中一篇文章:https://blog.csdn.net/qq_37967853/article/details/133946558?spm=1001.2014.3001.5502
接下来主要介绍另外两种方法:
1.标准库手写
struct Point { double x; double y;};// 定义拟合直线的模型struct Line { double slope; double intercept;};// 计算两点之间的距离double distance(Point p1, Point p2) { double dx = p2.x - p1.x; double dy = p2.y - p1.y; return sqrt(dx * dx + dy * dy);} constexpr double TOLERANCE = 1.0; // 用于计算直线拟合的容差值constexpr int MAX_ITERATIONS = 100; // RANSAC算法的最大迭代次数constexpr int MIN_POINTS = 5; // 拟合直线所需的最小点数constexpr int MIN_INLIERS = 20; // 拟合直线所需的最小内点数// 生成随机数double generateRandomNumber(double min, double max) { return min + static_cast(std::rand()) / (RAND_MAX / (max - min));}// RANSAC拟合直线Line ransac(const std::vector& points) { std::srand(std::time(nullptr)); // 初始化随机种子 Line bestLine; int bestInliers = 0; for (int i = 0; i bestInliers && inliers >= MIN_INLIERS) { bestInliers = inliers; bestLine.slope = slope; bestLine.intercept = intercept; } } return bestLine;} int main() { std::srand(std::time(nullptr)); // 初始化随机种子 // 生成随机点 std::vector points; for (int i = 0; i = MIN_POINTS) { Line line = ransac(points); lines.push_back(line); // 从数据中移除拟合的内点 std::vector remainingPoints; for (const Point& p : points) { double d = std::abs(p.y - line.slope * p.x - line.intercept); if (d >= TOLERANCE) { remainingPoints.push_back(p); } } points = remainingPoints; } // 输出拟合结果 for (int i = 0; i x = x; node->y = y; node->next = nullptr; head = node; } else { Node* temp1 = head; while (temp1->next != nullptr) { temp1 = temp1->next; } temp1->next = new Node; temp1->next->x = x; temp1->next->y = y; temp1->next->next = nullptr; }}//向链表末尾添加数据void addLineNode(LineNode*& head, double slope, double intercept) { if (head == nullptr) { LineNode* node = new LineNode; node->slope = slope; node->intercept = intercept; node->next = nullptr; head = node; } else { LineNode* temp1 = head; while (temp1->next != nullptr) { temp1 = temp1->next; } temp1->next = new LineNode; temp1->next->slope = slope; temp1->next->intercept = intercept; temp1->next->next = nullptr; }}//合并链表//将链表1加在链表2后面,顺序不变void appendList(Node*& list1, Node* list2) { if (list1 == nullptr) { list1 = list2; } else { Node* temp = list1; while (temp->next != nullptr) { temp = temp->next; } temp->next = list2; }}//链表1赋值替换链表2void replaceList(Node* list1, Node*& list2) { // 释放链表 list2 的内存 while (list2 != nullptr) { Node* temp = list2; list2 = list2->next; delete temp; } // 复制链表 list1 的节点到 list2 Node* current = list1; Node* head_1 = nullptr;//head_1为head_1链表的头节点地址 while (current != nullptr) { Node* newNode = new Node; newNode->x = current->x; newNode->y = current->y; newNode->next = nullptr; if (head_1 == nullptr) { head_1 = newNode; } else { Node* temp = head_1; while (temp->next != nullptr) {//循环遍历head_1链表(中已存放的newNode) temp = temp->next;//找到末尾的空地址 } temp->next = newNode;//新newNode数据添加到已存放的newNode末尾 } current = current->next; } list2 = head_1;//将head_1链表首地址赋值给list2首地址,指针的值复制}//计算链表sizetemplateint getListSize(T*& head) { int size = 0; T* current = head; while (current != nullptr) { size++; current = current->next; } return size;}// 从链表中随机选择节点Node* getRandomNode(Node* head) { int length = getListSize(head); // 生成随机数种子 std::srand(static_cast(std::time(nullptr))); // 生成随机索引 int randomIndex = std::rand() % length; Node* current = head; for (int i = 0; i next; } return current;}templatevoid printList(T* head) { T* temp = head; while (temp != nullptr) { std::cout Node* temp = head; int currentIndex = 0; while (temp != nullptr) { if (currentIndex == index) { return temp; // 返回节点的值(或你想要获取的值) } temp = temp-next; currentIndex++; } return nullptr; // 如果索引超出链表长度,可以根据具体情况进行错误处理 // 在此示例中,我们返回一个默认值 -1.0 /*return -1.0;*/}constexpr double TOLERANCE = 2.0; // 用于计算直线拟合的容差值constexpr int MAX_ITERATIONS = 100; // RANSAC算法的最大迭代次数constexpr int MIN_POINTS = 15; // 拟合直线所需的最小点数constexpr int MIN_INLIERS = 20; // 拟合直线所需的最小内点数// 生成随机数double generateRandomNumber(double min, double max) { return min + static_cast std::srand(std::time(nullptr)); // 初始化随机种子 //Line bestLine; LineNode* bestLine = nullptr; int bestInliers = 0; int size = getListSize(points); for (int i = 0; i x - randomNode1->x); intercept = randomNode1->y - slope * randomNode1->x; Node* current = points; while (current != nullptr) { double d = std::abs(current->y - slope * current->x - intercept); if (d next; } // 内点数最多的直线,更新最优直线 if (inliers > bestInliers && inliers >= MIN_INLIERS) { bestInliers = inliers; addLineNode(bestLine, slope, intercept); } } } cout std::srand(std::time(nullptr)); // 初始化随机种子 // 生成随机点 Node* points = nullptr; for (int i = 0; i
上一篇:域名怎么填写英文名