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

相关文章

C++使用栈实现括号匹配的代码详解

C++使用栈实现括号匹配的代码详解

《C++使用栈实现括号匹配的代码详解》在编程中,括号匹配是一个常见问题,尤其是在处理数学表达式、编译器解析等任务时,栈是一种非常适合处理此类问题的数据结构,能够精确地管理括号的匹配问题,本文将通过C+... 目录引言问题描述代码讲解代码解析栈的状态表示测试总结引言在编程中，括号匹配是一个常见问题，尤其是在
阅读更多...
Java调用DeepSeek API的最佳实践及详细代码示例

Java调用DeepSeek API的最佳实践及详细代码示例

《Java调用DeepSeekAPI的最佳实践及详细代码示例》：本文主要介绍如何使用Java调用DeepSeekAPI,包括获取API密钥、添加HTTP客户端依赖、创建HTTP请求、处理响应、... 目录1. 获取API密钥2. 添加HTTP客户端依赖3. 创建HTTP请求4. 处理响应5. 错误处理6.
阅读更多...
使用 sql-research-assistant进行 SQL 数据库研究的实战指南(代码实现演示)

使用 sql-research-assistant进行 SQL 数据库研究的实战指南(代码实现演示)

《使用sql-research-assistant进行SQL数据库研究的实战指南(代码实现演示)》本文介绍了sql-research-assistant工具,该工具基于LangChain框架,集... 目录技术背景介绍核心原理解析代码实现演示安装和配置项目集成LangSmith 配置（可选）启动服务应用场景
阅读更多...
Python中顺序结构和循环结构示例代码

Python中顺序结构和循环结构示例代码

《Python中顺序结构和循环结构示例代码》：本文主要介绍Python中的条件语句和循环语句,条件语句用于根据条件执行不同的代码块,循环语句用于重复执行一段代码,文章还详细说明了range函数的使... 目录一、条件语句（1）条件语句的定义（2）条件语句的语法（a）单分支 if（b）双分支 if-else（
阅读更多...
浅析如何使用Swagger生成带权限控制的API文档

浅析如何使用Swagger生成带权限控制的API文档

《浅析如何使用Swagger生成带权限控制的API文档》当涉及到权限控制时,如何生成既安全又详细的API文档就成了一个关键问题,所以这篇文章小编就来和大家好好聊聊如何用Swagger来生成带有... 目录准备工作配置 Swagger权限控制给 API 加上权限注解查看文档注意事项在咱们的开发工作里，API
阅读更多...
MySQL数据库函数之JSON_EXTRACT示例代码

MySQL数据库函数之JSON_EXTRACT示例代码

《MySQL数据库函数之JSON_EXTRACT示例代码》：本文主要介绍MySQL数据库函数之JSON_EXTRACT的相关资料,JSON_EXTRACT()函数用于从JSON文档中提取值,支持对... 目录前言基本语法路径表达式示例示例 1: 提取简单值示例 2: 提取嵌套值示例 3: 提取数组中的值注意
阅读更多...
修改若依框架Token的过期时间问题

修改若依框架Token的过期时间问题

《修改若依框架Token的过期时间问题》本文介绍了如何修改若依框架中Token的过期时间,通过修改`application.yml`文件中的配置来实现,默认单位为分钟,希望此经验对大家有所帮助,也欢迎... 目录修改若依框架Token的过期时间修改Token的过期时间关闭Token的过期时js间总结修改若依
阅读更多...
CSS3中使用flex和grid实现等高元素布局的示例代码

CSS3中使用flex和grid实现等高元素布局的示例代码

《CSS3中使用flex和grid实现等高元素布局的示例代码》：本文主要介绍了使用CSS3中的Flexbox和Grid布局实现等高元素布局的方法，通过简单的两列实现、每行放置3列以及全部代码的展示，展示了这两种布局方式的实现细节和效果，详细内容请阅读本文，希望能对你有所帮助... 过往的实现方法是使用浮动加
阅读更多...
JAVA调用Deepseek的api完成基本对话简单代码示例

JAVA调用Deepseek的api完成基本对话简单代码示例

《JAVA调用Deepseek的api完成基本对话简单代码示例》：本文主要介绍JAVA调用Deepseek的api完成基本对话的相关资料,文中详细讲解了如何获取DeepSeekAPI密钥、添加H... 获取API密钥首先，从DeepSeek平台获取API密钥，用于身份验证。添加HTTP客户端依赖使用Jav
阅读更多...
Java实现状态模式的示例代码

Java实现状态模式的示例代码

《Java实现状态模式的示例代码》状态模式是一种行为型设计模式,允许对象根据其内部状态改变行为,本文主要介绍了Java实现状态模式的示例代码,文中通过示例代码介绍的非常详细,需要的朋友们下面随着小编来... 目录一、简介1、定义2、状态模式的结构二、Java实现案例1、电灯开关状态案例2、番茄工作法状态案例
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2