激光数据的直线拟合

article/2025/11/8 2:16:03

激光雷达获取的信息是和周围物体之间的距离信息，在移动机器人尤其是自主移动机器人领域具有非常广泛的应用，那我们就从移动机器人的自主导航开始聊吧。

移动机器人导航是指移动机器人依靠传感器在特定环境中，按时间最优、路径最短或能耗最低等准则实现从起始位置到目标位置的无碰撞运动。

传统的移动机器人导航问题包含三大要素：地图创建、定位和运动控制，通过三大要素，解决三个基本问题：我在哪里？我要去哪里？如何去？

机器人定位的目的是回答“我在什么地方？”这个基本的问题。

激光雷达采集数据之后通过一定的运算可以准确的计算出当前的绝对位置或者相对位置，那么就可以知道机器人在什么位置，当然有时地图的创建和定位是同步进行的，例如SLAM算法（Simultaneous localization and mapping），此时可以看成是一个探索问题。   

说了这么多，无非是想说激光雷达是多么的有用，多么的强大，而完成这些功能首先还是应该获取激光雷达数据的特征，这就回到了特征提取这一主题上。

特征提取主要分为两个步骤：区域分割和特征提取。区域分割阶段主要完成特征模式的分类及识别确定，即确定特征属于哪类模式，如直线，圆弧等，并确定属于该特征模式的区域及区域内的激光数据点集。特征提取阶段主要完成各类特征模式参数的确定以及特征点的提取。


一、区域分割

对于每一帧距离数据，首先把激光扫描点分割成不同的区块。如果连续两个扫描点的距离小于一个阈值，这两个扫描点属于同一个区块。如果连续两个扫描点的距离大于一个阈值，数据帧就从这个地方分割开。最后把一帧距离数据分割成几个区块。分割的区块表示为 Ri(i=1,…,Q，其中 Q 是分割的区块数)，每一个区块包含 Ni个点。由于扫描点的分布并不是均匀的，通常情况下，离传感器近的扫描点密度大一些，而远离传感器的扫描点密度小一些。所以进行距离数据分割时，应用自适应变阈值分割方法。例如当某个扫描点离传感器中心的距离为 D 时，分割阈值选择为 d，当扫描点离传感器中心的距离为3D 时，阈值选择为 3d。除此之外，也可以选用其它的线性或非线性函数来定义自适应分割阈值。总之，在不同的扫描点选用不同的分割阈值，以求距离数据的分割区块能够更好地与实际环境特征模型一致。如果激光的有效测距距离为 10 米，并且角度分辨率为 0.25度，则相邻扫描点之间的距离最小为：2×10m×sin(0.125°)=0.0436m。根据该值可以设定合适的分隔阈值。

UTM-30LX 有效距离60m 角度分辨率0.25度，，但是一般对于室内应用一般不会超过10m

a、计算相邻两个点之间的距离Dj
b、判断Dj和阈值delta的关系

如果Dj,大于闽值delta,则认为点(x,y)是两个区域的分割点，阈值的选择一般按照动态阈值的方式

c、（可选）判断每个区域内的数据点的个数,如果某个区域包含数据点的个数小于等于三个,那么该区域被视为噪声区域,舍弃这些噪声点

//激光雷达区域分割效果，不同的区域用不同的颜色分割（同一种颜色并不代表在同一区域，只是颜色有限，几种颜色在循环使用）


二、特征提取

激光雷达扫描的数据中，几个重要的特征：撕裂点（breakPoint）、角点（Corner）、直线、圆弧等。

区域分割实际上就已经找到了数据中的撕裂点。折线也可以当成是一个特征，是直线加角点构成的特征。



直线作为一个很关键的特征在很多的论文中都是提取的关键，鉴于折线的是普遍存在的，那么角点的检测同样是一个难以回避的问题。那么我们就先提取角点，将所有的折线都打断成直线和角点。

1、角点检测

假设有一条折线，只有单个角点，那么我们可以采用多变形拟合方式确定角点的位置。首先将区域内的点拟合成一条直线，然后找出离直线最远的点，这个距离如果大于某个阈值，则可以认为是折线，而该点就是折线的分割点，否者就是一条直线。

当某区域含有多个角点时，就需要采用迭代或者递归的方式，不断的寻找角点-->拆分成两段，循环进行，直到每个区域都不存在角点。

//多边形拟合的方式确定是否存在角点，以及角点的位置

