**背景：**2D/3D激光雷達(dá)掃描的點(diǎn)云數(shù)據(jù)，擬合直線做分析，實(shí)現(xiàn)總共有三種方法：
（1）PCL點(diǎn)云庫(kù)實(shí)現(xiàn)
（2）利用標(biāo)準(zhǔn)庫(kù)手寫
（3）不使用任何庫(kù)，鏈表方式實(shí)現(xiàn)
使用手寫實(shí)現(xiàn)的主要目的是因?yàn)槌绦蚩赡軙?huì)在性能一般的單片機(jī)（不支持庫(kù)）上跑。
第一種方式可看本人激光雷達(dá)處理系列中一篇文章：https://blog.csdn.net/qq_37967853/article/details/133946558?spm=1001.2014.3001.5502
接下來主要介紹另外兩種方法：
1.標(biāo)準(zhǔn)庫(kù)手寫
struct Point {
double x;
double y;
};
// 定義擬合直線的模型
struct Line {
double slope;
double intercept;
};
// 計(jì)算兩點(diǎn)之間的距離
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; // 用于計(jì)算直線擬合的容差值
constexpr int MAX_ITERATIONS = 100; // RANSAC算法的最大迭代次數(shù)
constexpr int MIN_POINTS = 5; // 擬合直線所需的最小點(diǎn)數(shù)
constexpr int MIN_INLIERS = 20; // 擬合直線所需的最小內(nèi)點(diǎn)數(shù)
// 生成隨機(jī)數(shù)
double generateRandomNumber(double min, double max) {
return min + static_cast<double>(std::rand()) / (RAND_MAX / (max - min));
}
// RANSAC擬合直線
Line ransac(const std::vector<Point>& points) {
std::srand(std::time(nullptr)); // 初始化隨機(jī)種子
Line bestLine;
int bestInliers = 0;
for (int i = 0; i < MAX_ITERATIONS; ++i) {
// 隨機(jī)選擇兩個(gè)點(diǎn)
int index1 = std::rand() % points.size();
int index2 = std::rand() % points.size();
while (index2 == index1) {
index2 = std::rand() % points.size();
}
const Point& p1 = points[index1];
const Point& p2 = points[index2];
// 計(jì)算直線參數(shù)
double slope = (p2.y - p1.y) / (p2.x - p1.x);
double intercept = p1.y - slope * p1.x;
// 統(tǒng)計(jì)內(nèi)點(diǎn)數(shù)
int inliers = 0;
for (const Point& p : points) {
double d = std::abs(p.y - slope * p.x - intercept);
if (d < TOLERANCE) {
++inliers;
}
}
// 更新最優(yōu)直線
if (inliers > bestInliers && inliers >= MIN_INLIERS) {
bestInliers = inliers;
bestLine.slope = slope;
bestLine.intercept = intercept;
}
}
return bestLine;
}
int main() {
std::srand(std::time(nullptr)); // 初始化隨機(jī)種子
// 生成隨機(jī)點(diǎn)
std::vector<Point> points;
for (int i = 0; i < 100; ++i) {
double x = generateRandomNumber(-10.0, 10.0);
double y = 2.0 * x + generateRandomNumber(-1.0, 1.0); // 兩條直線的斜率分別為2和-2
points.push_back({ x, y });
}
std::vector<Point> points1;
for (int i = 0; i < 100; ++i) {
double x = generateRandomNumber(-10.0, 10.0);
double y = -2.0 * x + generateRandomNumber(-1.0, 1.0); // 兩條直線的斜率分別為2和-2
points1.push_back({ x, y });
}
points.insert(points.end(), points1.begin(), points1.end());
// RANSAC擬合直線
std::vector<Line> lines;
while (points.size() >= MIN_POINTS) {
Line line = ransac(points);
lines.push_back(line);
// 從數(shù)據(jù)中移除擬合的內(nèi)點(diǎn)
std::vector<Point> 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;
}
// 輸出擬合結(jié)果
for (int i = 0; i < lines.size(); i++) {
std::cout << "擬合直線 " << i + 1 << " 方程：y = " << lines[i].slope << "x + " << lines[i].intercept << std::endl;
}
return 0;
}
結(jié)果：
2.不使用任何庫(kù)，利用鏈表數(shù)據(jù)結(jié)構(gòu)
std只是用來隨機(jī)數(shù)生成點(diǎn)云數(shù)據(jù)的，算法部分涉及的隨機(jī)生成算法是自己實(shí)現(xiàn)的。詳細(xì)實(shí)現(xiàn)看代碼注釋。
using namespace std;
//點(diǎn)數(shù)據(jù)
struct Node {
double x;
double y;
Node* next;
};
//直線參數(shù)
struct LineNode {
double slope;
double intercept;
LineNode* next;
};
//隨機(jī)數(shù)生成算法
unsigned int xorshift32()
{
static unsigned int state = 2463534242; // 初始種子
unsigned int x = state;
x ^= x << 13;
x ^= x >> 17;
x ^= x << 5;
state = x;
return x;
}
//計(jì)算兩點(diǎn)x之間最大值（橫向最遠(yuǎn)）
//尋找鏈表中x的極值
double findMaxDist(Node* head) {
double xmax=0;
double xmin=0;
Node* current = head;
while (current != nullptr) {
if ( current->x < xmin) {
xmin = current->x;
}
if (current->x > xmax) {
xmax = current->x;
}
current = current->next;
}
return xmax - xmin;
}
//釋放鏈表內(nèi)存，在鏈表使用結(jié)束
template<typename T>
void deleteNodeList(T* head) {
while (head != nullptr) {
T* temp = head;
head = head->next;
delete temp;
}
}
template<typename T>
void deleteArryNodeList(T* lines[]) {
// 釋放數(shù)組鏈表中的內(nèi)存
int size = sizeof(lines) / sizeof(lines[0]);
for (int i = 0; i < size; i++) {
if (lines[i] != nullptr) {
deleteNodeList(lines[i]);
}
}
}
//向鏈表末尾添加數(shù)據(jù)
void addNode(Node*& head, double x, double y) {
if (head == nullptr) {
Node* node = new Node;
node->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;
}
}
//向鏈表末尾添加數(shù)據(jù)
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賦值替換鏈表2
void replaceList(Node* list1, Node*& list2) {
// 釋放鏈表 list2 的內(nèi)存
while (list2 != nullptr) {
Node* temp = list2;
list2 = list2->next;
delete temp;
}
// 復(fù)制鏈表 list1 的節(jié)點(diǎn)到 list2
Node* current = list1;
Node* head_1 = nullptr;//head_1為head_1鏈表的頭節(jié)點(diǎn)地址
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) {//循環(huán)遍歷head_1鏈表（中已存放的newNode）
temp = temp->next;//找到末尾的空地址
}
temp->next = newNode;//新newNode數(shù)據(jù)添加到已存放的newNode末尾
}
current = current->next;
}
list2 = head_1;//將head_1鏈表首地址賦值給list2首地址，指針的值復(fù)制
}
//計(jì)算鏈表size
template<typename T>
int getListSize(T*& head) {
int size = 0;
T* current = head;
while (current != nullptr) {
size++;
current = current->next;
}
return size;
}
// 從鏈表中隨機(jī)選擇節(jié)點(diǎn)
Node* getRandomNode(Node* head) {
int length = getListSize(head);
// 生成隨機(jī)數(shù)種子
std::srand(static_cast<unsigned int>(std::time(nullptr)));
// 生成隨機(jī)索引
int randomIndex = std::rand() % length;
Node* current = head;
for (int i = 0; i < randomIndex; i++) {
current = current->next;
}
return current;
}
template<typename T>
void printList(T* head) {
T* temp = head;
while (temp != nullptr) {
std::cout << "(" << temp->x << ", " << temp->y << ") ";
temp = temp->next;
}
std::cout << std::endl;
}
// 獲取鏈表中特定索引位置的指針
Node* getValueAtIndex(Node* head, int index) {
Node* temp = head;
int currentIndex = 0;
while (temp != nullptr) {
if (currentIndex == index) {
return temp; // 返回節(jié)點(diǎn)的值（或你想要獲取的值）
}
temp = temp->next;
currentIndex++;
}
return nullptr;
// 如果索引超出鏈表長(zhǎng)度，可以根據(jù)具體情況進(jìn)行錯(cuò)誤處理
// 在此示例中，我們返回一個(gè)默認(rèn)值 -1.0
/*return -1.0;*/
}
constexpr double TOLERANCE = 2.0; // 用于計(jì)算直線擬合的容差值
constexpr int MAX_ITERATIONS = 100; // RANSAC算法的最大迭代次數(shù)
constexpr int MIN_POINTS = 15; // 擬合直線所需的最小點(diǎn)數(shù)
constexpr int MIN_INLIERS = 20; // 擬合直線所需的最小內(nèi)點(diǎn)數(shù)
// 生成隨機(jī)數(shù)
double generateRandomNumber(double min, double max) {
return min + static_cast<double>(std::rand()) / (RAND_MAX / (max - min));
}
// RANSAC擬合直線
LineNode* ransac(Node*& points) {
std::srand(std::time(nullptr)); // 初始化隨機(jī)種子
//Line bestLine;
LineNode* bestLine = nullptr;
int bestInliers = 0;
int size = getListSize(points);
for (int i = 0; i < MAX_ITERATIONS; ++i) {
//隨機(jī)從鏈表中選擇兩個(gè)不相同的點(diǎn)，生成隨機(jī)數(shù)種子
//生成隨機(jī)索引
unsigned int randomIndex1 = xorshift32() % size;
Node* randomNode1 = getValueAtIndex(points, randomIndex1);
unsigned int randomIndex2 = xorshift32() % size;
while (randomIndex1 == randomIndex2) {
randomIndex2 = xorshift32() % size;
}
Node* randomNode2 = getValueAtIndex(points, randomIndex2);
// 統(tǒng)計(jì)內(nèi)點(diǎn)數(shù)
int inliers = 0;
double slope = 0;
double intercept = 0;
// 計(jì)算直線參數(shù)
if (randomNode1 != nullptr && randomNode2 != nullptr) {
slope = (randomNode2->y - randomNode1->y) / (randomNode2->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 < TOLERANCE) {
++inliers;
}
current = current->next;
}
// 內(nèi)點(diǎn)數(shù)最多的直線，更新最優(yōu)直線
if (inliers > bestInliers && inliers >= MIN_INLIERS) {
bestInliers = inliers;
addLineNode(bestLine, slope, intercept);
}
}
}
cout << "bestInliers:" << bestInliers << endl;
return bestLine;
}
int main() {
std::srand(std::time(nullptr)); // 初始化隨機(jī)種子
// 生成隨機(jī)點(diǎn)
Node* points = nullptr;
for (int i = 0; i < 100; ++i) {
double x = generateRandomNumber(-10.0, 10.0);
double y = 2.0 * x + generateRandomNumber(-1.0, 1.0); // 兩條直線的斜率分別為2和-2
addNode(points, x, y);
}
Node* points1 = nullptr;
for (int i = 0; i < 100; ++i) {
double x = generateRandomNumber(-10.0, 10.0);
double y = -2.0 * x + generateRandomNumber(-1.0, 1.0); // 兩條直線的斜率分別為2和-2
addNode(points1, x, y);
}
appendList(points, points1);
printList(points);
//線段最長(zhǎng)的
double max_line_length = 0;
//循環(huán)計(jì)數(shù)
int index = 0;
//符合墻面線要求的索引，方便求出符合要求的直線
int model_index=0;
//RANSAC擬合直線,假設(shè)提前知道在一百條以內(nèi)
LineNode* lines[100];
int points_Size = 0;
points_Size=getListSize(points);
while (points_Size >= MIN_POINTS) {//循環(huán)條件
cout << "points變化size"<<index<<":" << points_Size << endl;
LineNode* line = ransac(points);
//如果擬合不出直線則退出
if (line==nullptr) {
break;
}
cout << "best" << line->slope << " " << line->intercept << endl;
lines[index]=line;
//外點(diǎn)，從數(shù)據(jù)中移除擬合的內(nèi)點(diǎn)
Node* remainingPoints = nullptr;
//內(nèi)點(diǎn)，提取內(nèi)點(diǎn)
Node* interiorPoint = nullptr;
for (Node* current = points; current!=nullptr ; current=current->next)
{
double d = std::abs(current->y - line->slope * current->x - line->intercept);
if (d >= TOLERANCE) {
addNode(remainingPoints, current->x, current->y);
}
else {
addNode(interiorPoint, current->x, current->y);
}
}
int interiorPoint_Size = getListSize(interiorPoint);
cout << "內(nèi)點(diǎn)數(shù)目" << interiorPoint_Size << endl;
int remainingPoints_Size = getListSize(remainingPoints);
cout << "外點(diǎn)數(shù)目" << remainingPoints_Size<< endl;
//尋找內(nèi)點(diǎn)中最大和最小的點(diǎn)
double maxdist=findMaxDist(interiorPoint);
cout << "橫向最大距離" << maxdist << endl;
//內(nèi)點(diǎn)求滿足一定范圍斜率且橫向長(zhǎng)度最長(zhǎng)的
//if (-tan(45)<line->slope<tan(45) && maxdist> max_line_length) {
// max_line_length = maxdist;
// model_index = index;
//}
//計(jì)算符合條件的直線方程
if (0 < abs(line->slope) && maxdist> max_line_length) {
max_line_length = maxdist;
model_index = index;
}
//若沒有外點(diǎn)則退出循環(huán)
if (remainingPoints==nullptr) {
break;
}
replaceList(remainingPoints,points);
cout << "------------" << endl;
points_Size = getListSize(points);
index++;
}
std::cout << "目標(biāo)直線方程：y = " << lines[model_index]->slope << "x + " << lines[model_index]->intercept << std::endl;
//清除鏈表內(nèi)存，即使沒有通過new
deleteNodeList(points);
deleteNodeList(points1);
deleteArryNodeList(lines);
上述代碼對(duì)于ransac每次擬合出來的直線參數(shù)鏈表，我們存儲(chǔ)在假設(shè)內(nèi)存為100大小的數(shù)組中，實(shí)際上不嚴(yán)謹(jǐn)，因?yàn)槭孪炔⒉恢罆?huì)擬合出多少條直線，因此此處可以再定義一個(gè)結(jié)構(gòu)體鏈表來存儲(chǔ)多條直線參數(shù)鏈表（大鏈表中存儲(chǔ)一系列小鏈表）。
//存放直線參數(shù)鏈表的大鏈表
struct LineNodes
{
NodeList::LineNode* L;
LineNodes* next;
};
//向大鏈表中添加一系列小鏈表
template<typename T>
void addLineNodes(LineNodes*& head,T* L) {
if (head == nullptr) {
LineNodes* node = new LineNodes;
node->L = L;
node->next = nullptr;
head = node;
}
else {
LineNodes* temp1 = head;
while (temp1->next != nullptr) {
temp1 = temp1->next;
}
temp1->next = new LineNodes;
temp1->next->L = L;
temp1->next->next = nullptr;
}
}
main中：
LineNodes* linenodes=nullptr;//替換原來的LineNode* lines[100];
nodelist.addLineNodes(linenodes, line);//替換原來的lines[index]=line;
//打印
LineNodes* linenode = nodelist.getValueAtIndex(linenodes, model_index);
std::cout << "目標(biāo)直線方程：y = " << linenode->L->slope<< "x + " << linenode->L->intercept << std::endl;
//釋放
nodelist.deleteNodeList(linenodes);
結(jié)果：
**注意：**對(duì)于第二種方法，實(shí)現(xiàn)過程中一定要注意指針為空的情況，指針為空時(shí)會(huì)報(bào)錯(cuò)：讀取訪問權(quán)限錯(cuò)誤。另外擬合直線時(shí)，一定要對(duì)數(shù)據(jù)有初步認(rèn)識(shí)，最好先去除離群點(diǎn)再來擬合，容差設(shè)置尤為關(guān)鍵，太小可能會(huì)擬合不出直線，太大會(huì)導(dǎo)致得到的直線不準(zhǔn)確。文章來源：http://www.zghlxwxcb.cn/news/detail-838966.html
以上代碼均為本人手敲加調(diào)試實(shí)現(xiàn)，完成不易，轉(zhuǎn)載或引用請(qǐng)注明出處。文章來源地址http://www.zghlxwxcb.cn/news/detail-838966.html
到了這里，關(guān)于利用C++實(shí)現(xiàn)RANSAC擬合多條直線并提出符合要求的直線，標(biāo)準(zhǔn)庫(kù)和手寫（不使用任何庫(kù)、鏈表方式）兩種方法的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
