LeGo-LOAM源码篇:源码分析(2)

slamml

已于 2022-09-24 16:05:26 修改

阅读量1.8k

点赞数 3

分类专栏： # LeGO-LOAM 文章标签： slam 算法

于 2020-10-28 10:24:45 首次发布

本文链接：https://blog.csdn.net/weixin_42576673/article/details/109327473

版权

src/featureAssociation.cpp
订阅分割点云、外点、imu数据，总体流程：（特征提取部分）订阅传感器数据->运动畸变去除->曲率计算->去除瑕点->提取边、平面特征->发布特征点云；（特征关联部分）将IMU信息作为先验->根据线特征、面特征计算位姿变换->联合IMU数据进行位姿更新->发布里程计信息

...
        /**
        1. Feature Extraction
        */
        adjustDistortion();//去除运动畸变
        calculateSmoothness();//计算曲率，以相邻左右各五个点计算
        markOccludedPoints();//标记瑕点，相邻但距离较远的点（平行点被误判为平面点的情况），以及邻域距离变化较大的点（遮挡点被误判为边点的情况）
        extractFeatures();//特征提取
        publishCloud(); // cloud for visualization	
        /**
		2. Feature Association
        */
        if (!systemInitedLM) {
   
            checkSystemInitialization();//系统初始化，发布特征点云用于可视化
            return;
        }
        updateInitialGuess();//先验位姿
        updateTransformation();//更新位姿
        integrateTransformation();//变换
        publishOdometry();//发布里程计信息
        publishCloudsLast(); // cloud to mapOptimization
...

特征提取程序

订阅处理后的点云、IMU数据，初始化。

...
        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);
...        
        initializationValue();//变量、容器等初始化赋值
...

IMU数据处理

...
    //单帧IMU在全局坐标系下的位移、速度、角速度
    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];
        //IMU坐标系到世界坐标系的变换
        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;
        //imu测量周期
        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;
        }
    }
...
  void imuHandler(const sensor_msgs::Imu