lego-loam featureAssociation 源码注释（二）
咱们接着往下看initializationValue();！！！
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);
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
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();
}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>());首先，第一部分的初始化和loam的初始化几乎一样，可以看到提取的特征仍然是根据Curvature来计算的，也就是根据曲率来计算的。分别包括sharp、lesssharp、flat、lessflat四类。
timeScanCur = 0;
timeNewSegmentedCloud = 0;
timeNewSegmentedCloudInfo = 0;
timeNewOutlierCloud = 0;
newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
systemInitCount = 0;
systemInited = false;
当初始化完成后，systemInited会变成true。几个时间分别是当前扫描时间，特征提取的时间和找到离群点的时间。这些具体的步骤在后续的代码中详细介绍。
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;
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;
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;
imuRollLast = 0; imuPitchLast = 0; imuYawLast = 0;
imuShiftFromStartX = 0; imuShiftFromStartY = 0; imuShiftFromStartZ = 0;
imuVeloFromStartX = 0; imuVeloFromStartY = 0; imuVeloFromStartZ = 0;extern const int imuQueLength = 200;
transformCur包括三个平移向量和三个旋转角，roll,pitch,yaw。
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());
laserCloudOri.reset(new pcl::PointCloud<PointType>());
coeffSel.reset(new pcl::PointCloud<PointType>());
kdtreeCornerLast.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfLast.reset(new pcl::KdTreeFLANN<PointType>());
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;skipFrameNum = 1; frameCount = 1;
在ROS中，frame_id和child_frame_id是用来指定坐标系的，它们定义了数据的参考点。
-
laserOdometry.header.frame_id = "camera_init";这行代码设置了激光里程计消息的frame_id为"camera_init"。frame_id通常用来指定消息的参考坐标系。在这里，"camera_init"可能是指相机初始化时的坐标系。 -
laserOdometry.child_frame_id = "/laser_odom";这行代码设置了激光里程计消息的child_frame_id为"/laser_odom"。child_frame_id通常用来指定相对于frame_id的子坐标系。在这里，"/laser_odom"可能是指激光里程计的坐标系。 -
laserOdometryTrans.frame_id_ = "camera_init";和laserOdometryTrans.child_frame_id_ = "/laser_odom";这两行代码看起来是在设置一个转换（Transform）消息，它将"camera_init"坐标系中的点转换到"/laser_odom"坐标系中。