LeGO-LOAM源码解析3： featureAssociation（一）
featureAssociation.cpp解析一
- 一、概述
- 二、main函数
- 三、FeatureAssociation类的私有对象
- 四、FeatureAssociation类的构造函数
-
- 1. ROS消息订阅发布的具体定义
- 2. 变量的初始化函数initializationValue
- 3. 回调函数
-
- (1) 分割点云接受laserCloudHandler
- (2) 界外点云接受outlierCloudHandler
- (3) 自定义消息接受outlierCloudHandler
- (4) IMU数据接受imuHandler
-
-
- a) IMU数据解析
- b) IMU数据转换AccumulateIMUShiftAndRotation
-
loam源码地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.
论文学习：【论文阅读】LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain.
LeGO-LOAM源码解析汇总：
LeGO-LOAM源码解析1 ： 算法整体框架和utility.h.
LeGO-LOAM源码解析2： imageProjection.
一、概述
接着imageProjection之后，我们进入到了featureAssociation这一部分主要是对分割出的点云进行数据处理、特征提取、特征匹配、里程计输出等工作，也是作为一个独立的单元存在，是框架核心算法的第一部分。
imageProjection
featureAssociation
mapOptmization
transformFusion
二、main函数
与ImageProjection中一样，主函数相对来说比较简单，但是执行逻辑稍有不同，没有将相关的处理函数在回调函数中调用，而是以一定频率在循环中运行。注意是ros::spinOnce();而不是ros::spin()；
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");
FeatureAssociation FA;
ros::Rate rate(200);
while (ros::ok())
{
ros::spinOnce();
FA.runFeatureAssociation();
rate.sleep();
}
ros::spin();
return 0;
}
三、FeatureAssociation类的私有对象
同样的，在我们看该类的构造函数之前，我们先查看一下FeatureAssociation的私有对象，即定义的一些变量。在这里你不需要记住每一个变量的意义，但是提前了解有助于接下来代码的理解。
定义ROS的订阅者：
private:
ros::NodeHandle nh;
ros::Subscriber subLaserCloud;
ros::Subscriber subLaserCloudInfo;
ros::Subscriber subOutlierCloud;
ros::Subscriber subImu;
定义有关特征点云的ROS发布者：
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
定义点云储存变量：
//分割后的点云
pcl::PointCloud<PointType>::Ptr segmentedCloud;
//异常界外点云
pcl::PointCloud<PointType>::Ptr outlierCloud;
//曲率大的点云
pcl::PointCloud<PointType>::Ptr cornerPointsSharp;
//曲率较大的点云
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp;
//曲率小点云
pcl::PointCloud<PointType>::Ptr surfPointsFlat;
//曲率较小的点云
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScanDS;
//体素滤波
pcl::VoxelGrid<PointType> downSizeFilter;
具体时间：
double timeScanCur;
double timeNewSegmentedCloud;
double timeNewSegmentedCloudInfo;
double timeNewOutlierCloud;
接受点云的标志：
bool newSegmentedCloud;
bool newSegmentedCloudInfo;
bool newOutlierCloud;
自定义消息及时间戳，系统初始化标志：
cloud_msgs::cloud_info segInfo;
std_msgs::Header cloudHeader;
int systemInitCount;
bool systemInited;
点云曲率及标签：
std::vector<smoothness_t> cloudSmoothness;
float *cloudCurvature;
int *cloudNeighborPicked;
int *cloudLabel;
IMU的相关定义（看名字就基本了解变量用途了～）：
int imuPointerFront;
int imuPointerLast;
int imuPointerLastIteration;
float imuRollStart, imuPitchStart, imuYawStart;
float cosImuRollStart, cosImuPitchStart, cosImuYawStart, sinImuRollStart, sinImuPitchStart, sinImuYawStart;
float imuRollCur, imuPitchCur, imuYawCur;
float imuVeloXStart, imuVeloYStart, imuVeloZStart;
float imuShiftXStart, imuShiftYStart, imuShiftZStart;
float imuVeloXCur, imuVeloYCur, imuVeloZCur;
float imuShiftXCur, imuShiftYCur, imuShiftZCur;
float imuShiftFromStartXCur, imuShiftFromStartYCur, imuShiftFromStartZCur;
float imuVeloFromStartXCur, imuVeloFromStartYCur, imuVeloFromStartZCur;
float imuAngularRotationXCur, imuAngularRotationYCur, imuAngularRotationZCur;
float imuAngularRotationXLast, imuAngularRotationYLast, imuAngularRotationZLast;
float imuAngularFromStartX, imuAngularFromStartY, imuAngularFromStartZ;
double imuTime[imuQueLength];
float imuRoll[imuQueLength];
float imuPitch[imuQueLength];
float imuYaw[imuQueLength];
float imuAccX[imuQueLength];
float imuAccY[imuQueLength];
float imuAccZ[imuQueLength];
float imuVeloX[imuQueLength];
float imuVeloY[imuQueLength];
float imuVeloZ[imuQueLength];
float imuShiftX[imuQueLength];
float imuShiftY[imuQueLength];
float imuShiftZ[imuQueLength];
float imuAngularVeloX[imuQueLength];
float imuAngularVeloY[imuQueLength];
float imuAngularVeloZ[imuQueLength];
float imuAngularRotationX[imuQueLength];
float imuAngularRotationY[imuQueLength];
float imuAngularRotationZ[imuQueLength];
与里程计相关的发布器：
ros::Publisher pubLaserCloudCornerLast;
ros::Publisher pubLaserCloudSurfLast;
ros::Publisher pubLaserOdometry;
ros::Publisher pubOutlierCloudLast;
int skipFrameNum;
bool systemInitedLM;
int laserCloudCornerLastNum;
int laserCloudSurfLastNum;
int *pointSelCornerInd;
float *pointSearchCornerInd1;
float *pointSearchCornerInd2;
int *pointSelSurfInd;
float *pointSearchSurfInd1;
float *pointSearchSurfInd2;
float *pointSearchSurfInd3;
float transformCur[6];
float transformSum[6];
float imuRollLast, imuPitchLast, imuYawLast;
float imuShiftFromStartX, imuShiftFromStartY, imuShiftFromStartZ;
float imuVeloFromStartX, imuVeloFromStartY, imuVeloFromStartZ;
保存上帧点云的定义：
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast;
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast;
里程计和tf发布：
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
PointType pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff;
nav_msgs::Odometry laserOdometry;
tf::TransformBroadcaster tfBroadcaster;
tf::StampedTransform laserOdometryTrans;
有关退化的定义：
bool isDegenerate;
cv::Mat matP;
int frameCount;
四、FeatureAssociation类的构造函数
1. ROS消息订阅发布的具体定义
ROS消息的订阅器：
FeatureAssociation():
nh("~")
{
//已分割的点云
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
//自定义消息
subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
//界外点云
subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
//IMU信息
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
ROS消息的发布器：
//四种曲率的点云
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);
//上帧的点云信息
pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
//里程计信息
pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
initializationValue();
}
2. 变量的初始化函数initializationValue
存储点云相关，例如曲率、标签、索引等初始化:
void initializationValue()
{
cloudCurvature = new float[N_SCAN*Horizon_SCAN];
cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
cloudLabel = new int[N_SCAN*Horizon_SCAN];
pointSelCornerInd = new int[N_SCAN*Horizon_SCAN];
pointSearchCornerInd1 = new float[N_SCAN*Horizon_SCAN];
pointSearchCornerInd2 = new float[N_SCAN*Horizon_SCAN];
pointSelSurfInd = new int[N_SCAN*Horizon_SCAN];
pointSearchSurfInd1 = new float[N_SCAN*Horizon_SCAN];
pointSearchSurfInd2 = new float[N_SCAN*Horizon_SCAN];
pointSearchSurfInd3 = new float[N_SCAN*Horizon_SCAN];
cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
体素滤波的立方体设置：
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
点云初始化：
segmentedCloud.reset(new pcl::PointCloud<PointType>());
outlierCloud.reset(new pcl::PointCloud<PointType>());
cornerPointsSharp.reset(new pcl::PointCloud<PointType>());
cornerPointsLessSharp.reset(new pcl::PointCloud<PointType>());
surfPointsFlat.reset(new pcl::PointCloud<PointType>());
surfPointsLessFlat.reset(new pcl::PointCloud<PointType>());
surfPointsLessFlatScan.reset(new pcl::PointCloud<PointType>());
surfPointsLessFlatScanDS.reset(new pcl::PointCloud<PointType>());
每帧点云时间、标志初始化：
timeScanCur = 0;
timeNewSegmentedCloud = 0;
timeNewSegmentedCloudInfo = 0;
timeNewOutlierCloud = 0;
newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
系统初始化标志初始化：
systemInitCount = 0;
systemInited = false;
开始时刻和当前的IMU数据初始化：
imuPointerFront = 0;
imuPointerLast = -1;
imuPointerLastIteration = 0;
imuRollStart = 0; imuPitchStart = 0; imuYawStart = 0;
cosImuRollStart = 0; cosImuPitchStart = 0; cosImuYawStart = 0;
sinImuRollStart = 0; sinImuPitchStart = 0; sinImuYawStart = 0;
imuRollCur = 0; imuPitchCur = 0; imuYawCur = 0;
imuVeloXStart = 0; imuVeloYStart = 0; imuVeloZStart = 0;
imuShiftXStart = 0; imuShiftYStart = 0; imuShiftZStart = 0;
imuVeloXCur = 0; imuVeloYCur = 0; imuVeloZCur = 0;
imuShiftXCur = 0; imuShiftYCur = 0; imuShiftZCur = 0;
IMU的变换初始化：
imuShiftFromStartXCur = 0; imuShiftFromStartYCur = 0; imuShiftFromStartZCur = 0;
imuVeloFromStartXCur = 0; imuVeloFromStartYCur = 0; imuVeloFromStartZCur = 0;
imuAngularRotationXCur = 0; imuAngularRotationYCur = 0; imuAngularRotationZCur = 0;
imuAngularRotationXLast = 0; imuAngularRotationYLast = 0; imuAngularRotationZLast = 0;
imuAngularFromStartX = 0; imuAngularFromStartY = 0; imuAngularFromStartZ = 0;
IMU数据存储初始化：
for (int i = 0; i < imuQueLength; ++i)
{
imuTime[i] = 0;
imuRoll[i] = 0; imuPitch[i] = 0; imuYaw[i] = 0;
imuAccX[i] = 0; imuAccY[i] = 0; imuAccZ[i] = 0;
imuVeloX[i] = 0; imuVeloY[i] = 0; imuVeloZ[i] = 0;
imuShiftX[i] = 0; imuShiftY[i] = 0; imuShiftZ[i] = 0;
imuAngularVeloX[i] = 0; imuAngularVeloY[i] = 0; imuAngularVeloZ[i] = 0;
imuAngularRotationX[i] = 0; imuAngularRotationY[i] = 0; imuAngularRotationZ[i] = 0;
}
变换矩阵初始化：
skipFrameNum = 1;
for (int i = 0; i < 6; ++i){
transformCur[i] = 0;
transformSum[i] = 0;
}
优化初始化：
systemInitedLM = false;
IMU相对于初始的变化量：
imuRollLast = 0; imuPitchLast = 0; imuYawLast = 0;
imuShiftFromStartX = 0; imuShiftFromStartY = 0; imuShiftFromStartZ = 0;
imuVeloFromStartX = 0; imuVeloFromStartY = 0; imuVeloFromStartZ = 0;
上帧点云初始化：
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());
laserCloudOri.reset(new pcl::PointCloud<PointType>());
coeffSel.reset(new pcl::PointCloud<PointType>());
KD树初始化：
kdtreeCornerLast.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfLast.reset(new pcl::KdTreeFLANN<PointType>());
ROS消息坐标定义：
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometryTrans.frame_id_ = "/camera_init";
laserOdometryTrans.child_frame_id_ = "/laser_odom";
退化矩阵初始化：
isDegenerate = false;
matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
frameCount = skipFrameNum;
}
3. 回调函数
(1) 分割点云接受laserCloudHandler
主要是获取接受点云的时间、将点云信息将sensor_msgs::PointCloud2ConstPtr转换成pcl::PointCloud以及设置标志位。
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
cloudHeader = laserCloudMsg->header;
timeScanCur = cloudHeader.stamp.toSec();
timeNewSegmentedCloud = timeScanCur;
segmentedCloud->clear();
pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);
newSegmentedCloud = true;
(2) 界外点云接受outlierCloudHandler
同样是接受点云的时间、将点云信息将sensor_msgs::PointCloud2ConstPtr转换成pcl::PointCloud以及设置标志位。
void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){
timeNewOutlierCloud = msgIn->header.stamp.toSec();
outlierCloud->clear();
pcl::fromROSMsg(*msgIn, *outlierCloud);
newOutlierCloud = true;
}
(3) 自定义消息接受outlierCloudHandler
这个回调函数更加简单，由于是自定义数据格式，所以直接赋值即可。
void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn)
{
timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
segInfo = *msgIn;
newSegmentedCloudInfo = true;
}
(4) IMU数据接受imuHandler
由于这部分和LOAM中的代码并没有什么不同，这里把之前的解析链接贴在这里，以供参考loam源码解析1 ： scanRegistration（一）.
a) IMU数据解析
接受IMU数据：
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
将IMU数据进行坐标轴转换(从xyz转换到yzx)并去除重力加速度的影响：
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
时间戳循环位移：
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
角度：
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
加速度(在IMU坐标系下)：
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
角加速度：
imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;
AccumulateIMUShiftAndRotation();
}
b) IMU数据转换AccumulateIMUShiftAndRotation
这里与loam源码解析1 ： scanRegistration（一）中的AccumulateIMUShift函数完全一样，更详细的解释请看链接。
这里主要是实现 a w = a I ∗ R z − 1 ∗ R x − 1 ∗ R y − 1 a^w=a^I*R_z^{-1}*R_x^{-1}*R_y^{-1} aw=aI∗Rz−1∗Rx−1∗Ry−1
void AccumulateIMUShiftAndRotation()
{
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
位移：
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
if (timeDiff < scanPeriod) {
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;
速度：
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
旋转角：
imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;
imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;
imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;
}
}