// 进行多边形拟合： Points : 轮廓上的点      n -- 轮廓点数目  Eps -- 拟合精度
// 返回值： 若该轮廓段需要分段，则返回分段点在该轮廓点列中的索引，否则，返回 0 表示不需要分段
// 这里是整个算法计算复杂性最大的一个地方
// 为了提高程序运行效率，对点到直线的距离计算进行改进：
// 多边形拟合中的直线是由点列中的点决定的
// 为了计算点到直线的距离，
// 采用坐标系旋转，将直线旋转到x轴方向，这样点到直线的距离即为各个点
// 在坐标旋转后的y值的绝对值
// 同时，坐标旋转矩阵在该次运算中为定值，只需一次计算，不需要多次的开方或三角计算
int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根据轮廓点，用多边形拟合该轮廓点    
{double dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1])) +  ((Y[0] - Y[n - 1])* (Y[0] - Y[n - 1]))));double cosTheta = (X[n- 1] - X[0]) / dis;double sinTheta = - ( Y[n- 1] - Y[0] )/dis;double MaxDis = 0;int i ;int MaxDisInd = -1;double dbDis;for(i = 1 ; i < n - 1 ; i++){// 进行坐标旋转，求旋转后的点到x轴的距离dbDis = abs( (Y[i] - Y[0]) * cosTheta + (X[i] - X[0])* sinTheta);if( dbDis > MaxDis){MaxDis = dbDis;MaxDisInd = i;}}if(MaxDis > Eps){return MaxDisInd;//        cout << "Line 1 : " << endl;//        cout << "Start :" << Points[0].x << "  " << Points[0].y  << " --- " << Points[MaxDisInd].x << "  " << Points[MaxDisInd].y << endl;//        cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;//        cout << "Line 2 :" << endl;//        cout << "Start :" << Points[MaxDisInd].x << "  " << Points[MaxDisInd].y  << " --- " << Points[n - 1].x << "  " << Points[n - 1].y << endl;//        cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x ) / 3.1415926;}//    else{//        cout << "Line 1 : " << endl;//        cout << "Start :" << Points[0].x << "  " << Points[0].y  << " --- " << Points[n - 1].x << "  " << Points[n - 1].y << endl;//        cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;//    }return 0;
}

以上只能检测具有单个角点的折线，任意个角点的折线采用了递归的方式，想提速的可以自己转化为迭代的方式实现。 

//将折线拆多段
int OpenRadar::BreakPolyLine(vector<int>& BreakedRadarRho,vector<double>& BreakedRadarTheta,vector<int>& SepRadarRho ,   vector<double>&SepRadarTheta)
{int rho = 0;double theta = 0.0;int X[1200] = {0};int Y[1200] = {0};int rhoCopy[1200] = {0};double thetaCopy[1200] = {0};int pointCnt = 0;int lineCnt = 0;int N = 0;SepRadarRho.clear();SepRadarTheta.clear();Corners.clear();//进行多次迭代，将所有的折线都拆分成直线段vector<int>CornerIndex;int CornerCnt = 0;int tempIndex = 0;for (int i = 0; i < static_cast<int>(BreakedRadarRho.size());i++){rho   = BreakedRadarRho.at(i);theta = BreakedRadarTheta.at(i);if (rho < 0){if (pointCnt > 200)//数目比较少的点直接抛弃{CornerIndex.clear();CornerCnt = FindCorners(CornerIndex,X,Y,0,pointCnt,200);if (CornerIndex.size() == 0){for (int k = 0 ; k < pointCnt;k++){SepRadarRho.push_back(rhoCopy[k]);SepRadarTheta.push_back(thetaCopy[k]);}SepRadarRho.push_back(-1);SepRadarTheta.push_back(1000.0);lineCnt++;}else{tempIndex = 0;for (int k = 0 ; k < pointCnt;k++){SepRadarRho.push_back(rhoCopy[k]);SepRadarTheta.push_back(thetaCopy[k]);if (k == CornerIndex.at(tempIndex)){SepRadarRho.push_back(-1);SepRadarTheta.push_back(1000.0);lineCnt++;if (tempIndex < static_cast<int>(CornerIndex.size()) -1){tempIndex++;}  }}SepRadarRho.push_back(-1);SepRadarTheta.push_back(1000.0);lineCnt++;}}pointCnt = 0;continue;}X[pointCnt] = static_cast<int>(rho*cos(theta));Y[pointCnt] = static_cast<int>(rho*sin(theta));rhoCopy[pointCnt]   = rho;thetaCopy[pointCnt] = theta;pointCnt++;}//cout<<"lineCnt: "<<lineCnt<<endl;return lineCnt;
}

2、直线拟合

如果某区域不存在角点，并且点数据比较大，那么一般都是直线，（不是绝对，直线的判定方法后面的博客再写）

直线拟合的原理比较简单，实际就是一个最小二乘法，或者为了提高拟合的精度可以采用加权的最下二乘法，这里采用的是加权最小二乘。

