3-LVI-SAM源码分析_utility

最新推荐文章于 2024-06-08 10:59:03 发布

loyer_kong

最新推荐文章于 2024-06-08 10:59:03 发布

阅读量1k

点赞数 3

分类专栏： LVI-SAM分析文章标签：自动驾驶 c++

本文链接：https://blog.csdn.net/loyer_kong/article/details/120734066

版权

LVI-SAM分析专栏收录该内容

15 篇文章 23 订阅

订阅专栏

2021SC@SDUSC

LVI-SAM源码分析-lidar_odometry/utility.h

这个头文件虽然不是LVI SAM中的核心部分，但却在每个地方都有引入。对于还是初学者的我们来说，还是有必要搞懂这个头文件里面包含了什么内容，因此，对这个头文件进行了一定的分析。里面还是包含了我们很多没有见过的知识点。

一、引入头文件的部分

1. ROS

这部分包含了<ros/ros.h>以及一些消息的定义。其中，<ros/ros.h>里面包含了一些ros程序常用的头文件，如下：

#include "ros/time.h"
#include "ros/rate.h"
#include "ros/console.h"
#include "ros/assert.h"
#include "ros/common.h"
#include "ros/types.h"
#include "ros/node_handle.h"
#include "ros/publisher.h"
#include "ros/single_subscriber_publisher.h"
#include "ros/service_server.h"
#include "ros/subscriber.h"
#include "ros/service.h"
#include "ros/init.h"
#include "ros/master.h"
#include "ros/this_node.h"
#include "ros/param.h"
#include "ros/topic.h"
#include "ros/names.h"

3-1

这些程序主要用于ros的机制的实现，如ros节点的创建，ros的通信等。除了这一个包之外，还utility.h还引入了自己定义的消息，这些消息只有经过了catkin_make编译之后才会有相应的头文件。

#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

2. OpenCV

还引入了opencv的库进行对图像的处理。OpenCV（开源计算机视觉库）是一个主要针对实时计算机视觉的编程函数库。最初由英特尔开发，后来由Willow Garage和Itseez（后来被英特尔收购）支持。该库是跨平台的，在开源的Apache 2许可证下免费使用。

#include <opencv/cv.h>

3. PCL

点云库（PCL）是一个独立的、大规模的、开放的项目，用于2D/3D图像和点云处理。PCL是根据BSD许可条款发布的，因此可免费用于商业和研究用途。在头文件中，引入了如下pcl的内容：

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h> 
#include <pcl_conversions/pcl_conversions.h>

point_cloud

里面包含着一些头文件，和一个类的定义，PointCloud。在PCL中，PointCloud是用于存储3D点集合的基类。

point_types

定义所有 PCL 实现的 PointT 点类型结构

search/impl/search

一个通用搜索类，所有的搜索包装器都必须继承于它。每种搜索方法必须实现 2 种不同类型的搜索：（1）NearKSearch - 搜索 K-最近邻；（2）radiusSearch - 在给定半径的球体中搜索所有最近的邻居。每种搜索方法的输入可以通过 3 种不同的方式给出：（1）作为查询点；（2）作为（云，索引）对；（3）作为索引。对于后一个选项，假设用户首先通过setInputCloud () 方法指定了输入。

range_image/range_image

RangeImage派生自 pcl/PointCloud，并提供侧重于从特定视点捕获 3D 场景的情况的功能。

kdtree/kdtree_flann

KdTreeFLANN是一种使用 kD 树结构的通用 3D 空间定位器。

3-2

common/common

定义所有方法通用的标准 C 方法和 C++ 类。

common/transforms

网上没有相应的说明文档，但是按照我对transforms，应该是对坐标的转换的通用的标准C方法和C++类。

registration/icp

提供了迭代最近点算法的基本实现。IterativeClosestPoint提供了一个Iterative Closest Point算法的基础实现。变换是基于奇异值分解（SVD）来估计的。

registration/gicp

GeneralizedIterativeClosestPoint是一个ICP变体，它实现了Alex Segal等人所描述的广义迭代最接近点算法。该方法是基于使用各向异性的成本函数来优化最接近的点分配后的排列。原始代码使用GSL和ANN，而在我们的代码中，我们使用特征映射的BFGS和FLANN。

io/pcd_io

点云数据（PCD）文件格式阅读器。

filters/filter

过滤器代表基础过滤器类。所有的过滤器都必须继承自这个接口。

