2021SC@SDUSC
（六）lvi-sam源代码阅读4
lidar_odometry
mapOptmization.cpp
6D位姿点云结构定义：
/*
* A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
* 具有 6D 姿态信息的点云类型（[x,y,z,roll,pitch,yaw] 强度是时间戳）
*/
struct PointXYZIRPYT
{
PCL_ADD_POINT4D //preferred way of adding a XYZ+padding 添加XYZ+填充的首选方法
PCL_ADD_INTENSITY;
float roll; //翻滚角 绕前后轴
float pitch; //俯仰角 绕左右轴
float yaw; // 偏航角 绕竖直轴
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //貌似是与SSE加速后的对齐问题有关的// make sure our new allocators are aligned 确保我们的新分配器对齐
} EIGEN_ALIGN16;//enforce SSE padding for correct memory alignment 强制SSE填充以实现正确的内存对齐
// 注册为PCL点云格式 （PCL内有自己定义的一系列点云格式，用结构体扩展后需要注册应该）
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
(double, time, time))
typedef PointXYZIRPYT PointTypePose;//重命名
成员变量：
// gtsam
NonlinearFactorGraph gtSAMgraph; //包含非线性因子的 因子图
//Values 用于指定因子图中一组变量的值
Values initialEstimate;
Values optimizedEstimate;
//优化器
ISAM2 *isam;
Values isamCurrentEstimate;
Eigen::MatrixXd poseCovariance; //位置协方差
//发布内容见构造函数的注解
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubOdomAftMappedROS;
ros::Publisher pubKeyPoses;
ros::Publisher pubPath;
ros::Publisher pubHistoryKeyFrames;
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRecentKeyFrame;
ros::Publisher pubCloudRegisteredRaw;
ros::Publisher pubLoopConstraintEdge;
ros::Subscriber subLaserCloudInfo; //订阅当前激光帧点云信息，来自FeatureExtraction;
ros::Subscriber subGPS; //订阅GPS里程计；
ros::Subscriber subLoopInfo; //订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。
//deque 双端队列
std::deque<nav_msgs::Odometry> gpsQueue;
lvi_sam::cloud_info cloudInfo;
// 历史所有关键帧的角点集合（降采样）
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
// 历史所有关键帧的平面点集合（降采样）
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
// 历史关键帧位姿（位置）
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
// 历史关键帧位姿
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization 当前激光帧角点集合
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization 当前激光帧平面点集合
//楼下两个点云是上面的经过降采样后的数据
pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization 当前激光帧角点集合，降采样，DS：DownSize
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization 当前激光帧平面点集合，降采样
// 当前帧与局部map匹配上了的角点、平面点，加入同一集合：后面是对应点的参数
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
// 当前帧与局部map匹配上了的角点、参数、标记
std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
std::vector<PointType> coeffSelCornerVec;
std::vector<bool> laserCloudOriCornerFlag;
// 当前帧与局部map匹配上了的平面点、参数、标记
std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
std::vector<PointType> coeffSelSurfVec;
std::vector<bool> laserCloudOriSurfFlag;
//局部map的角点集合、平面点集合、 降采样过的角点集合、平面点集合
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
//局部关键帧构建的地图点云，对应kdtree,用于scan-to-map找相邻（KLANN）
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
pcl::PointCloud<PointType>::Ptr latestKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr nearHistoryKeyFrameCloud;
// 降采样
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterICP;
pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
ros::Time timeLaserInfoStamp;
double timeLaserInfoCur;
float transformTobeMapped[6];
std::mutex mtx;
bool isDegenerate = false;
cv::Mat matP;
// 当前激光帧角点数量
int laserCloudCornerLastDSNum = 0;
// 当前激光帧平面点数量
int laserCloudSurfLastDSNum = 0;
bool aLoopIsClosed = false;
//在imuPreintegration里有这个变量，当imu发生跳变是 这个变量会变
int imuPreintegrationResetId = 0;
//一个关于运动路径的消息
nav_msgs::Path globalPath;
// 当前帧位姿
Eigen::Affine3f transPointAssociateToMap;
//目测跟回环有关的？
map<int, int> loopIndexContainer; // from new to old
vector<pair<int, int>> loopIndexQueue;
vector<gtsam::Pose3> loopPoseQueue;
vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;
构造函数：
mapOptimization()
{
ISAM2Params parameters; //ISAM2优化器的参数
//设置上界和下界（见代码阅读3）
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
//初始化优化器
isam = new ISAM2(parameters);
// 发布历史关键帧里程计（关键帧里是一个点云的信息）
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/trajectory", 1);
// 发布局部关键帧地图的特征点云
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/map_global", 1);
// 发布激光里程计，rviz中表现为坐标轴
pubOdomAftMappedROS = nh.advertise<nav_msgs::Odometry> (PROJECT_NAME + "/lidar/mapping/odometry", 1);
// 发布激光里程计，它与上面的激光里程计基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制
pubPath = nh.advertise<nav_msgs::Path> (PROJECT_NAME + "/lidar/mapping/path", 1);
//订阅当前激光帧点云信息，来自featureExtraction
subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
//订阅GPS里程计
subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 50, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
//订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上
subLoopInfo = nh.subscribe<std_msgs::Float64MultiArray>(PROJECT_NAME + "/vins/loop/match_frame", 5, &mapOptimization::loopHandler, this, ros::TransportHints().tcpNoDelay());
//发布闭环匹配关键帧局部地图
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/loop_closure_history_cloud", 1);
//发布当前关键帧经过闭环优化后的位姿变换之后的特征点云
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/loop_closure_corrected_cloud", 1);
//发布闭环边，rviz中表现为闭环帧之间的连线
pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>(PROJECT_NAME + "/lidar/mapping/loop_closure_constraints", 1);
//发布局部地图的降采样平面点集合
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/map_local", 1);
//发布历史帧（累加的）的角点、平面点降采样集合
pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/cloud_registered", 1);
//发布当前帧原始点云配准之后的点云
pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/cloud_registered_raw", 1);
//设置每个体素的大小，分别为 长宽高（楼下是正方体，在utility中定义）
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);// 0.2
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); //0.2
downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);// 0.2
//用于扫描到贴图优化的周围关键姿势
downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization 1.0
//初始化变量
allocateMemory();
}
void allocateMemory()
{
// 对定义的一系列关键帧变量进行初始化
cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
laserCloudOri.reset(new pcl::PointCloud<PointType>());
coeffSel.reset(new pcl::PointCloud<PointType>());
laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
latestKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
nearHistoryKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
for (int i = 0; i < 6; ++i){
transformTobeMapped[i] = 0;
}
matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
}
前置知识学习
PCL 点云库
PCL_ADD_POINT4D
表示欧几里得 xyz 坐标和强度值的点结构
PCL点云数据点类型介绍
https://zhuanlan.zhihu.com/p/102426294
PointCloud
表示 PCL 中用于存储 3D 点集合的基类
VoxelGrid
https://blog.csdn.net/small_munich/article/details/108348164
pcl库中的VoxelGrid对点云进行体素化，主要就是创建一个三维体素栅格(就是每个比较小的立方体组成的体素栅格)。在每个体素(三维立方体)里面，求取该立方体内的所有点云重心点来代表这个立方体的表示，以此达到下采样（对一个样值序列间隔几个样值取样一次，得到新序列）的目的。
体素：一个小正方体
https://www.zhihu.com/question/426530234
// 设置每个体素的大小, leaf_size没别为lx ly lz的长 宽 高
inline void setLeafSize (const Eigen::Vector3f &leaf_size);
// 不同的接口输入方式
inline void setLeafSize (float lx, float ly, float lz);
// 获取每个体素的大小
inline Eigen::Vector3f getLeafSize ();
// 设置下采样的维度，默认false, 只对XYZ进行下采样；true为对所有下采样
inline void setDownsampleAllData (bool downsample);
// 获取下采样的方式是否为XYZ还是全部下采样
inline bool getDownsampleAllData ();
降采样
微信文章：降采样
关于IMU的一个例子：
由于IMU的ODR通常比实际软件控制所需要的数据要大，所以通常需要对IMU输出的数据进行降采样。
对于连续时间信号，可以对其在任意时间点进行采样。对于均匀采样的离散时间信号，由于数据只出现在一系列的离散时间点上，因此对它进行降采样的时候，就会出现一些限制。
如果降采样的比例是整数时，则每隔几个数据保留一个数据就可以完成。比如将数据降采样两倍，则每两个相邻数据保留一个数据就可以完成。
如果降采样的比例不是整数时，比如前面同学所碰到的，800比上500，即8:5，此时就需要每8个数据，只保留5个数据，因此就无法做到降采样之后的数据在时间上均匀分布了。
kdtree
https://blog.csdn.net/silangquan/article/details/41483689
分割
在一个n维空间里，对空间里的点进行大小比较，可以通过经过该点的一个超平面（分割超平面），将点集合分割成大于该点和小于该点的两个集合；递归上述过程…知道找到目标点。
在BSTree中，节点分割的是一维数轴，那么在二维中，就应当是分割平面了，就像这样：
黄色的点作为根节点，上面的点归左子树，下面的点归右子树，接下来再不断地划分，最后得到一棵树BSPTree（binary space partitioning tree）. 分割的那条线叫做分割超平面（splitting hyperplane），在一维中是一个点，二维中是线，三维的是面。
KDTree就是超平面都垂直于轴的BSPTree。同样的数据集，用KDTree划分之后就是这样：
Roll, Pitch, Yaw
-
基本概念
-
- Roll：翻滚角
- Pitch：俯仰角
- Yaw：偏航角
-
区分
-
- 绕着前后轴（front-to-back axis）旋转角度为Roll
- 绕着左右轴（side-to-side axis）旋转角度为Pitch
- 绕着竖直轴（vertical axis）旋转角度为Yaw
-
区分方法
-
- 首先应先给定物体一个姿态，就是确定其前后、左右、上下，这样就能对应Roll、Pitch、Yaw。结合建立在物体上的坐标系，就可以搞定了。
- 方向的话，遵循右手定则
sensor/msgs
- sensor_msgs/PointCloud.msg
# This message holds a collection of 3d points, plus optional additional
# information about each point.
// 此消息包含 3d 点的集合，以及有关每个点的可选附加信息。
# Time of sensor data acquisition, coordinate frame ID. 传感器数据采集时间，坐标系ID。
Header header
# Array of 3d points. Each Point32 should be interpreted as a 3d point
# in the frame given in the header.
// 3d 点数组。 每个 Point32 应被解释为标题中给出的帧中的 3d 点。
geometry_msgs/Point32[] points
# Each channel should have the same number of elements as points array,
# and the data in each channel should correspond 1:1 with each point.
# Channel names in common practice are listed in ChannelFloat32.msg.
//每个通道的元素数量应该与点数组相同，每个通道中的数据应该与每个点1:1对应。
//ChannelFloat32.msg 中列出了常用的频道名称。
ChannelFloat32[] channels
Compact Message Definition:
std_msgs/Header header
geometry_msgs/Point32[] points
sensor_msgs/ChannelFloat32[] channels
- sensor_msgs/ChannelFloat32.msg
# This message is used by the PointCloud message to hold optional data
# associated with each point in the cloud. The length of the values
# array should be the same as the length of the points array in the
# PointCloud, and each value should be associated with the corresponding
# point.
// PointCloud 消息使用此消息来保存与云中每个点相关联的可选数据。 values数组的长度应该与PointCloud中points数组的长度相同，并且每个值都应该与对应的点相关联。
# Channel names in existing practice include: 现有实践中的频道名称包括：
# "u", "v" - row and column (respectively) in the left stereo image.
# This is opposite to usual conventions but remains for
# historical reasons. The newer PointCloud2 message has no
# such problem.
//"u", "v" - 左侧立体图像中的行和列（分别）。这与通常的约定相反，但由于历史原因仍然存在。 较新的 PointCloud2 消息没有这样的问题。
# "rgb" - For point clouds produced by color stereo cameras. uint8
# (R,G,B) values packed into the least significant 24 bits,
# in order.
// "rgb" - 用于彩色立体相机产生的点云。 uint8 (R,G,B) 值按顺序打包到最低有效 24 位。
# "intensity" - laser or pixel intensity.
// 激光或者像素的强度
# "distance"
# The channel name should give semantics of the channel (e.g.
# "intensity" instead of "value").
// 通道名称应该给出通道的语义（例如“强度”而不是“值”）
string name
# The values array should be 1-1 with the elements of the associated
# PointCloud.
// values 数组应该是 1-1 与关联的 PointCloud 的元素。
float32[] values
Compact Message Definition:
string name
float32[] values
- sensor_msgs/PointCloud2.msg
# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.
// 此消息包含 N 维点的集合，其中可能包含法线、强度等附加信息。点数据存储为二进制 blob，其布局由“字段”数组的内容描述。
# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.
// 点云数据可以按 2d（类图像）或 1d（无序）组织。组织为二维图像的点云可以由相机深度传感器（例如立体或飞行时间）产生。
# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header
# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
//点云的二维结构。如果云是无序的，则高度为 1，宽度为点云的长度。
uint32 height
uint32 width
# Describes the channels and their layout in the binary data blob.
// 描述二进制数据 blob 中的通道及其布局。
PointField[] fields
bool is_bigendian # Is this data bigendian? 这个数据是双端的嘛
uint32 point_step # Length of a point in bytes 点的长度，以字节为单位
uint32 row_step # Length of a row in bytes 行的长度，以字节为单位
uint8[] data # Actual point data, size is (row_step*height) //实际点的数据，大小为（row_step*height）
bool is_dense # True if there are no invalid points 如果没有无效点则为真
Compact Message Definition:
std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense
一个小例子：
https://blog.csdn.net/weixin_40826634/article/details/108767704
header:
seq: 2116
stamp:
secs: 1586919439
nsecs: 448866652
frame_id: "LidarSensor1"
height: 1
width: 3
fields:
-
name: "x"
offset: 0
datatype: 7
count: 1
-
name: "y"
offset: 4
datatype: 7
count: 1
-
name: "z"
offset: 8
datatype: 7
count: 1
is_bigendian: False
point_step: 12
row_step: 36
data: [143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64, 143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64, 143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64]
is_dense: True
- sensor_msgs/PointField.msg
//此消息包含PointCloud2 消息格式中的一个点条目的描述。
uint8 INT8 = 1
uint8 UINT8 = 2
uint8 INT16 = 3
uint8 UINT16 = 4
uint8 INT32 = 5
uint8 UINT32 = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8
string name # Name of field
uint32 offset # Offset from start of point struct
uint8 datatype # Datatype enumeration, see above
uint32 count # How many elements in the field
- 在网上没有查到PointCloud和PointCloud2的区别，暂时认为PointCloud2是1的迭代版本？（感觉表示的内容都差不多）
geometry/msgs
- geometry_msgs/Point32.msg
# This contains the position of a point in free space(with 32 bits of precision).这包含一个点在自由空间中的位置（精度为 32 位）。
# It is recommeded to use Point wherever possible instead of Point32. 建议尽可能使用Point而不是Point32。
#
# This recommendation is to promote interoperability. 这个建议是为了促进互操作性
#
# This message is designed to take up less space when sending
# lots of points at once, as in the case of a PointCloud. 此消息旨在在一次发送大量点时占用更少的空间，例如在 PointCloud 的情况下。
Compact Message Definition:
float32 x
float32 y
float32 z
std/msgs
- std_msgs/Float64MultiArray.msg
Compact Message Definition:
MultiArrayLayout layout # specification of data layout 数据布局规范
float64[] data # array of data
- std_msgs/MultiArrayLayout.msg
//multiarray 声明了一个特定数据类型的通用多维数组。 尺寸从最外到最内排序。
Compact Message Definition:
std_msgs/MultiArrayDimension[] dim //维度属性数组
uint32 data_offset //在数据前面填充元素
# 访问器应该始终按照维度步幅和指定的最外层维度首先编写。
#
# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]
#
# 标准的 3 通道 640x480 图像，带有交错颜色通道
# 将被指定为：
#
# dim[0].label = "height"
#dim[0].size = 480
# dim[0].stride = 3*640*480 = 921600 (注意 dim[0] stride 只是图像的大小)
# dim[1].label = "width"
#dim[1].size = 640
#dim[1].stride = 3*640 = 1920
#dim[2].label = "channel"
#dim[2].size = 3
#dim[2].stride = 3
#
# multiarray(i,j,k) 指的是第 i 行、第 j 列和第 k 个通道。
- std_msgs/MultiArrayDimension.msg
string label
uint32 size
uint32 stride
visualization/msgs
- visualization_msgs/MarkerArray.msg
visualization_msgs/Marker[] markers
- visualization_msgs/Marker.msg
uint8 ARROW=0
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8
uint8 TEXT_VIEW_FACING=9
uint8 MESH_RESOURCE=10
uint8 TRIANGLE_LIST=11
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
Header header # header for time/frame information
string ns # Namespace to place this object in... used in conjunction with id to create a unique name for the object 放置这个对象的命名空间... 与 id 结合使用来为对象创建一个唯一的名称
int32 id # object ID useful in conjunction with the namespace for manipulating and deleting the object later 对象 ID 与命名空间结合使用，用于稍后操作和删除对象
int32 type # Type of object
int32 action # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects 0 添加/修改一个对象，1（不推荐使用），2 删除一个对象，3 删除所有对象
geometry_msgs/Pose pose # Pose of the object
geometry_msgs/Vector3 scale # Scale of the object 1,1,1 means default (usually 1 meter square) 物体的比例尺 1,1,1 表示默认（通常为 1 平方米）
std_msgs/ColorRGBA color # Color [0.0-1.0]
duration lifetime # How long the object should last before being automatically deleted. 0 means forever 对象在被自动删除之前应该持续多长时间。 0 表示永远
bool frame_locked # If this marker should be frame-locked, i.e. retransformed into its frame every timestep 如果这个标记应该是帧锁定的，即每个时间步都重新转换成它的帧
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...) 仅当指定的类型对它们有一些用处时才使用（例如。POINTS、LINE_STRIP、...）
geometry_msgs/Point[] points
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
#number of colors must either be 0 or equal to the number of points
#NOTE: alpha is not yet used
std_msgs/ColorRGBA[] colors
# NOTE: only used for text markers
string text
# NOTE: only used for MESH_RESOURCE markers
string mesh_resource
bool mesh_use_embedded_materials
Compact Message Definition:
std_msgs/Header header
string ns
int32 id
int32 type
int32 action
geometry_msgs/Pose pose
geometry_msgs/Vector3 scale
std_msgs/ColorRGBA color
duration lifetime
bool frame_locked
geometry_msgs/Point[] points
std_msgs/ColorRGBA[] colors
string text
string mesh_resource
bool mesh_use_embedded_materials
- std_msgs/ColorRGBA.msg
float32 r
float32 g
float32 b
float32 a
lvi_sam::cloud_info(作者自定义消息类型)
同组的罗源同学已经分析了这个消息类型，所以直接学习他的博客，并进行了总结：
地址：
用来传递已经矫正了运动畸变的点云信息。
# Cloud Info
Header header
int32[] startRingIndex
int32[] endRingIndex
int32[] pointColInd # point column index in range image
float32[] pointRange # point range
int64 imuAvailable
int64 odomAvailable
# Attitude for lidar odometry initialization
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit
# Odometry
float32 odomX
float32 odomY
float32 odomZ
float32 odomRoll
float32 odomPitch
float32 odomYaw
# Odometry reset ID
int64 odomResetId
# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed
sensor_msgs/PointCloud2 cloud_corner # extracted corner feature
sensor_msgs/PointCloud2 cloud_surface # extracted surface feature
startRingIndex and endRingIndex。按照我个人的理解，这是指检测线（环）其实位置和结束的位置，至于这个检测环怎么计算，在cloudExtraction()函数中提到，起始点是每个扫描的第5个激光点在一维数组中的索引，结束点为倒数第5个激光点在一维数组中的索引。pointColInd and pointRange。这里记录的是点云的信息，pointColInd记录点的索引，而pointRange记录点的范围。imuAvailable and odomAvailable。记录imu和里程计是否可以使用。imu。接下来的imu信息是用来作为lidar数据帧的初始姿态角。Odometry。里程计信息，用于当前激光帧起始时刻的odom，初始化lidar位姿，后面用于mapOptmization。Point cloud messages。还有后面的点云数据，在上面的已经介绍过这个消息类型。分别用来发布矫正的点云，提取中心特征（imageProjection未使用），还有表面特征（imageProjection未使用）。
1697
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
