代码浅析DLIO(一)---整体框架梳理

2023-10-30 15:28
文章标签 代码 整体 框架 浅析 梳理 dlio

本文主要是介绍代码浅析DLIO(一)---整体框架梳理，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

0. 简介

我们刚刚了解过DLIO的整个流程，我们发现相比于Point-LIO而言，这个方法更适合我们去学习理解，同时官方给出的结果来看DLIO的结果明显好于现在的主流方法，当然指的一提的是，这个DLIO是必须需要六轴IMU的，所以如果没有IMU的画，那只有DLO可以使用了。

在这里插入图片描述

1. OdomNode–DLIO构造函数

OdomNode是一个ROS节点的构造函数，主要用于初始化节点的参数、订阅和发布的话题以及一些算法所需的参数和数据结构。在构造函数中，首先通过调用getParams()函数获得节点的参数，然后获取最大的线程数。接着初始化一些标志位和数据结构，例如dlio标定是否初始化、第一帧有效观测、第一帧IMU数据等。之后通过ros::NodeHandle订阅和发布话题，其中包括点云、IMU数据、odom、pose、path、关键帧位姿、关键帧点云和去畸变的点云。然后通过创建timer定时发布pose。接下来初始化位姿、lidar位置、IMU数据以及点云数据结构等。之后设置一些算法所需的参数，例如gicp的参数、voxel的参数等。最后获取CPU的信息并初始化一些度量指标和CPU时间。整个构造函数的主要作用是初始化节点的参数、订阅和发布话题以及一些算法所需的参数和数据结构，为后续的节点运行做好准备。

dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) {this->getParams(); //获得参数，主要是在cfg中读取的this->num_threads_ = omp_get_max_threads(); //获取最大的thread线程this->dlio_initialized = false;   // dlio标定是否初始化this->first_valid_scan = false;   //第一帧有效观测this->first_imu_received = false; //第一帧IMU数据if (this->imu_calibrate_) { // IMU是否已经标定，设置为true时候直接从cfg中读取this->imu_calibrated = false;} else {this->imu_calibrated = true;}this->deskew_status = false; //是否进行去畸变this->deskew_size = 0;       //去畸变的点云数量this->lidar_sub =this->nh.subscribe("pointcloud", 1, &dlio::OdomNode::callbackPointCloud,this, ros::TransportHints().tcpNoDelay());this->imu_sub = this->nh.subscribe("imu", 1000, &dlio::OdomNode::callbackImu,this, ros::TransportHints().tcpNoDelay());this->odom_pub =this->nh.advertise<nav_msgs::Odometry>("odom", 1, true); //发布odomthis->pose_pub = this->nh.advertise<geometry_msgs::PoseStamped>("pose", 1, true); //发布posethis->path_pub =this->nh.advertise<nav_msgs::Path>("path", 1, true); //发布paththis->kf_pose_pub = this->nh.advertise<geometry_msgs::PoseArray>("kf_pose", 1, true); //发布关键帧的位姿this->kf_cloud_pub = this->nh.advertise<sensor_msgs::PointCloud2>("kf_cloud", 1, true); //发布关键帧的点云this->deskewed_pub = this->nh.advertise<sensor_msgs::PointCloud2>("deskewed", 1, true); //发布去畸变的点云this->publish_timer =this->nh.createTimer(ros::Duration(0.01), &dlio::OdomNode::publishPose,this); //根据timer，发布posethis->T = Eigen::Matrix4f::Identity();       //初始化Tthis->T_prior = Eigen::Matrix4f::Identity(); //初始化T_prior，上一帧位姿this->T_corr = Eigen::Matrix4f::Identity(); //初始化T_corr,当前位姿this->origin = Eigen::Vector3f(0., 0., 0.);         //初始化原点this->state.p = Eigen::Vector3f(0., 0., 0.);        //初始化位置this->state.q = Eigen::Quaternionf(1., 0., 0., 0.); //初始化四元数this->state.v.lin.b = Eigen::Vector3f(0., 0., 0.); //初始化线速度,机体坐标系下this->state.v.lin.w = Eigen::Vector3f(0., 0., 0.); //初始化线速度,世界坐标系下this->state.v.ang.b = Eigen::Vector3f(0., 0., 0.); //初始化角速度,机体坐标系下this->state.v.ang.w = Eigen::Vector3f(0., 0., 0.); //初始化角速度,世界坐标系下this->lidarPose.p = Eigen::Vector3f(0., 0., 0.);        //初始化lidar位置this->lidarPose.q = Eigen::Quaternionf(1., 0., 0., 0.); //初始化lidar四元数this->imu_meas.stamp = 0.;this->imu_meas.ang_vel[0] = 0.; //初始化IMU的角速度this->imu_meas.ang_vel[1] = 0.;this->imu_meas.ang_vel[2] = 0.;this->imu_meas.lin_accel[0] = 0.; //初始化IMU的线加速度this->imu_meas.lin_accel[1] = 0.;this->imu_meas.lin_accel[2] = 0.;this->imu_buffer.set_capacity(this->imu_buffer_size_); //设置IMU的bufferthis->first_imu_stamp = 0.;this->prev_imu_stamp = 0.;this->original_scan = pcl::PointCloud<PointType>::ConstPtr(boost::make_shared<const pcl::PointCloud<PointType>>()); //初始化原始点云this->deskewed_scan = pcl::PointCloud<PointType>::ConstPtr(boost::make_shared<const pcl::PointCloud<PointType>>()); //初始化去畸变点云this->current_scan = pcl::PointCloud<PointType>::ConstPtr(boost::make_shared<const pcl::PointCloud<PointType>>()); //初始化当前点云this->submap_cloud = pcl::PointCloud<PointType>::ConstPtr(boost::make_shared<const pcl::PointCloud<PointType>>()); //初始化子图点云this->num_processed_keyframes = 0; //初始化处理的关键帧数量this->submap_hasChanged = true; //初始化子图是否改变,第一帧肯定改变this->submap_kf_idx_prev.clear(); //初始化上一帧的关键帧索引this->first_scan_stamp = 0.; //初始化第一帧点云的时间戳this->elapsed_time = 0.;     //初始化时间this->length_traversed;      //初始化长度this->convex_hull.setDimension(3);  //设置凸包的维度this->concave_hull.setDimension(3); //设置凹包的维度this->concave_hull.setAlpha(this->keyframe_thresh_dist_); //设置凹包的阈值this->concave_hull.setKeepInformation(true); //设置凹包保留信息this->gicp.setCorrespondenceRandomness(this->gicp_k_correspondences_); //设置gicp的参数,这个值代表每次迭代时，随机选择的点对的数量this->gicp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_); //设置gicp的参数,这个值代表两个点云中对应点之间的最大距离this->gicp.setMaximumIterations(this->gicp_max_iter_); //设置gicp的参数,这个值代表最大迭代次数this->gicp.setTransformationEpsilon(this->gicp_transformation_ep_); //设置gicp的参数,这个值代表两次迭代之间的最小差异this->gicp.setRotationEpsilon(this->gicp_rotation_ep_); //设置gicp的参数,这个值代表两次迭代之间的最小旋转差异this->gicp.setInitialLambdaFactor(this->gicp_init_lambda_factor_); //设置gicp的参数,这个值代表初始lambda因子this->gicp_temp.setCorrespondenceRandomness(this->gicp_k_correspondences_); //设置gicp的参数,这个值代表每次迭代时，随机选择的点对的数量this->gicp_temp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_); //设置gicp的参数,这个值代表两个点云中对应点之间的最大距离this->gicp_temp.setMaximumIterations(this->gicp_max_iter_); //设置gicp的参数,这个值代表最大迭代次数this->gicp_temp.setTransformationEpsilon(this->gicp_transformation_ep_); //设置gicp的参数,这个值代表两次迭代之间的最小差异this->gicp_temp.setRotationEpsilon(this->gicp_rotation_ep_); //设置gicp的参数,这个值代表两次迭代之间的最小旋转差异this->gicp_temp.setInitialLambdaFactor(this->gicp_init_lambda_factor_); //设置gicp的参数,这个值代表初始lambda因子pcl::Registration<PointType, PointType>::KdTreeReciprocalPtr temp;this->gicp.setSearchMethodSource(temp, true); //设置gicp的参数,这个值代表搜索源点的方法this->gicp.setSearchMethodTarget(temp, true); //设置gicp的参数,这个值代表搜索目标点的方法this->gicp_temp.setSearchMethodSource(temp, true); //设置gicp的参数,这个值代表搜索源点的方法this->gicp_temp.setSearchMethodTarget(temp, true); //设置gicp的参数,这个值代表搜索目标点的方法this->geo.first_opt_done = false; //初始化几何观测的第一次优化this->geo.prev_vel = Eigen::Vector3f(0., 0., 0.); //初始化几何观测的上一次速度pcl::console::setVerbosityLevel(pcl::console::L_ERROR);this->crop.setNegative(true); //设置crop的参数,让所有内部的点都被删除this->crop.setMin(Eigen::Vector4f(-this->crop_size_, -this->crop_size_,-this->crop_size_, 1.0));this->crop.setMax(Eigen::Vector4f(this->crop_size_, this->crop_size_,this->crop_size_, 1.0));this->voxel.setLeafSize(this->vf_res_, this->vf_res_,this->vf_res_); //设置voxel的参数,这个值代表体素的大小this->metrics.spaciousness.push_back(0.); //初始化度量指标this->metrics.density.push_back(this->gicp_max_corr_dist_); //初始化度量指标// CPU Specschar CPUBrandString[0x40];memset(CPUBrandString, 0, sizeof(CPUBrandString));this->cpu_type = "";#ifdef HAS_CPUID //如果有cpuidunsigned int CPUInfo[4] = {0, 0, 0, 0};__cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2],CPUInfo[3]); //获取CPU的信息unsigned int nExIds = CPUInfo[0];for (unsigned int i = 0x80000000; i <= nExIds; ++i) {__cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]);if (i == 0x80000002) //获取CPU的型号memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo));else if (i == 0x80000003)memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo));else if (i == 0x80000004)memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo));}this->cpu_type = CPUBrandString;boost::trim(this->cpu_type);
#endifFILE *file;struct tms timeSample;char line[128];this->lastCPU = times(&timeSample);       //获取CPU的时间this->lastSysCPU = timeSample.tms_stime;  //获取CPU的系统时间this->lastUserCPU = timeSample.tms_utime; //获取CPU的用户时间file = fopen("/proc/cpuinfo", "r");this->numProcessors = 0;while (fgets(line, 128, file) != nullptr) {if (strncmp(line, "processor", 9) == 0)this->numProcessors++;}fclose(file);
}