filters/voxel_grid

VoxelGrid在给定的PointCloud上组装一个本地3D网格，并对数据进行降样和过滤。

filters/crop_box

CropBox是一个过滤器，允许用户过滤给定框内的所有数据。

pcl_conversions/pcl_conversions

使用field_map将PCLPointCloud2的二进制数据blob转换为pcl::PointCloud对象。

4. TF

tf是一个让用户在一段时间内跟踪多个坐标框架的软件包。tf在一个时间缓冲的树状结构中保持坐标框架之间的关系，并让用户在任何需要的时间点在任何两个坐标框架之间转换点、向量等。

简单地说，就是可以记录下不同坐标系之间的相对位置，以及绝队位置。并且，能够实时更新维护这些转换关系，只要知道一个坐标系上的点，就可以通过TF得到在其他坐标系下，这个点的位置。这个包是一个很常用的包，在智能车比赛中都有用到。

二、定义的类与方法

1. ParamServer

首先，这个类一开始定义了很多变量，有和GPS设置相关的，也有和PCD保存相关，还有关于Velodyne传感器配置相关的，还有IMU信息等等。在这个类初始化的时候，会从参数服务器中，获取相关的参数的信息，然后赋值给一开始定义的变量。如果参数服务器没有信息，会赋值默认的信息，其中默认的信息已经给出。如nh.param<std::string>("/PROJECT_NAME", PROJECT_NAME, "sam");如果参数服务器中，有/PROJECT_NAME的信息，就会把相应的信息赋值给变量PROJECT_NAME，否则就会把字符串sam赋值给变量PROJECT_NAME。并且，这个类可以在初始化节点之后，可以获取节点的句柄，去发布和订阅话题。
以下是构造函数

