2020.8.2 LeGO-LOAM代码阅读(2)——featureAssociation.cpp

最新推荐文章于 2022-10-01 09:00:37 发布

shuang_yu_

最新推荐文章于 2022-10-01 09:00:37 发布

阅读量744

点赞数 2

本文链接：https://blog.csdn.net/shuang_yu_/article/details/107733893

版权

处理流程

在这里插入图片描述

main函数

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

参量注释

class FeatureAssociation{
private:
    // ROS句柄
	ros::NodeHandle nh;                                
    //三个点云信息订阅者 
    ros::Subscriber subLaserCloud;                         
    ros::Subscriber subLaserCloudInfo;
    ros::Subscriber subOutlierCloud;
    //IMU数据订阅者
    ros::Subscriber subImu;
    //多个发布者 
    ros::Publisher pubCornerPointsSharp;
    ros::Publisher pubCornerPointsLessSharp;
    ros::Publisher pubSurfPointsFlat;
    ros::Publisher pubSurfPointsLessFlat;

    //点云集合
    pcl::PointCloud<PointType>::Ptr segmentedCloud;         //存储来自主题"/segmented_cloud"的点云,是经过imageProjection.cpp处理后的点云:主要是被分割点和经过降采样的地面点,其 intensity 记录了该点在深度图上的索引位置
    pcl::PointCloud<PointType>::Ptr outlierCloud;           //存储来自主题 "/outlier_cloud"的的点云,在imageProjection.cpp中被放弃的点云：主要是经过降采样的未被分割点云。
    pcl::PointCloud<PointType>::Ptr cornerPointsSharp;      //强角点
    pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp;  //普通角点
    pcl::PointCloud<PointType>::Ptr surfPointsFlat;         //平面点集合
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlat;     //除了角点的其他点(经过了降采样,即surfPointsLessFlatScanDS的累计值)
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan; //当前线上的 除了角点的其他所有点 
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScanDS;   //上述点集的降采样
    pcl::VoxelGrid<PointType> downSizeFilter;               // 用于对 surfPointsLessFlatScan 进行降采样

    //点云的时间，指的是传感器采集到点云信息的时间。
    double timeScanCur;                 // segmentedCloud的时间
    double timeNewSegmentedCloud;       // segmentedCloud的时间(与上者相同)
    double timeNewSegmentedCloudInfo;   // segInfo       的时间
    double timeNewOutlierCloud;         // outlierCloud  的时间

    // 三个点云数据进入featureAssociation的标志位,接收到数据使,赋值为true,进入处理过程后,再次赋值为false
    bool newSegmentedCloud;         // segmentedCloud   的标志位 
    bool newSegmentedCloudInfo;     // segInfo          的标志位 
    bool newOutlierCloud;           // outlierCloud     的标志位 


    cloud_msgs::cloud_info segInfo; // 点云分割信息.即主题"/segmented_cloud_info"的接收载体
    std_msgs::Header cloudHeader;   // segmentedCloud进入时，记录其header作为当前点云的header 

    int systemInitCount;
    bool systemInited;

    std::vector<smoothness_t> cloudSmoothness;//点云中每个点的光滑度,每一项包含<float value,size_t ind>即粗糙度值、和该点在点云中的索引。

    float   *cloudCurvature;        //粗糙度序列
    int     *cloudNeighborPicked;   //择取特征点的标志位(如果值1,则不选为特征点),防止特征点集中地出现
    int     *cloudLabel;            //记录特征点的属性 2:强角点;1普通角点;-1 平面点

    int imuPointerFront;            //与点云数据时间轴对齐的IMU数据位置
    int imuPointerLast;             //最新的IMU数据在队列中的位置
    int imuPointerLastIteration;    //处理上一张点云时的结束IMU数据位置 


