LeGO-LOAM 几个特有函数的分析(2)

2024-01-05 21:36

本文主要是介绍LeGO-LOAM 几个特有函数的分析(2),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

接上回LeGO-LOAM 几个特有函数的分析(1)

二、广度优先遍历

广度优先遍历(Breadth-First Search, BFS)是一种用于遍历或搜索树或图的算法。这种算法从树的根(或图的某一指定节点)开始,然后探索邻近的节点,之后对每一个邻近的节点,它再去探索它们各自相邻的节点,这个过程持续进行直到访问所有可达的节点。

广度优先遍历的主要特点是它按照距离起始点的“层次”来遍历。首先访问距离起点最近的节点,然后是它们的邻居,如此类推。

2.1 广度优先遍历的步骤:

  1. 初始化:首先将起始节点放入队列中。

  2. 遍历

    • 从队列中弹出一个节点。
    • 检查该节点是否为目标节点。如果是,则完成搜索。
    • 将该节点的所有未访问过的邻居节点加入队列。
  3. 重复:重复步骤2,直到队列为空或找到目标节点。

  4. 结束:当队列为空且目标未找到,或已找到目标节点时,算法结束。

2.2基于 BFS 的点云聚类和外点剔除

2.2.1原理

 

 2.2.2源码注释

    void labelComponents(int row, int col){// use std::queue std::vector std::deque will slow the program down greatly// 声明所需的变量,输入的ROW和col是单帧点云第几行第几列的点// 用于存储距离和角度计算的临时变量float d1, d2, alpha, angle;// 用于存储索引的变量int fromIndX, fromIndY, thisIndX, thisIndY;// 标记是否每个扫描线都至少有一个点被添加bool lineCountFlag[N_SCAN] = {false};//用两个数组分别保存x,yqueueIndX[0] = row;queueIndY[0] = col;//算法标志int queueSize = 1;// 队列开始的索引int queueStartInd = 0;// 队列结束的索引int queueEndInd = 1;// 初始化聚类数组allPushedIndX[0] = row;allPushedIndY[0] = col;//计数int allPushedIndSize = 1;//很巧妙,有有效邻点就加一,每次循环减1,实现bfs广度优先遍历关键while(queueSize > 0){// Pop point// 取出当前点x,y坐标fromIndX = queueIndX[queueStartInd];fromIndY = queueIndY[queueStartInd];//队列大小减一--queueSize;//索引加一++queueStartInd;// Mark popped point// 标记该点为一类,聚类就是给点加标签,标签一致的就是一类labelMat.at<int>(fromIndX, fromIndY) = labelCount;// Loop through all the neighboring grids of popped grid// 检查所有邻点for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){// new index// 计算邻点的索引,其实就是上下左右四个点thisIndX = fromIndX + (*iter).first;thisIndY = fromIndY + (*iter).second;// index should be within the boundary// 如果raw为0或者15,上或者下没有邻点,跳过if (thisIndX < 0 || thisIndX >= N_SCAN)continue;// at range image margin (left or right side)//设置矩阵最两边的点也为邻点,因为VLP16是360度//在cow为0时左边的邻点,在1799if (thisIndY < 0)thisIndY = Horizon_SCAN - 1;//在cow为1799时左边的邻点,在0if (thisIndY >= Horizon_SCAN)thisIndY = 0;// prevent infinite loop (caused by put already examined point back)// 如果该点已被标记,则跳过if (labelMat.at<int>(thisIndX, thisIndY) != 0)continue;// 计算角度差以决定是否将邻点加入到当前区域// 距离雷达远的是D1,近的是D2d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),rangeMat.at<float>(thisIndX, thisIndY));d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY));//(0,-1),(0,1),意味着是一条线上的点,角度是360/1800*3.14/180=0.0035if ((*iter).first == 0)alpha = segmentAlphaX;else//(1,0),(-1,0),意味着是上下两条线上的点,角度是30/(16-1)*3.14/180=0.035alpha = segmentAlphaY;//计算图中angle角度angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));//如果角度大于60度if (angle > segmentTheta){//把此邻点放入队列queueIndX[queueEndInd] = thisIndX;queueIndY[queueEndInd] = thisIndY;//增加size++queueSize;//末尾索引右移++queueEndInd;//把此邻点赋上和之前取出来的点一样的标签labelMat.at<int>(thisIndX, thisIndY) = labelCount;//这行有点被标记过lineCountFlag[thisIndX] = true;//保存聚类结果allPushedIndX[allPushedIndSize] = thisIndX;allPushedIndY[allPushedIndSize] = thisIndY;++allPushedIndSize;}}}// check if this segment is validbool feasibleSegment = false;//如果聚类大于30则认为是一个好的聚类if (allPushedIndSize >= 30)feasibleSegment = true;//如果大于5,而且都是竖着的超过3个,也认为是一个好聚类,可能是树,电线杆else if (allPushedIndSize >= segmentValidPointNum){int lineCount = 0;for (size_t i = 0; i < N_SCAN; ++i)if (lineCountFlag[i] == true)++lineCount;if (lineCount >= segmentValidLineNum)feasibleSegment = true;            }// segment is valid, mark these points//如果聚类成功,标签加一if (feasibleSegment == true){++labelCount;}else{ // segment is invalid, mark these pointsfor (size_t i = 0; i < allPushedIndSize; ++i){//不成功,则标记为999999,代表依托答辩labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;}}}
 需要注意的点:
一是 邻点的定义,就是代表取当前点上下左右四个点
std::pair<int8_t, int8_t> neighbor;
neighbor.first = -1; neighbor.second =  0; neighborIterator.push_back(neighbor);
neighbor.first =  0; neighbor.second =  1; neighborIterator.push_back(neighbor);
neighbor.first =  0; neighbor.second = -1; neighborIterator.push_back(neighbor);
neighbor.first =  1; neighbor.second =  0; neighborIterator.push_back(neighbor);
 二是 巧妙的通过queueSize 实现广度优先遍历算法的核心

开始是int queueSize =1,让其进入循环

while(queueSize > 0){//队列大小减一--queueSize;for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){//如果角度大于60度if (angle > segmentTheta){//增加size++queueSize;}}
}
三是 聚类时候,大于30个点,或者大于5个点,但是有三个竖着的聚为一类

我觉得原因是考虑到竖着的点距离远的因素

四是 通过计算角度来判断是否是邻点

想象一下,是不是D1越长,angle越小

2.3函数的调用

用此种方式实现了一帧雷达所有点的聚类

        for (size_t i = 0; i < N_SCAN; ++i)for (size_t j = 0; j < Horizon_SCAN; ++j)//上一个函数说过地面点label被置为1 //如果这个点既不是地面点也没有聚类过,开始聚类if (labelMat.at<int>(i,j) == 0)labelComponents(i, j);

这篇关于LeGO-LOAM 几个特有函数的分析(2)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/574232

相关文章

每天认识几个maven依赖(ActiveMQ+activemq-jaxb+activesoap+activespace+adarwin)

八、ActiveMQ 1、是什么? ActiveMQ 是一个开源的消息中间件(Message Broker),由 Apache 软件基金会开发和维护。它实现了 Java 消息服务(Java Message Service, JMS)规范,并支持多种消息传递协议,包括 AMQP、MQTT 和 OpenWire 等。 2、有什么用? 可靠性:ActiveMQ 提供了消息持久性和事务支持,确保消

性能分析之MySQL索引实战案例

文章目录 一、前言二、准备三、MySQL索引优化四、MySQL 索引知识回顾五、总结 一、前言 在上一讲性能工具之 JProfiler 简单登录案例分析实战中已经发现SQL没有建立索引问题,本文将一起从代码层去分析为什么没有建立索引? 开源ERP项目地址:https://gitee.com/jishenghua/JSH_ERP 二、准备 打开IDEA找到登录请求资源路径位置

hdu1171(母函数或多重背包)

题意:把物品分成两份,使得价值最接近 可以用背包,或者是母函数来解,母函数(1 + x^v+x^2v+.....+x^num*v)(1 + x^v+x^2v+.....+x^num*v)(1 + x^v+x^2v+.....+x^num*v) 其中指数为价值,每一项的数目为(该物品数+1)个 代码如下: #include<iostream>#include<algorithm>

uva 10061 How many zero's and how many digits ?(不同进制阶乘末尾几个0)+poj 1401

题意是求在base进制下的 n!的结果有几位数,末尾有几个0。 想起刚开始的时候做的一道10进制下的n阶乘末尾有几个零,以及之前有做过的一道n阶乘的位数。 当时都是在10进制下的。 10进制下的做法是: 1. n阶位数:直接 lg(n!)就是得数的位数。 2. n阶末尾0的个数:由于2 * 5 将会在得数中以0的形式存在,所以计算2或者计算5,由于因子中出现5必然出现2,所以直接一

SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

查看原文>>>全流程SWAP农业模型数据制备、敏感性分析及气候变化影响实践技术应用 SWAP模型是由荷兰瓦赫宁根大学开发的先进农作物模型,它综合考虑了土壤-水分-大气以及植被间的相互作用;是一种描述作物生长过程的一种机理性作物生长模型。它不但运用Richard方程,使其能够精确的模拟土壤中水分的运动,而且耦合了WOFOST作物模型使作物的生长描述更为科学。 本文让更多的科研人员和农业工作者

MOLE 2.5 分析分子通道和孔隙

软件介绍 生物大分子通道和孔隙在生物学中发挥着重要作用,例如在分子识别和酶底物特异性方面。 我们介绍了一种名为 MOLE 2.5 的高级软件工具,该工具旨在分析分子通道和孔隙。 与其他可用软件工具的基准测试表明,MOLE 2.5 相比更快、更强大、功能更丰富。作为一项新功能,MOLE 2.5 可以估算已识别通道的物理化学性质。 软件下载 https://pan.quark.cn/s/57

衡石分析平台使用手册-单机安装及启动

单机安装及启动​ 本文讲述如何在单机环境下进行 HENGSHI SENSE 安装的操作过程。 在安装前请确认网络环境,如果是隔离环境,无法连接互联网时,请先按照 离线环境安装依赖的指导进行依赖包的安装,然后按照本文的指导继续操作。如果网络环境可以连接互联网,请直接按照本文的指导进行安装。 准备工作​ 请参考安装环境文档准备安装环境。 配置用户与安装目录。 在操作前请检查您是否有 sud

C++操作符重载实例(独立函数)

C++操作符重载实例,我们把坐标值CVector的加法进行重载,计算c3=c1+c2时,也就是计算x3=x1+x2,y3=y1+y2,今天我们以独立函数的方式重载操作符+(加号),以下是C++代码: c1802.cpp源代码: D:\YcjWork\CppTour>vim c1802.cpp #include <iostream>using namespace std;/*** 以独立函数

线性因子模型 - 独立分量分析(ICA)篇

序言 线性因子模型是数据分析与机器学习中的一类重要模型,它们通过引入潜变量( latent variables \text{latent variables} latent variables)来更好地表征数据。其中,独立分量分析( ICA \text{ICA} ICA)作为线性因子模型的一种,以其独特的视角和广泛的应用领域而备受关注。 ICA \text{ICA} ICA旨在将观察到的复杂信号

函数式编程思想

我们经常会用到各种各样的编程思想,例如面向过程、面向对象。不过笔者在该博客简单介绍一下函数式编程思想. 如果对函数式编程思想进行概括,就是f(x) = na(x) , y=uf(x)…至于其他的编程思想,可能是y=a(x)+b(x)+c(x)…,也有可能是y=f(x)=f(x)/a + f(x)/b+f(x)/c… 面向过程的指令式编程 面向过程,简单理解就是y=a(x)+b(x)+c(x)