ParamServer()
{
    nh.param<std::string>("/PROJECT_NAME", PROJECT_NAME, "sam");

    nh.param<std::string>("/robot_id", robot_id, "roboat");

    nh.param<std::string>(PROJECT_NAME + "/pointCloudTopic", pointCloudTopic, "points_raw");
    nh.param<std::string>(PROJECT_NAME + "/imuTopic", imuTopic, "imu_correct");
    nh.param<std::string>(PROJECT_NAME + "/odomTopic", odomTopic, "odometry/imu");
    nh.param<std::string>(PROJECT_NAME + "/gpsTopic", gpsTopic, "odometry/gps");

    nh.param<bool>(PROJECT_NAME + "/useImuHeadingInitialization", useImuHeadingInitialization, false);
    nh.param<bool>(PROJECT_NAME + "/useGpsElevation", useGpsElevation, false);
    nh.param<float>(PROJECT_NAME + "/gpsCovThreshold", gpsCovThreshold, 2.0);
    nh.param<float>(PROJECT_NAME + "/poseCovThreshold", poseCovThreshold, 25.0);

    nh.param<bool>(PROJECT_NAME + "/savePCD", savePCD, false);
    nh.param<std::string>(PROJECT_NAME + "/savePCDDirectory", savePCDDirectory, "/tmp/loam/");

    nh.param<int>(PROJECT_NAME + "/N_SCAN", N_SCAN, 16);
    nh.param<int>(PROJECT_NAME + "/Horizon_SCAN", Horizon_SCAN, 1800);
    nh.param<std::string>(PROJECT_NAME + "/timeField", timeField, "time");
    nh.param<int>(PROJECT_NAME + "/downsampleRate", downsampleRate, 1);

    nh.param<float>(PROJECT_NAME + "/imuAccNoise", imuAccNoise, 0.01);
    nh.param<float>(PROJECT_NAME + "/imuGyrNoise", imuGyrNoise, 0.001);
    nh.param<float>(PROJECT_NAME + "/imuAccBiasN", imuAccBiasN, 0.0002);
    nh.param<float>(PROJECT_NAME + "/imuGyrBiasN", imuGyrBiasN, 0.00003);
    nh.param<float>(PROJECT_NAME + "/imuGravity", imuGravity, 9.80511);
    nh.param<vector<double>>(PROJECT_NAME+ "/extrinsicRot", extRotV, vector<double>());
    nh.param<vector<double>>(PROJECT_NAME+ "/extrinsicRPY", extRPYV, vector<double>());
    nh.param<vector<double>>(PROJECT_NAME+ "/extrinsicTrans", extTransV, vector<double>());
    extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
    extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
    extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
    extQRPY = Eigen::Quaterniond(extRPY);

    nh.param<float>(PROJECT_NAME + "/edgeThreshold", edgeThreshold, 0.1);
    nh.param<float>(PROJECT_NAME + "/surfThreshold", surfThreshold, 0.1);
    nh.param<int>(PROJECT_NAME + "/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
    nh.param<int>(PROJECT_NAME + "/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);

    nh.param<float>(PROJECT_NAME + "/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
    nh.param<float>(PROJECT_NAME + "/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
    nh.param<float>(PROJECT_NAME + "/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);

    nh.param<float>(PROJECT_NAME + "/z_tollerance", z_tollerance, FLT_MAX);
    nh.param<float>(PROJECT_NAME + "/rotation_tollerance", rotation_tollerance, FLT_MAX);

    nh.param<int>(PROJECT_NAME + "/numberOfCores", numberOfCores, 2);
    nh.param<double>(PROJECT_NAME + "/mappingProcessInterval", mappingProcessInterval, 0.15);

    nh.param<float>(PROJECT_NAME + "/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
    nh.param<float>(PROJECT_NAME + "/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
    nh.param<float>(PROJECT_NAME + "/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
    nh.param<float>(PROJECT_NAME + "/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);

    nh.param<bool>(PROJECT_NAME + "/loopClosureEnableFlag", loopClosureEnableFlag, false);
    nh.param<int>(PROJECT_NAME + "/surroundingKeyframeSize", surroundingKeyframeSize, 50);
    nh.param<float>(PROJECT_NAME + "/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
    nh.param<float>(PROJECT_NAME + "/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
    nh.param<int>(PROJECT_NAME + "/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
    nh.param<float>(PROJECT_NAME + "/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);

    nh.param<float>(PROJECT_NAME + "/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
    nh.param<float>(PROJECT_NAME + "/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
    nh.param<float>(PROJECT_NAME + "/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);

    usleep(100);
}

2. imuConverter

这个函数也是ParamServer类里面定义的一个函数- sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)。其中把从原始的imu信息中，计算旋转加速度、旋转陀螺仪、转动、滚动、俯仰、偏转等信息。

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
    sensor_msgs::Imu imu_out = imu_in;
    // rotate acceleration
    Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
    acc = extRot * acc;
    imu_out.linear_acceleration.x = acc.x();
    imu_out.linear_acceleration.y = acc.y();
    imu_out.linear_acceleration.z = acc.z();
    // rotate gyroscope
    Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
    gyr = extRot * gyr;
    imu_out.angular_velocity.x = gyr.x();
    imu_out.angular_velocity.y = gyr.y();
    imu_out.angular_velocity.z = gyr.z();
    // rotate roll pitch yaw
    Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
    Eigen::Quaterniond q_final = q_from * extQRPY;
    imu_out.orientation.x = q_final.x();
    imu_out.orientation.y = q_final.y();
    imu_out.orientation.z = q_final.z();
    imu_out.orientation.w = q_final.w();

    if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
    {
        ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
        ros::shutdown();
    }

    return imu_out;
}

3. 工具方法

后面就定义了一些工具性质的静态方法，如发布云点图，获取ros中的时间信息，把从imu传感器中的数据转变成ros中的数据，计算在云点图中的两点之间的距离，以及到零点的距离。

template<typename T>
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, T thisCloud, ros::Time thisStamp, std::string thisFrame)
{
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);
    tempCloud.header.stamp = thisStamp;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub->getNumSubscribers() != 0)
        thisPub->publish(tempCloud);
    return tempCloud;
}

template<typename T>
double ROS_TIME(T msg)
{
    return msg->header.stamp.toSec();
}


template<typename T>
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
    *angular_x = thisImuMsg->angular_velocity.x;
    *angular_y = thisImuMsg->angular_velocity.y;
    *angular_z = thisImuMsg->angular_velocity.z;
}


template<typename T>
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
{
    *acc_x = thisImuMsg->linear_acceleration.x;
    *acc_y = thisImuMsg->linear_acceleration.y;
    *acc_z = thisImuMsg->linear_acceleration.z;
}


template<typename T>
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
    double imuRoll, imuPitch, imuYaw;
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);

    *rosRoll = imuRoll;
    *rosPitch = imuPitch;
    *rosYaw = imuYaw;
}


float pointDistance(PointType p)
{
    return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}


float pointDistance(PointType p1, PointType p2)
{
    return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
}