    // 每一帧点云的第一个点时的机器人的相关属性值 
    float imuRollStart, imuPitchStart, imuYawStart;   //起始 RPY值
    float cosImuRollStart, cosImuPitchStart, cosImuYawStart, sinImuRollStart, sinImuPitchStart, sinImuYawStart;//起始RPY值的正余弦
    float imuVeloXStart, imuVeloYStart, imuVeloZStart; //起始速度值
    float imuShiftXStart, imuShiftYStart, imuShiftZStart; //起始的位置里程计

    // 当前点时 机器人的相关属性值  
    float imuRollCur, imuPitchCur, imuYawCur;       //RPY值 
    float imuVeloXCur, imuVeloYCur, imuVeloZCur;    // 速度值
    float imuShiftXCur, imuShiftYCur, imuShiftZCur; // 位置里程计 
    float imuAngularRotationXCur, imuAngularRotationYCur, imuAngularRotationZCur; // 角度里程计

    float imuShiftFromStartXCur, imuShiftFromStartYCur, imuShiftFromStartZCur; // 位置里程计变化
    float imuVeloFromStartXCur, imuVeloFromStartYCur, imuVeloFromStartZCur;    // 当前点距离当前帧起始点的 速度变化

    //与上一帧有关的两个角度值 
    float imuAngularRotationXLast, imuAngularRotationYLast, imuAngularRotationZLast; // 记录上一阵点云起始点的 角度里程计 
    float imuAngularFromStartX, imuAngularFromStartY, imuAngularFromStartZ;  //   上一阵过程中 角度里程计(角度变化量)


    //IMU数据队列(长度是imuQueLength,循环使用)：分别存储IMU数据的 时间、RPY、加速度、速度、位置里程计、角速度里程计、角度里程计 队列,imuQueLength
    double imuTime[imuQueLength]; //时间
    float imuRoll[imuQueLength];  //RPY值
    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;  //以skipFrameNum的频率发布的 Less角点点云 
    ros::Publisher pubLaserCloudSurfLast;    //以skipFrameNum的频率发布的 Less平面点点云    
    ros::Publisher pubLaserOdometry;         
    ros::Publisher pubOutlierCloudLast;
  
    int skipFrameNum;   // 系统每过 skipFrameNum 张点云,进行一次信息发布:outlierCloud\Less角点\Less平面点
    bool systemInitedLM;  // 系统是否进行初始化的标志 

    int laserCloudCornerLastNum; // 上一帧中 less角点的数量 
    int laserCloudSurfLastNum;   // 上一帧中 less平面点的数量

    int *pointSelCornerInd;
    float *pointSearchCornerInd1;
    float *pointSearchCornerInd2;

    int *pointSelSurfInd;
    float *pointSearchSurfInd1; //最近点 序列
    float *pointSearchSurfInd2; // 前向次近点 序列
    float *pointSearchSurfInd3; // 后向次近点 序列 

    float transformCur[6]; // 当前的位姿估计
    float transformSum[6]; // 累积位姿变换量。 

    float imuRollLast, imuPitchLast, imuYawLast;   //当前帧末尾  RPY值 
    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;//记录上一帧less角点的KD树 
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast;  //记录上一帧less平面点的KD树

    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; // 用来辅助 skipFrameNum 进行信息发布频率的控制 

public:
	... 
}

初始化函数