2. callbackPointCloud–DLIO主要进程输入

这个代码是基本DLIO所有主要函数的调用渠道，用于处理传感器数据并进行实时SLAM。具体来说，该函数接收一个类型为sensor_msgs::PointCloud2ConstPtr的指针，并对其进行一系列处理。以下是该函数的主要步骤：

  • 获取当前时间戳，用于计算处理时间。
  • 如果是第一次处理，则记录第一个扫描的时间戳。
  • 进行DLIO的初始化，包括IMU校准和重力校准。
  • 将传入的点云数据转换为DLIO格式。
  • 预处理点云。
  • 检查点云是否有效，如果点数过少则返回错误。
  • 计算度量指标，并在单独的线程中进行。
  • 如果开启了自适应参数，则设置自适应参数。
  • 将当前扫描设置为输入源，并传入GICP算法进行配准。
  • 如果当前没有关键帧，则将初始帧设置为第一关键帧，并在单独的线程中构建子地图和关键帧。
  • 通过IMU、S2M和GEO方法获取下一个姿态。
  • 更新当前关键帧姿态和地图。
  • 如果需要，构建关键帧法线和子地图。
  • 更新轨迹和时间戳。
  • 将处理后的点云发布到ROS。
  • 更新一些统计数据。
  • 发布自定义DLIO消息。

该代码段中的函数主要使用了多线程技术，并涉及到大量的SLAM算法和数据处理操作，如GICP配准、IMU校准、重力校准、位姿估计、轨迹更新等。

