代码浅析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。

相关文章

springboot循环依赖问题案例代码及解决办法

springboot循环依赖问题案例代码及解决办法

《springboot循环依赖问题案例代码及解决办法》在SpringBoot中,如果两个或多个Bean之间存在循环依赖（即BeanA依赖BeanB,而BeanB又依赖BeanA）,会导致Spring的... 目录1. 什么是循环依赖？2. 循环依赖的场景案例3. 解决循环依赖的常见方法方法 1：使用 @La
阅读更多...
使用C#代码在PDF文档中添加、删除和替换图片

使用C#代码在PDF文档中添加、删除和替换图片

《使用C#代码在PDF文档中添加、删除和替换图片》在当今数字化文档处理场景中,动态操作PDF文档中的图像已成为企业级应用开发的核心需求之一,本文将介绍如何在.NET平台使用C#代码在PDF文档中添加、... 目录引言用C#添加图片到PDF文档用C#删除PDF文档中的图片用C#替换PDF文档中的图片引言在当
阅读更多...
C#使用SQLite进行大数据量高效处理的代码示例

C#使用SQLite进行大数据量高效处理的代码示例

《C#使用SQLite进行大数据量高效处理的代码示例》在软件开发中,高效处理大数据量是一个常见且具有挑战性的任务,SQLite因其零配置、嵌入式、跨平台的特性,成为许多开发者的首选数据库,本文将深入探... 目录前言准备工作数据实体核心技术批量插入：从乌龟到猎豹的蜕变分页查询：加载百万数据异步处理：拒绝界面
阅读更多...
用js控制视频播放进度基本示例代码

用js控制视频播放进度基本示例代码

《用js控制视频播放进度基本示例代码》写前端的时候,很多的时候是需要支持要网页视频播放的功能,下面这篇文章主要给大家介绍了关于用js控制视频播放进度的相关资料,文中通过代码介绍的非常详细,需要的朋友可... 目录前言html部分:JavaScript部分:注意：总结前言在javascript中控制视频播放
阅读更多...
Spring Boot 3.4.3 基于 Spring WebFlux 实现 SSE 功能(代码示例)

Spring Boot 3.4.3 基于 Spring WebFlux 实现 SSE 功能(代码示例)

《SpringBoot3.4.3基于SpringWebFlux实现SSE功能(代码示例)》SpringBoot3.4.3结合SpringWebFlux实现SSE功能,为实时数据推送提供... 目录1. SSE 简介1.1 什么是 SSE？1.2 SSE 的优点1.3 适用场景2. Spring WebFlu
阅读更多...
java之Objects.nonNull用法代码解读

java之Objects.nonNull用法代码解读

《java之Objects.nonNull用法代码解读》：本文主要介绍java之Objects.nonNull用法代码,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐... 目录Java之Objects.nonwww.chinasem.cnNull用法代码Objects.nonN
阅读更多...
SpringBoot实现MD5加盐算法的示例代码

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.
阅读更多...
python+opencv处理颜色之将目标颜色转换实例代码

python+opencv处理颜色之将目标颜色转换实例代码

《python+opencv处理颜色之将目标颜色转换实例代码》OpenCV是一个的跨平台计算机视觉库,可以运行在Linux、Windows和MacOS操作系统上,：本文主要介绍python+ope... 目录下面是代码+ 效果 + 解释转HSV： 关于颜色总是要转HSV的掩膜再标注总结 目标：将红色的部分滤
阅读更多...
Python Dash框架在数据可视化仪表板中的应用与实践记录

Python Dash框架在数据可视化仪表板中的应用与实践记录

《PythonDash框架在数据可视化仪表板中的应用与实践记录》Python的PlotlyDash库提供了一种简便且强大的方式来构建和展示互动式数据仪表板,本篇文章将深入探讨如何使用Dash设计一... 目录python Dash框架在数据可视化仪表板中的应用与实践1. 什么是Plotly Dash？1.1
阅读更多...
在C#中调用Python代码的两种实现方式

在C#中调用Python代码的两种实现方式

《在C#中调用Python代码的两种实现方式》：本文主要介绍在C#中调用Python代码的两种实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录C#调用python代码的方式1. 使用 Python.NET2. 使用外部进程调用 Python 脚本总结C#调
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2