class FeatureAssociation{
private:
	...
public:
	FeatureAssociation():nh("~"){
		//多个订阅
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
        // 主题"/segmented_cloud"发送的是经过imageProjection.cpp处理后的点云:主要是被分割点和经过降采样的地面点,其 intensity 记录了该点在深度图上的索引位置
        subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
        // 主题"/segmented_cloud_info"发送的是激光数据分割信息:
        // segMsg{
        //     Header header                       //与接收到的当前帧点云数据的header一致
        //     int32[]     startRingIndex          //segMsg点云(以一维形式存储)中,每一行点云的起始和结束索引
        //     int32[]     endRingIndex      
        //     float32     startOrientation        //起始点与结束点的水平角度(atan(y,x))
        //     float32     endOrientation  
        //     float32     orientationDiff         //以上两者的差
        //     bool[]      segmentedCloudGroundFlag//segMsg中点云的地面点标志序列(true:ground point)
        //     uint32[]    segmentedCloudColInd    //segMsg中点云的cols序列
        //     float32[]   segmentedCloudRange     //segMsg中点云的range   
        // }
        subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
        // 主题"/outlier_cloud"发送的是在imageProjection.cpp中被放弃的点云：主要是经过降采样的未被分割点云。
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
        // 主题imuTopic发送的是IMU数据
        // std_msgs/Header header
        // geometry_msgs/Quaternion orientation    //姿态
        // float64[9] orientation_covariance       //姿态的方差
        // geometry_msgs/Vector3 angular_velocity  //角速度
        // float64[9] angular_velocity_covariance  // 角速度的方差
        // geometry_msgs/Vector3 linear_acceleration   //线速度
        // float64[9] linear_acceleration_covariance   //线速度的方差

        //多个发布 
        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);
          = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
        pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
        // 变量初始化
        initializationValue();
	}
	void initializationValue(){
		...
    }

初始化函数之后就开始循环信息的处理，即FA.runFeatureAssociation()；同时ROS节点监听点云与IMU数据，并实时接收。

点云信息的接收与处理

三个点云订阅者的回调函数：

    /*
    主题"/segmented_cloud"的回调函数,接收到的是经过imageProjection.cpp处理后的点云:主要是被分割点和经过降采样的地面点,其 intensity 记录了该点在深度图上的索引位置
    由点云 segmentedCloud 接收
    同时 
        cloudHeader 记录消息的header 
        timeScanCur 记录header中的时间 
        标志位 newSegmentedCloud 置为true
    */
    void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
        cloudHeader = laserCloudMsg->header;
        timeScanCur = cloudHeader.stamp.toSec();
        timeNewSegmentedCloud = timeScanCur;
        segmentedCloud->clear();
        pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);
        newSegmentedCloud = true;
    }
    /* 主题"/outlier_cloud"的回调函数,
        outlierCloud 记录传回的点云 
        同时 timeNewOutlierCloud 记录header的时间
        标志位 newOutlierCloud 置为1 
    */
    void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){
        timeNewOutlierCloud = msgIn->header.stamp.toSec();
        outlierCloud->clear();
        pcl::fromROSMsg(*msgIn, *outlierCloud);
        newOutlierCloud = true;
    }
    /*
        主题"/segmented_cloud_info"的回调函数 
        timeNewSegmentedCloudInfo 记录时间 
        标志位  newSegmentedCloudInfo置true 
        segInfo 记录分割信息 
    */
    void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn)
    {   
        timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
        segInfo = *msgIn;
        newSegmentedCloudInfo = true;
    }

IMU数据的回调函数

class FeatureAssociation{
private:
	...
public:
    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){
        /* 
        sensor_msgs::Imu: 
        Header header  
        geometry_msgs/Quaternion orientation   // 姿态
        float64[9] orientation_covariance # Row major about x, y, z axes  
        geometry_msgs/Vector3 angular_velocity  //方向速度
        float64[9] angular_velocity_covariance # Row major about x, y, z axes  
        geometry_msgs/Vector3 linear_acceleration  // 线性速度
        float64[9] linear_acceleration_covariance # Row major x, y z 
        */
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        // 提取PRY值、与真实的加速度值
        tf::quaternionMsgToTF(imuIn->orientation, orientation); //从IMU中提取出 表达姿态的四元数 
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);    //将姿态四元数转化为 RPY值
        float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;//获得真实的x方向的加速度
        float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;//同上
        float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;  //同上 
        // 接收到新的IMU数据,索引 imuPointerLast++ （imuQueLength是缓存队列的长度)，并更新：
            // imuTime(IMU时间队列) 
            // imuRoll、imuPitch、imuYaw(RPY队列) 
            // imuAccX、imuAccY、imuAccZ(加速度队列) 
            // imuAngularVeloX、imuAngularVeloY、imuAngularVeloZ(角速度队列)
        imuPointerLast = (imuPointerLast + 1) % imuQueLength; 
        imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); //imuTime 队列记录消息时间
        imuRoll[imuPointerLast] = roll;  // imuRoll、imuPitch、imuYaw 队列存储RPY值
        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(); 
        // 取出前后两个IMU数据,如果时间上相邻(相差小于一次扫描周期)，那么计算：
        // imuShiftX、imuShiftY、imuShiftZ 里程计序列(利用速度值与加速度计算位置里程计)
        // imuVeloX、imuVeloY、imuVeloZ 速度里程计 
        // imuAngularRotationX、imuAngularRotationY、imuAngularRotationZ 角度历程计
    }
    void AccumulateIMUShiftAndRotation(){
    	...
    }
	...
}

