<launch>
    ........

    <node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/>
    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/>
    <node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/>

</launch>

    ImageProjection::ImageProjection() : nh("~") {
        // init params
        InitParams();
        // subscriber
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1,
                                                               &ImageProjection::cloudHandler, this);
        // publisher
        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);

        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1);  // 离群点或异常点

        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
        nanPoint.intensity = -1;

        allocateMemory();
        resetParameters();
    }

    void ImageProjection::cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
        // 1. Convert ros message to pcl point cloud
        copyPointCloud(laserCloudMsg);
        // 2. Start and end angle of a scan
        findStartEndAngle();
        // 3. Range image projection
        projectPointCloud();
        // 4. Mark ground points
        groundRemoval();
        // 5. Point cloud segmentation
        cloudSegmentation();
        // 6. Publish all clouds
        publishCloud();
        // 7. Reset parameters for next iteration
        resetParameters();
    }

void FeatureAssociation::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);

        // 对加速度进行坐标变换
        // 进行加速度坐标交换时将重力加速度去除，然后再进行xxx到zzz,yyy到xxx,zzz到yyy的变换。
        // 去除重力加速度的影响时，需要把重力加速度分解到三个坐标轴上，然后分别去除他们分量的影响，在去除的过程中需要注意加减号（默认右手坐标系的旋转方向来看）。
        // 在上面示意图中，可以简单理解为红色箭头实线分解到红色箭头虚线上（根据pitchpitchpitch进行分解）,然后再按找rollrollroll角进行分解。
        // 原文链接：https://blog.csdn.net/wykxwyc/article/details/98317544
        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;

        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();
    }

//  主程序入口
    void FeatureAssociation::runFeatureAssociation() {
        // 有新数据进来才执行
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) {

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        } else {
            return;
        }
        /**
            1. Feature Extraction
        */
        adjustDistortion();  //  imu去畸变

        calculateSmoothness(); //  计算光滑性

        markOccludedPoints();  // 距离较大或者距离变动较大的点标记

        extractFeatures();  //  特征提取（未看懂）

        publishCloud(); // cloud for visualization

        /**
        2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();  //  更新初始位姿

        //  一个是找特征平面，通过面之间的对应关系计算出变换矩阵。
        // 另一个部分是通过角、边特征的匹配，计算变换矩阵。
        updateTransformation();

        integrateTransformation();  //  计算旋转角的累积变化量。

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization
    }