void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr &pc) {std::unique_lock<decltype(this->main_loop_running_mutex)> lock(main_loop_running_mutex);this->main_loop_running = true;lock.unlock();double then = ros::Time::now().toSec();if (this->first_scan_stamp == 0.) {this->first_scan_stamp = pc->header.stamp.toSec();}// DLIO Initialization procedures (IMU calib, gravity align)if (!this->dlio_initialized) {this->initializeDLIO();}// 将传入的扫描转换为DLIO格式this->getScanFromROS(pc);// 预处理点云this->preprocessPoints();if (!this->first_valid_scan) {return;}if (this->current_scan->points.size() <= this->gicp_min_num_points_) {ROS_FATAL("Low number of points in the cloud!");return;}// 计算度量指标this->metrics_thread = std::thread(&dlio::OdomNode::computeMetrics, this);this->metrics_thread.detach();// 设置自适应参数if (this->adaptive_params_) {this->setAdaptiveParams();}// 将新帧设置为输入源，并传入GICPthis->setInputSource();// 将初始帧设置为第一关键帧if (this->keyframes.size() == 0) {this->initializeInputTarget();this->main_loop_running = false;this->submap_future =std::async(std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap,this, this->state);this->submap_future.wait(); // 等待任务完成return;}// 通过IMU + S2M + GEO获取下一个姿态this->getNextPose();// 更新当前关键帧姿态和地图this->updateKeyframes();// 如果需要，构建关键帧法线和子地图（如果我们还没有在等待中）if (this->new_submap_is_ready) {this->main_loop_running = false;this->submap_future =std::async(std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap,this, this->state);} else {lock.lock();this->main_loop_running = false;lock.unlock();this->submap_build_cv.notify_one();}// 更新轨迹this->trajectory.push_back(std::make_pair(this->state.p, this->state.q));// 更新时间戳this->lidar_rates.push_back(1. / (this->scan_stamp - this->prev_scan_stamp));this->prev_scan_stamp = this->scan_stamp;this->elapsed_time = this->scan_stamp - this->first_scan_stamp;// 将信息发布到ROSpcl::PointCloud<PointType>::ConstPtr published_cloud;if (this->densemap_filtered_) {published_cloud = this->current_scan;} else {published_cloud = this->deskewed_scan;}this->publish_thread = std::thread(&dlio::OdomNode::publishToROS, this,published_cloud, this->T_corr);this->publish_thread.detach();// 更新一些统计数据this->comp_times.push_back(ros::Time::now().toSec() - then);this->gicp_hasConverged = this->gicp.hasConverged();// 调试语句和发布自定义DLIO消息this->debug_thread = std::thread(&dlio::OdomNode::debug, this);this->debug_thread.detach();this->geo.first_opt_done = true; //第一次优化完成
}

3. callbackImu—DLIO的IMU状态传播

下面的函数主要用于处理传感器数据，进行IMU校准，并将校准后的IMU测量值存储到IMU缓冲区中。该函数接收一个sensor_msgs::Imu类型的指针作为输入参数，并将该指针转换为sensor_msgs::Imu::Ptr类型的指针，以便进行操作。

函数首先将接收到的IMU数据标记为已接收状态，然后获取该数据的角速度和线性加速度信息。如果是第一次接收到IMU数据，则记录第一次接收到的时间戳。接下来进行IMU校准程序，该程序持续三秒钟，期间会记录一段时间内的陀螺仪和加速度计数据，并计算它们的平均值。如果需要进行重力校准，则会通过估计重力向量，计算出重力对齐方向，并更新状态量。如果需要进行加速度计校准，则会将重力从平均加速度中减去以得到偏差，并更新状态量。如果需要进行陀螺仪校准，则会计算陀螺仪的偏差，并更新状态量。最后，将IMU校准完成标记为真。

…详情请参照古月居

这篇关于代码浅析DLIO(一)---整体框架梳理的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/308842。

相关文章

利用Python调试串口的示例代码

利用Python调试串口的示例代码

《利用Python调试串口的示例代码》在嵌入式开发、物联网设备调试过程中,串口通信是最基础的调试手段本文将带你用Python+ttkbootstrap打造一款高颜值、多功能的串口调试助手,需要的可以了... 目录概述：为什么需要专业的串口调试工具项目架构设计1.1 技术栈选型1.2 关键类说明1.3 线程模
阅读更多...
Python Transformers库(NLP处理库)案例代码讲解