处理过程

class FeatureAssociation{
private:
	...
public:
    void runFeatureAssociation()
    {
        // 获得了三个点云主题的msg,且时间上一致,将标志位置0,继续接下来的处理
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){ 
            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        }else{
            return;
        }
       
		
        // 功能: 消除因平台的移动导致的点云畸变。
        adjustDistortion();
        // 步骤:
        // 循环点云 segmentedCloud 上的所有点
        //     1:计算每个传感器在采集每个点时的确切时间,通过确定当前点的偏航角在扫描范围内的位置实现。
        //     并更新点的 intensity 属性： intensity+(本次扫描开始到采集到该点的时间)
        //     2. 将IMU数据时间与点云数据时间对齐
        //     即确定IMU数据索引 imuPointerFront,使得该IMU采集时间与当前点的采集时间紧邻
        //     3. 通过线性插值 求解机器人取得当前点时的状态:
        //         imuRollCur,imuPitchCur,imuYawCur RPY值
        //         imuVeloXCur, imuVeloYCur, imuVeloZCur  速度值
        //         imuShiftXCur, imuShiftYCur, imuShiftZCur  位置里程计 
        //     4. 如果是一帧的第一个点,则另外记录当前点时的状态值作为当前帧的初始状态：
        //         imuRollStart, imuPitchStart, imuYawStart 起始RPY值 
        //         imuVeloXStart, imuVeloYStart, imuVeloZStart; //起始速度值
        //         imuShiftXStart, imuShiftYStart, imuShiftZStart; //起始的位置里程计
        //         imuAngularRotationXCur, imuAngularRotationYCur, imuAngularRotationZCur; // 角度里程计 
        //         cosImuRollStart, cosImuPitchStart, cosImuYawStart, sinImuRollStart, sinImuPitchStart, sinImuYawStart;//起始RPY值的正余弦
        //     并更新：
        //         imuAngularFromStartX, imuAngularFromStartY, imuAngularFromStartZ;  //   上一阵过程中 角度里程计(角度变化量)
        //         imuAngularRotationXLast, imuAngularRotationYLast, imuAngularRotationZLast; // 记录上一阵点云起始点的 角度里程计 
        //     5. 如果不是第一个点,那么计算当前扫描中,当前点的速度变化量,更新:
        //         imuVeloFromStartXCur、imuVeloFromStartYCur、imuVeloFromStartZCur 
        //     修正因机器人的移动造成的点云失真,更新 segmentedCloud 中点的位置属性 point.x、y、z。


        // 功能：计算每个点的粗糙度 
        calculateSmoothness();
        // 赋值粗糙度序列 cloudCurvature 
        // 赋值粗糙度信息序列 cloudSmoothness,其中每一个元素<value,ind>记录某点的粗糙度以及其索引

        // 功能：将被遮挡物体的择取特征点的标志位置为1(不选为特征点),防止由于遮挡导致在被遮挡的平面上选出角特征点.
        markOccludedPoints();
        // 更新相应部分的 择取特征点的标志 cloudNeighborPicked