//原始数据图 蓝点是雷达的位置

//拟合直线和角点图，粗点是角点，粗实线是拟合出的直线



由于只对点数比价多的直线进行了拟合，上部分的直线并为画出，实际上点数比较少的直线误差也会大，并不是关键的特征。

源码下载：http://download.csdn.net/detail/u012700322/9748910

原文地址： 点击打开链接

http://chatgpt.dhexx.cn/article/2g1bpCqA.shtml

相关文章

直线拟合2

直线拟合2

Opencv中的fitLine函数&#xff0c;实现了《直线拟合》一文中的总体最小二乘法拟合直线。 C: void fitLine(InputArray points, OutputArray line, int distType, double param, double reps, double aeps) 参数说明&#xff1a; points&#xff1a;需要拟合的点&#xff1b; L…
阅读更多...
C++ 最小二乘法 直线拟合、曲线拟合、平面拟合、高斯拟合

C++ 最小二乘法 直线拟合、曲线拟合、平面拟合、高斯拟合

本节介绍如何用Eigen求解线性最小二乘方程组。求解Axb的最小二乘问题&#xff0c;等价于求解方程 使用Eigen的求解的代码如下&#xff1a; Eigen::MatrixXd MatX;//样本数据Eigen::MatrixXd MatY;//观测值Eigen::MatrixXd MatLS;//待定系数MatLS (MatX.transpose() * MatX).in…
阅读更多...
C语言直线拟合函数

C语言直线拟合函数

直线拟合算法 函数说明&#xff1a; 通过一些点拟合出一条直线。 参数&#xff1a; pt_input 指向传入的点的指针。 ptNumbers 传入的点数量。 k 指向拟合直线参数k的指针。 b 指向拟合直线参数b的指针。 返回值&#xff1a; 返回一个代数值判断拟合是否成功。如果成功…
阅读更多...
用RANSAC算法实现干扰严重的直线拟合~

用RANSAC算法实现干扰严重的直线拟合~

1.说到直线拟合&#xff0c;一般是用最小二乘啦&#xff0c;在opencv里面就是用cv.fitLine来完成&#xff0c;首先简单介绍一下该函数&#xff1a; cv.fitLine(points, distType, param, reps, aeps[, line]) -> line points&#xff1a;点集坐标 distType&#xff1a;距…
阅读更多...
RANSAC直线拟合和最小二乘直线拟合的简单介绍

RANSAC直线拟合和最小二乘直线拟合的简单介绍

RANSAC是“RANdom SAmple Consensus&#xff08;随机抽样一致&#xff09;”的缩写。它可以从一组包含“局外点”的观测数据集中&#xff0c;通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果&#xff1b;为了提高概率必须提高迭代次…
阅读更多...
RANSAC三维直线拟合

RANSAC三维直线拟合

RANSAC算法 &#xff08;1&#xff09;要得到一个直线模型&#xff0c;需要两个点唯一确定一个直线方程。所以第一步随机选择两个点&#xff1b; &#xff08;2&#xff09;通过这两个点&#xff0c;可以计算出这两个点所表示的模型方程&#xff1b; &#xff08;3&#xff09…
阅读更多...
二维直线拟合--opencv实现

二维直线拟合--opencv实现

二维/三维直线拟合–opencv实现 ​ Hough变换可以提取图像中的直线&#xff0c;但是提取线的精度不高&#xff0c;然而我们的很多场合下需要精确估计直线的参数&#xff0c;这时就需要进行直线拟合。 ​ 直线拟合的方法有很多&#xff0c;比如&#xff1a;一元线性回归就是一…
阅读更多...
直线拟合最小二乘法

直线拟合最小二乘法

数据 x(1,2,3,4,5) y (1,1.5,3,4.5,5) 算法结果 R语言运行结果 算法原理 x的均值&#xff1a; xpsum&#xff08;x1x2x3…xn&#xff09;/n y的均值 : ypsum&#xff08;y1y2y3…yn&#xff09;/n x的平方差之和&#xff1a; lxxsum( (xi-xp) ^ 2 ) 协方差之和 lxy…
阅读更多...
直线生成以及pcl直线拟合

直线生成以及pcl直线拟合

目录 写在前面codecompile&run result参考 写在前面 1、本文内容 pcl直线拟合&#xff0c;生成带噪声的直线并进行直线拟合的demo 2、平台 windows, pcl1.10.0 3、转载请注明出处&#xff1a; https://blog.csdn.net/qq_41102371/article/details/127147223 code line_…
阅读更多...
opencv 直线拟合

opencv 直线拟合