Python Transformers库(NLP处理库)案例代码讲解

《PythonTransformers库(NLP处理库)案例代码讲解》本文介绍transformers库的全面讲解,包含基础知识、高级用法、案例代码及学习路径,内容经过组织,适合不同阶段的学习者,对... 目录一、基础知识1. Transformers 库简介2. 安装与环境配置3. 快速上手示例二、核心模
阅读更多...
Java的栈与队列实现代码解析

Java的栈与队列实现代码解析

《Java的栈与队列实现代码解析》栈是常见的线性数据结构,栈的特点是以先进后出的形式,后进先出,先进后出,分为栈底和栈顶,栈应用于内存的分配,表达式求值,存储临时的数据和方法的调用等,本文给大家介绍J... 目录栈的概念（Stack）栈的实现代码队列（Queue）模拟实现队列(双链表实现)循环队列（循环数组
阅读更多...
使用Java将DOCX文档解析为Markdown文档的代码实现

使用Java将DOCX文档解析为Markdown文档的代码实现

《使用Java将DOCX文档解析为Markdown文档的代码实现》在现代文档处理中,Markdown（MD）因其简洁的语法和良好的可读性,逐渐成为开发者、技术写作者和内容创作者的首选格式,然而,许多文... 目录引言1. 工具和库介绍2. 安装依赖库3. 使用Apache POI解析DOCX文档4. 将解析
阅读更多...
C++使用printf语句实现进制转换的示例代码

C++使用printf语句实现进制转换的示例代码

《C++使用printf语句实现进制转换的示例代码》在C语言中,printf函数可以直接实现部分进制转换功能,通过格式说明符（formatspecifier）快速输出不同进制的数值,下面给大家分享C+... 目录一、printf 原生支持的进制转换1. 十进制、八进制、十六进制转换2. 显示进制前缀3. 指
阅读更多...
浅析Java中如何优雅地处理null值

浅析Java中如何优雅地处理null值

《浅析Java中如何优雅地处理null值》这篇文章主要为大家详细介绍了如何结合Lambda表达式和Optional,让Java更优雅地处理null值,感兴趣的小伙伴可以跟随小编一起学习一下... 目录场景 1：不为 null 则执行场景 2：不为 null 则返回，为 null 则返回特定值或抛出异常场景
阅读更多...
使用Python实现全能手机虚拟键盘的示例代码

使用Python实现全能手机虚拟键盘的示例代码

《使用Python实现全能手机虚拟键盘的示例代码》在数字化办公时代,你是否遇到过这样的场景：会议室投影电脑突然键盘失灵、躺在沙发上想远程控制书房电脑、或者需要给长辈远程协助操作？今天我要分享的Pyth... 目录一、项目概述：不止于键盘的远程控制方案1.1 创新价值1.2 技术栈全景二、需求实现步骤一、需求
阅读更多...
Java中Date、LocalDate、LocalDateTime、LocalTime、时间戳之间的相互转换代码

Java中Date、LocalDate、LocalDateTime、LocalTime、时间戳之间的相互转换代码

《Java中Date、LocalDate、LocalDateTime、LocalTime、时间戳之间的相互转换代码》：本文主要介绍Java中日期时间转换的多种方法,包括将Date转换为LocalD... 目录一、Date转LocalDateTime二、Date转LocalDate三、LocalDateTim
阅读更多...
jupyter代码块没有运行图标的解决方案

jupyter代码块没有运行图标的解决方案

《jupyter代码块没有运行图标的解决方案》：本文主要介绍jupyter代码块没有运行图标的解决方案,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录jupyter代码块没有运行图标的解决1.找到Jupyter notebook的系统配置文件2.这时候一般会搜索到
阅读更多...
Python通过模块化开发优化代码的技巧分享

Python通过模块化开发优化代码的技巧分享

《Python通过模块化开发优化代码的技巧分享》模块化开发就是把代码拆成一个个“零件”,该封装封装,该拆分拆分,下面小编就来和大家简单聊聊python如何用模块化开发进行代码优化吧... 目录什么是模块化开发如何拆分代码改进版：拆分成模块让模块更强大：使用 __init__.py你一定会遇到的问题模www.
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2