        // 提取特征点 
        extractFeatures();
        // 将每一条线16等分,并在每一等分上，对所有点的粗糙度排序，遴选出 
        // cornerPointsSharp        强角点
        // cornerPointsLessSharp    普通角点
        // surfPointsFlat           平面点
        // surfPointsLessFlat       除了角点的所有点(经过了降采样,且这个不属于特征点)
        // 每次选出一个特征点 ，更新该点和附近点的择选标志位 cloudNeighborPicked ,防止特征点扎堆。
        // 并更新每个点的特征标志位 cloudLabel(2:强角点;1普通角点;-1 平面点,0 其他)


        publishCloud(); // cloud for visualization
        // 如果存储订阅者,那么发布 
        // 主题"/laser_cloud_sharp"发布强角点点云 cornerPointsSharp
        // 主题"/laser_cloud_less_sharp" 发布Less角点点云 cornerPointsLessSharp 
        // 主题"/laser_cloud_flat"发布平面点点云  surfPointsFlat
        // 主题"/laser_cloud_less_flat"发布Less平面点点云  surfPointsLessFlat 
    	
        /**
		2. Feature Association
        */

        // 检查系统是否进行初始化了(当前是否是第一帧)
        if (!systemInitedLM) {
            checkSystemInitialization();
            // 第一帧时，无法进行定位，为直接进入下一帧的处理进行准备,即更新
            // laserCloudCornerLast    更新上一帧的Less角点
            // laserCloudSurfLast      更新上一帧的Less平面点
            // laserCloudCornerLastNum 上一帧Less角点数量
            // laserCloudSurfLastNum   上一帧Less平面点的数量
            // 系统初始化标志位 systemInitedLM 置 true
            return;
        }

        updateInitialGuess();
        // 由IMU获得位姿估计的初始值,即更新
        //     位姿 transformCur[0~5] 
        // 并更新 
        //     imuPitchLast、imuYawLast、imuRollLast 上一帧RPY值
        //     imuShiftFromStartX、imuShiftFromStartY、imuShiftFromStartZ  当前帧位置变化量
        //     imuVeloFromStartX、imuVeloFromStartY、imuVeloFromStartZ  当前帧速度变化量 


        updateTransformation();

        integrateTransformation();

        publishOdometry();
        // 主题"/laser_odom_to_init"发布里程计信息 
        //     msg.header.stamp 存储时间戳 
        //     msg.header.frame_id = "/camera_init";
        //     msg.child_frame_id = "/laser_odom";
        //     msg.pose.pose.orientation 存储姿态的四元数 
        //     msg.pose.pose.position 存储位置

        publishCloudsLast(); // cloud to mapOptimization
        // 1. 更新起始PRY值的正余弦值 (为什么？)
        // 2. 将每个Less角点、Less平面点转化到该帧末尾的实际位置
        // 3. 为进入下一帧，更新相关的"上一帧点云"为当前点云
        //     laserCloudCornerLast        上一帧 Less角点
        //     laserCloudSurfLast          上一帧 Less平面点 
        //     laserCloudCornerLastNum     上一帧 Less角点 数量 
        //     laserCloudSurfLastNum       上一帧 Less平面点 点数量 
        //     kdtreeCornerLast            上一帧 Less角点的KD树
        //     kdtreeSurfLast              上一帧 Less平面点的KD树
        // 4. 每处理 skipFrameNum 帧,发布消息
        //     主题"/outlier_cloud_last" 发布消息 outlierCloud 
        //     主题"/laser_cloud_corner_last" 发布消息 laserCloudCornerLast 
        //     主题"/laser_cloud_surf_last" 发布消息 laserCloudSurfLast 
	

	}
	...
}

牛顿高斯方程部分

待续- -！

shuang_yu_

关注

2
点赞
踩
12

收藏

觉得还不错? 一键收藏
1
评论
2020.8.2 LeGO-LOAM代码阅读(2)——featureAssociation.cpp

main函数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
复制链接

扫一扫