一、话说直线拟合 霍夫直线检测容易受到线段形状与噪声的干扰而失真&#xff0c;这个时候我们需要另辟蹊径&#xff0c;通过对图像进行二值分析&#xff0c;提取骨架&#xff0c;对骨架像素点拟合生成直线&#xff0c;这种做法在一些场景下非常有效&#xff0c;而且效果还比较…
阅读更多...
拟合算法之一 直线拟合

拟合算法之一 直线拟合

直线拟合 很早就想学习拟合了&#xff0c;经常听同事用到拟合&#xff0c;当时尚且一窍不通&#xff0c;必须快递加急紧追此处才是&#xff0c;也参考了网上大佬的一些宝贵经验&#xff0c;先将拟合方法总结如下&#xff1a; 最小二乘法 1.原理 2.举例实现 void fitline3(…
阅读更多...
直线拟合问题（Python实现）

直线拟合问题（Python实现）

程序如下&#xff1a; import matplotlib.pyplot as plt import numpy as npx_list [466, 741, 950, 1422, 1634] y_list [7.04, 4.28, 3.40, 2.54, 2.13] # x_list [0, 1, 3, 5] # y_list [1, 2, 4, 8]l_mat11 len(x_list) l_mat12 l_mat21 sum(x for x in x_list) l_…
阅读更多...
OpenCV直线拟合检测

OpenCV直线拟合检测

点击上方“小白学视觉”&#xff0c;选择加"星标"或“置顶” 重磅干货&#xff0c;第一时间送达本文转自&#xff1a;opencv学堂 OpenCV直线拟合检测 霍夫直线检测容易受到线段形状与噪声的干扰而失真&#xff0c;这个时候我们需要另辟蹊径&#xff0c;通过对图像进行…
阅读更多...
RANSAC 直线拟合

RANSAC 直线拟合

result&#xff1a; code&#xff1a; #include "stdafx.h". #include <opencv2\opencv.hpp> #include <iostream> #include <ctime>using namespace std; using namespace cv;//生成[0,1]之间符合均匀分布的数 double uniformRandom(void) {ret…
阅读更多...
OpenCV——直线拟合

OpenCV——直线拟合

相比于直线检测&#xff0c;直线拟合的最大特点是将所有数据只拟合出一条直线 void fitLine( InputArray points, OutputArray line, int distType,double param, double reps, double aeps ); points&#xff1a;输入待拟合直线的2D或者3D点集。line&#xff1a;输出描述直线…
阅读更多...
直线拟合

直线拟合

在进行直线拟合算法中&#xff0c;一直使用最小二乘法&#xff0c;使用时间长了&#xff0c;也比较熟练了&#xff0c;但是在最近一次使用中&#xff0c;最小二乘法在拟合垂直或者接近垂直的直线时&#xff0c;效果不好&#xff1b;斜率很大&#xff0c;使用稳定性不好。查阅《…
阅读更多...
Android 冷启动 热启动 测试

Android 冷启动 热启动 测试

一、应用的启动 启动方式 通常来说&#xff0c;在安卓中应用的启动方式分为两种&#xff1a;冷启动和热启动。 1、冷启动&#xff1a;当启动应用时&#xff0c;后台没有该应用的进程&#xff0c;这时系统会重新创建一个新的进程分配给该应用&#xff0c;这个启动方式就是冷启动…
阅读更多...
Spring Boot 热启动

Spring Boot 热启动

目的&#xff1a;修改类文件可以马上编译发布&#xff0c;提高了工作效率 步骤&#xff1a; 第一步&#xff1a; 修改pom.xml <!-- 热启动 --> <dependency><groupId>org.springframework.boot</groupId><artifactId>spring-boot-devtools&l…
阅读更多...
APP冷热启动专项测试

APP冷热启动专项测试

一、冷热启动的概念 冷启动&#xff1a;当启动应用时&#xff0c;后台没有该应用的进程&#xff0c;这时系统会重新创建一个新的进程分配给该应用&#xff0c;这个启动方式就是冷启动。 热启动&#xff1a;当启动应用时&#xff0c;后台已有该应用的进程&#xff08;例&#…
阅读更多...
嵌入式linux热启动和冷启动,使用keil判断ARM的冷启动和热启动的方法

嵌入式linux热启动和冷启动,使用keil判断ARM的冷启动和热启动的方法

微处理器:LPC2114 编译环境:Keil MDK V4.10 思路: 常把单片机系统的复位分为冷启动和热启动。所谓冷启动&#xff0c;也就是一般所说的上电复位&#xff0c;冷启动后片内外RAM的内容是随机的&#xff0c;通常是0x00或0xFF&#xff1b;单片机的热启动是通过外部电路给运行中的单…
阅读更多...
推荐文章

Copyright @ 2022~2023