当前位置：首页 > news >正文

lego-loam featureAssociation 源码注释（二）

news 2025/7/8 6:33:40

咱们接着往下看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"坐标系中。