FAST-LIO2.0代码解析(一)preprocess.h
不时会对其中的错误和模糊之处进行更新,未来会把完整的更新在gitee里
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomMsg.h>using namespace std;#define IS_VALID(a) ((abs(a)>1e8) ? true : false) // 没用到typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;// 枚举类型:表示支持的雷达类型
enum LID_TYPE
{AVIA = 1,VELO16,OUST64
}; //{1, 2, 3}// 枚举类型:表示特征点的类型
enum Feature{Nor, // 正常点Poss_Plane, // 可能的平面点Real_Plane, // 确定的平面点Edge_Jump, // 有跨越的边Edge_Plane, // 边上的平面点Wire, // 线段 这个也许当了无效点?也就是空间中的小线段?ZeroPoint // 无效点 程序中未使用
};// 枚举类型:位置标识
enum Surround{Prev, // 前一个Next // 后一个
};// 枚举类型:表示有跨越边的类型
enum E_jump{Nr_nor, // 正常Nr_zero, // 0Nr_180, // 180Nr_inf, // 无穷大 跳变较远?Nr_blind // 在盲区?
};// orgtype类:用于存储激光雷达点的一些其他属性
struct orgtype
{double range; // 点云在xy平面离雷达中心的距离double dista; // 当前点与后一个点之间的距离//假设雷达原点为O 前一个点为M 当前点为A 后一个点为Ndouble angle[2]; // 这个是角OAM和角OAN的cos值double intersect; // 这个是角MAN的cos值E_jump edj[2]; // 前后两点的类型Feature ftype; // 点类型// 构造函数orgtype(){range = 0;edj[Prev] = Nr_nor;edj[Next] = Nr_nor;ftype = Nor;//默认为正常点intersect = 2;}
};// velodyne数据结构
namespace velodyne_ros
{struct EIGEN_ALIGN16 Point{PCL_ADD_POINT4D; // 4D点坐标类型float intensity; // 强度float time; // 时间uint16_t ring; // 点所属的圈数EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐};
} // namespace velodyne_ros// 注册velodyne_ros的Point类型
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(uint16_t, ring, ring)
)// ouster数据结构
namespace ouster_ros
{struct EIGEN_ALIGN16 Point{PCL_ADD_POINT4D; // 4D点坐标类型float intensity; // 强度uint32_t t; // 时间uint16_t reflectivity; // 反射率uint8_t ring; // 点所属的圈数uint16_t ambient; // 没用到uint32_t range; // 距离EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐};
} // namespace ouster_ros// clang-format off
// 注册ouster的Point类型
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)// use std::uint32_t to avoid conflicting with pcl::uint32_t(std::uint32_t, t, t)(std::uint16_t, reflectivity, reflectivity)(std::uint8_t, ring, ring)(std::uint16_t, ambient, ambient)(std::uint32_t, range, range)
)// Preproscess类:用于对激光雷达点云数据进行预处理
class Preprocess
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEWPreprocess(); // 构造函数~Preprocess(); // 析构函数void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对Livox自定义Msg格式的激光雷达数据进行处理void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对ros的Msg格式的激光雷达数据进行处理void set(bool feat_en, int lid_type, double bld, int pfilt_num);// sensor_msgs::PointCloud2::ConstPtr pointcloud;PointCloudXYZI pl_full, pl_corn, pl_surf; // 全部点、边缘点、平面点PointCloudXYZI pl_buff[128]; //maximum 128 line lidarvector<orgtype> typess[128]; //maximum 128 line lidarint lidar_type, point_filter_num, N_SCANS, SCAN_RATE; // 雷达类型、采样间隔、扫描线数、扫描频率double blind; // 最小距离阈值(盲区)bool feature_enabled, given_offset_time; // 是否提取特征、是否进行时间偏移ros::Publisher pub_full, pub_surf, pub_corn; // 发布全部点、发布平面点、发布边缘点private:void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); // 用于对Livox激光雷达数据进行处理void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对ouster激光雷达数据进行处理void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对velodyne激光雷达数据进行处理void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);void pub_func(PointCloudXYZI &pl, const ros::Time &ct);int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); // 没有用到bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);int group_size;double disA, disB, inf_bound;double limit_maxmid, limit_midmin, limit_maxmin;double p2l_ratio;double jump_up_limit, jump_down_limit;double cos160;double edgea, edgeb;double smallp_intersect, smallp_ratio;double vx, vy, vz;
};
FAST-LIO2.0代码解析(一)preprocess.h相关推荐
- FAST-LIO2.0代码解析(二)preprocess.cpp
不时会对其中的错误和模糊之处进行更新,未来会把完整的更新在gitee里 #include "preprocess.h"#define RETURN0 0x00 #define RE ...
- FAST-LIO2.0代码解析(四)laserMapping.cpp
完整版已传gitee https://gitee.com/qq408007026/fast-lio2-noted // This is an advanced implementation of th ...
- FAST-LIO2.0代码解析(三)IMU_Processing.hpp
不时会对其中的错误和模糊之处进行更新,未来会把完整的更新在gitee里 https://gitee.com/qq408007026/fast-lio2-noted 方差推导需要用到 方差_ ...
- 基于STM32+0.96寸OLED - - 7脚SPI接线显示+代码解析
前言 本次我们学习一下STM32关于SPI七脚OLED的 接线方法,OLED的代码解析,主要教会大家怎么使用OLED调试和看懂OLED驱动的代码,让大家对OLED有个简单的了解,本篇博客大部分是自己 ...
- Mina Kimchi SNARK 代码解析
1. 引言 Mina系列博客有: Mina概览 Mina的支付流程 Mina的zkApp Mina中的Pasta(Pallas和Vesta)曲线 Mina中的Schnorr signature Min ...
- Compact Multi-Signatures for Smaller Blockchains代码解析
1. 引言 Boneh等人2018年论文<Compact Multi-Signatures for Smaller Blockchains>,论文解读参见博客 Compact Multi- ...
- STARK中的FRI代码解析
1. 引言 前序博客有: STARK入门知识 STARKs and STARK VM: Proofs of Computational Integrity Fast Reed-Solomon Inte ...
- 中科大+快手出品 CIRS: Bursting Filter Bubbles by Counterfactual Interactive Recommender System 代码解析
文章目录 前言 论文介绍: 代码介绍: 代码: 一. CIRS-UserModel-kuaishou.py 0. get_args() 解析参数 1. create_dir() 2. Prepare ...
- FAST-LIO2代码解析(三)
0. 简介 在讲述完一,二后IMU和雷达的数据输入算是讲明白了,下面几节我们将结合主程序来介绍本文最关键的迭代卡尔曼滤波器和iKD-Tree的算法. 1. 主程序 一开始在laserMapping.c ...
最新文章
- oracle数据库第八章答案,Oracle培训(三十)——Oracle 11g 第八章知识点小结——处理数据...
- 单核7:全景闹钟和单核工作法
- 从锤子手机谈产品的逼格
- DataV:可视化大屏展示神器实战分享
- mysql common是什么_MySQL common_schema简介
- json文件读取之reader.onload中的定义的变量在其函数外部进行处理
- Python大神告诉你,学习Python应该读哪些书!
- 手把手教你D2C,走向前端智能化
- opencv如何把一个矩阵不同列分离开_学习OPEN_CV
- 96. Unique Binary Search Trees(I 和 II)
- ireport使用参考
- Android FrameLayout和AbsoluteLayout示例教程
- P+XS算法中Dirac comb的解释
- 模拟生成微软序列号python_【python】13位随机序列号生成工具 源码分析
- 远程调试监视器 已在计算机上关闭,错误:“Microsoft Visual Studio 远程调试监视器”(MSVSMON.EXE) 似乎没有在远程计算机上运行。...
- 古文觀止卷七_獲麟解_韓愈
- 计算机硬件是外观吗,计算机硬件从外观上看主要有主机箱.doc
- JAVA中两台电脑通信_如何实现两台PC终端基于MAC地址互相通信
- Bezier曲线原理及实现代码(c++)
- BSP板机支持包、linux启动分析、ARM裸机编程
热门文章
- 学习实践-Vicuna【小羊驼】(部署+运行)
- 测试——Monkey测试的介绍及使用
- 2019下半年的教师资格考试~学霸大佬们总结的记忆口诀涨分必备
- 斩获微软offer后,我总结出这10个面试必备技巧(五星干货)
- 计算机无法搜索到打印机驱动,电脑连接打印机需要装什么驱动(电脑搜不到打印机设备)...
- android socket上传视频教程,android socket视频流方案
- isNaN()和isFinite()的应用
- 台式电脑怎么添加计算机硬盘,台式机怎么加硬盘 台式机加硬盘教程介绍【图文详解】...
- Android NFC的应用
- hbuilderx升级3.6.5版本后运行到手机端同步资源失败,未得到同步资源的授权,请停止运行后重新运行,并注意手机上的授权提示