devbox
我家小花儿
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

LIO_SAM实测运行，论文学习及代码注释[附对应google driver数据]_lio sam

作者：我家小花儿 | 2024-02-19 20:36:29

赞

踩

lio sam

LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

作者Tixiao Shan在2018年发表过LeGO-LOAM，当时他还在史蒂文斯理工学院读博士，19年毕业之后去了MIT做助理研究员（羡慕.jpg）。。。这篇文章LIO-SAM实际上是LeGO-LOAM的扩展版本，添加了IMU预积分因子和GPS因子，去除了帧帧匹配部分，然后更详细地描述了LeGO-LOAM帧图匹配部分的设计动机和细节。(引用于知乎大佬文章【论文阅读38】LIO-SAM)现在论文已经被IROS2020录用，作为高精度，imu，雷达，gps结合，程序还少的开源slam，非常值得学习。

目录

原文代码和数据

安装和运行

论文分析

程序注释

程序ros节点图

全部函数说明

imageProjection.cpp

featureExtraction.cpp

imuPreintegration.cpp

mapOptmization.cpp

原文代码和数据

原文：https://github.com/TixiaoShan/LIO-SAM/blob/master/config/doc/paper.pdf

代码：https://github.com/TixiaoShan/LIO-SAM

数据：链接: https://pan.baidu.com/s/1-sAB_cNlYPqTjDuaFgz9pg 提取码: ejmu （walk不需要改配置文件，其他两个需要下文有）

安装和运行

ros

  1. sudo apt-get install -y ros-kinetic-navigation
  2. sudo apt-get install -y ros-kinetic-robot-localization
  3. sudo apt-get install -y ros-kinetic-robot-state-publisher

gtsam4.0.2（网速慢用国内的码云https://gitee.com/eminbogen/gtsam/tags）

  1. wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip
  2. cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
  3. cd ~/Downloads/gtsam-4.0.2/
  4. mkdir build && cd build
  5. cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
  6. sudo make install -j8

install（同上https://gitee.com/eminbogen/LIOSAM）

  1. cd ~/catkin_ws/src
  2. git clone https://github.com/TixiaoShan/LIO-SAM.git
  3. cd ..
  4. catkin_make

运行walk数据包不需要改params.yaml文件。其他两个数据包运行要修改topics和extrinsicRPY，extrinsicRot。需要保存pcd请修改保存true和路径。之后

sudo gedit /opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/nodeprocess.py

调大_TIMEOUT_SIGINT值

具体params.yaml配置修改：

  1. lio_sam:
  2.  
  3.   # Topics
  4.   pointCloudTopic: "points_raw"               # Point cloud data
  5.   imuTopic: "imu_correct"                         # IMU data
  6.   odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  7.   gpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file
  8.  
  9.   # GPS Settings
  10.   useImuHeadingInitialization: false           # if using GPS data, set to "true"
  11.   useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  12.   gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  13.   poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  14.   
  15.   # Export settings
  16.   savePCD: true                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  17.   savePCDDirectory: "/data/lio/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
  18.  
  19.   # Sensor Settings
  20.   N_SCAN: 16                                  # number of lidar channel (i.e., 16, 32, 64, 128)
  21.   Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
  22.   timeField: "time"                           # point timestamp field, Velodyne - "time", Ouster - "t"
  23.   downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 
  24.  
  25.   # IMU Settings
  26.   imuAccNoise: 3.9939570888238808e-03
  27.   imuGyrNoise: 1.5636343949698187e-03
  28.   imuAccBiasN: 6.4356659353532566e-05
  29.   imuGyrBiasN: 3.5640318696367613e-05
  30.   imuGravity: 9.80511
  31.  
  32.   # Extrinsics (lidar -> IMU)
  33.   extrinsicTrans: [0.0, 0.0, 0.0]
  34.   extrinsicRPY: [1,  0, 0,
  35.                  0, 1, 0,
  36.                   0, 0, 1]
  37.   extrinsicRot: [1, 0, 0,
  38.                    0, 1, 0,
  39.                    0, 0, 1]
  40.   # extrinsicRPY: [1, 0, 0,
  41.   #                 0, 1, 0,
  42.   #                 0, 0, 1]
  43.  
  44.   # LOAM feature threshold
  45.   edgeThreshold: 1.0
  46.   surfThreshold: 0.1
  47.   edgeFeatureMinValidNum: 10
  48.   surfFeatureMinValidNum: 100
  49.  
  50.   # voxel filter paprams
  51.   odometrySurfLeafSize: 0.4                     # default: 0.4
  52.   mappingCornerLeafSize: 0.2                    # default: 0.2
  53.   mappingSurfLeafSize: 0.4                      # default: 0.4
  54.  
  55.   # robot motion constraint (in case you are using a 2D robot)
  56.   z_tollerance: 1000                            # meters
  57.   rotation_tollerance: 1000                     # radians
  58.  
  59.   # CPU Params
  60.   numberOfCores: 4                              # number of cores for mapping optimization
  61.   mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
  62.  
  63.   # Surrounding map
  64.   surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
  65.   surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
  66.   surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
  67.   surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
  68.  
  69.   # Loop closure
  70.   loopClosureEnableFlag: false
  71.   surroundingKeyframeSize: 25                   # submap size (when loop closure enabled)
  72.   historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
  73.   historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
  74.   historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
  75.   historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
  76.  
  77.   # Visualization
  78.   globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
  79.   globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
  80.   globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
  81.  
  82.  
  83.  
  84.  
  85. # Navsat (convert GPS coordinates to Cartesian)
  86. navsat:
  87.   frequency: 50
  88.   wait_for_datum: false
  89.   delay: 0.0
  90.   magnetic_declination_radians: 0
  91.   yaw_offset: 0
  92.   zero_altitude: true
  93.   broadcast_utm_transform: false
  94.   broadcast_utm_transform_as_parent_frame: false
  95.   publish_filtered_gps: false
  96.  
  97. # EKF for Navsat
  98. ekf_gps:
  99.   publish_tf: false
  100.   map_frame: map
  101.   odom_frame: odom
  102.   base_link_frame: base_link
  103.   world_frame: odom
  104.  
  105.   frequency: 50
  106.   two_d_mode: false
  107.   sensor_timeout: 0.01
  108.   # -------------------------------------
  109.   # External IMU:
  110.   # -------------------------------------
  111.   imu0: imu_correct
  112.   # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
  113.   imu0_config: [false, false, false,
  114.                 true,  true,  true,
  115.                 false, false, false,
  116.                 false, false, true,
  117.                 true,  true,  true]
  118.   imu0_differential: false
  119.   imu0_queue_size: 50 
  120.   imu0_remove_gravitational_acceleration: true
  121.   # -------------------------------------
  122.   # Odometry (From Navsat):
  123.   # -------------------------------------
  124.   odom0: odometry/gps
  125.   odom0_config: [true,  true,  true,
  126.                  false, false, false,
  127.                  false, false, false,
  128.                  false, false, false,
  129.                  false, false, false]
  130.   odom0_differential: false
  131.   odom0_queue_size: 10
  132.  
  133.   #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
  134.   process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
  135.                                0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
  136.                                0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
  137.                                0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
  138.                                0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
  139.                                0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
  140.                                0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
  141.                                0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
  142.                                0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
  143.                                0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
  144.                                0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
  145.                                0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
  146.                                0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
  147.                                0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
  148.                                0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

效果从地面平整，墙面贴合，墙体厚薄看还是非常好的。

论文分析

论文认为loam系列文章存在一些问题。

1.将其数据保存在全局体素地图中
2.难以执行闭环检测
3.没有结合其他绝对测量（GPS，指南针等）
4.当该体素地图变得密集时，在线优化过程的效率降低

决定使用因子图的思想优化激光SLAM，引入四种因子。

1.IMU预积分因子
2.激光雷达里程因子
3.GPS因子
4.闭环因子

对于预积分因子。计算采用常见的角速度，加速度，线速度，位置的公式推导方式即s=vt+1/2at^2

对于里程计因子。采用三步。

1.粗计算新帧与前关键帧的相对位姿变换，按阈值提取关键帧，五个关键帧合成一次区域的体素化点云地图。e,p代表edge和plane。M代表合成点云地图，F代表单关键帧点云，取∪集是合成。

2.edge和plane进行匹配。

3.优化也是常规优化。点对线和面对面来优化变换矩阵。

对于GPS因子。作者提出。

1.GPS测量值的时间戳根据里程计时间戳进行线性插值。
2.无需不断添加GPS因子。
3.当估计的位置协方差大于接收的GPS位置协方差时，添加GPS因子。
4.GPS在z方向可信度较低。

这里计算协方差是提取0,7,14号位置，根据ros手册和协方差知识，只是指xyz的测量方差。因为常见GPS为50HZ，所以短时间内能测多次，可以求方差。

对于闭环因子。作者提出。

1.使用的是一种简单但有效的基于欧氏距离的闭环检测方法。
2.闭环系数对于校正机器人高度的漂移特别有用，因为GPS的海拔高度测量非常不准确。

成果

1.精度对比

2.时间上特征多的区域即使scan少也更耗时，park时间少于Amsterdam。左图park

3.程序上作者注释，如果运动慢，点云由于运动导致的矫正不如没有

程序注释

程序ros节点图

全部函数说明

imageProjection.cpp:

PointXYZIPT 类型，点和强度，线数，时间。可以进pcl库使用。

Main ImageProjection():接收rosbag和lidar信息，imu的数值,来自imuPreintegration的odom数据；内存+参数初始化重置

void allocateMemory():点云输入，全部点云，提取点云空间重置（pointColInd与pointRange的定义问题）

void resetParameters():输入点云，提取点云，各点与光心距，imu数据清零

void imuHandler(const sensor_msgs::Imu::ConstPtr &imuMsg):仅接收放队列

void odometryHandler(const nav_msgs::Odometry::ConstPtr &odometryMsg):仅接收放队列

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg):深度图生成，去畸变；深度图中提取点云，发布，重置参数

bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg):清除临时点云，并检查点云帧里面是否有ring和time通道

bool deskewInfo():检测队列数量，

void imuDeskewInfo():计算imu转角

void odomDeskewInfo():读取odom数据，并根据协方差判断是否相信

void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur):将imuDeskewInfo数值用于每个点云点在一条线中的位置

void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur):高速时按位移百分比平移点云

PointType deskewPoint(PointType *point, double relTime):根据时间戳,对每个点去畸变

void projectPointCloud():将点云投影成深度图

void cloudExtraction():去边缘与过远点

void publishClouds():ros输出

featureExtraction.cpp:

struct smoothness_t 曲率与序号

struct by_value 大小排序函数

Main FeatureExtraction():获取imageProjection修正后输入点云,输出特征点和输入点云；参数初始化

void initializationValue():曲率储存，降采样，特征点，临近标签初始化

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn):点云句柄，包括下面所有内容

void calculateSmoothness():计算每个点的曲率

void markOccludedPoints():边界过远一侧去除，上下三线变化过大去除

void extractFeatures()：特征点提取

void publishFeatureCloud():ros输出

imuPreintegration.cpp:

Main IMUPreintegration(): 接收imu数据，map来的odom数据，输出imu的odom，path；预积分参数设置，gtsam参数设置

void resetOptimization():gtsam相关优化参数重置

void resetParams():初始参数设定；是否第一帧；是否进行过优化；系统初始化

void odometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg):接收map里的odom数据

  1. 获取map里的odom位置和四元数数据，转换到imu坐标系
  2. 初始化优化参数，引入PVB数据，优化，重置优化器
  3. 更新bias重新预积分

bool failureDetection(const gtsam::Vector3 &velCur, const gtsam::imuBias::ConstantBias &biasCur):判断优化后速度和bias是否计算过大

void imuHandler(const sensor_msgs::Imu::ConstPtr &imu_raw):接收imu原始数据，使用gtsam短暂预积分，计算PVB，输出暂时轨迹

mapOptmization.cpp:

struct PointXYZIRPYT 位置欧拉角时间

Main mapOptimization():接收imageProjection的lidar，IMU信息；数据集的GPS信息，发布位姿，周围点，路径，历史帧，icp帧，局部帧；定义滤波参数与内存；

void allocateMemory():历史，局部位姿，棱面点，特征值，协方差，协方差添加标志，降采样点云，KD树，最近关键帧点云，临近历史帧点云

void pointAssociateToMap(PointType const *const pi, PointType *const po):点云点到世界坐标系转换

gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint):pcl到gtsam格式

gtsam::Pose3 trans2gtsamPose(float transformIn[]):数组到gtsam格式

Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint):pcl到仿射矩阵格式(实际欧拉角)

Eigen::Affine3f trans2Affine3f(float transformIn[]):矩阵到仿射矩阵格式

PointTypePose trans2PointTypePose(float transformIn[]):数组到pcl格式

三个线程：

1.接收数据

void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg):GPS数据进队列

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn):

void updateInitialGuess():更新初始位姿

void extractSurroundingKeyFrames():从关键帧里面提取附近回环候选帧

void downsampleCurrentScan():进行下采样

void scan2MapOptimization(): 构建点到平面、点到直线的残差, 用高斯牛顿法进行优化

void saveKeyFramesAndFactor():添加factor到gtsam

void correctPoses():更新位姿

void publishOdometry():ros输出

void publishFrames():ros输出

2.闭环检测

void loopClosureThread():定期启动performLoopClosure()进行闭环检测

void performLoopClosure():根据两帧计算icp引入到gtsam优化中

bool detectLoopClosure(int *latestID, int *closestID):寻找临近历史帧和关键帧

3.显示

void visualizeGlobalMapThread():循环调用publishGlobalMap()；ros环境关闭时退出循环并生成pcd

void publishGlobalMap():输出临近点云

imageProjection.cpp

  1. #include "utility.h"
  2. #include "lio_sam/cloud_info.h"
  3.  
  4. // Velodyne点云点结构体构造，point4d是xyz和强度intensity.ring是线数，EIGEN_MAKE_ALIGNED_OPERATOR_NEW字符对齐
  5. struct PointXYZIRT
  6. {
  7.     PCL_ADD_POINT4D
  8.     PCL_ADD_INTENSITY;
  9.     uint16_t ring;
  10.     float time;
  11.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  12. } EIGEN_ALIGN16;
  13.  
  14. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT,  
  15.     (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
  16.     (uint16_t, ring, ring) (float, time, time)
  17. )
  18.  
  19. // Ouster
  20. // struct PointXYZIRT {
  21. //     PCL_ADD_POINT4D;
  22. //     float intensity;
  23. //     uint32_t t;
  24. //     uint16_t reflectivity;
  25. //     uint8_t ring;
  26. //     uint16_t noise;
  27. //     uint32_t range;
  28. //     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  29. // }EIGEN_ALIGN16;
  30.  
  31. // POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
  32. //     (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
  33. //     (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
  34. //     (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
  35. // )
  36.  
  37. //循环imu长度，作者imu Microstrain 3DM-GX5-25,为500hz，使用loam数据集时要修改
  38. const int queueLength = 500;
  39.  
  40. //引用参数
  41. class ImageProjection : public ParamServer
  42. {
  43. private:
  44.  
  45.     std::mutex imuLock;
  46.     std::mutex odoLock;
  47.  
  48.     ros::Subscriber subLaserCloud;
  49.     ros::Publisher  pubLaserCloud;
  50.     
  51.     //输出距离不过近的点云点
  52.     ros::Publisher pubExtractedCloud;
  53.     //输出下文的cloudInfo
  54.     ros::Publisher pubLaserCloudInfo;
  55.  
  56.     ros::Subscriber subImu;
  57.     std::deque<sensor_msgs::Imu> imuQueue;
  58.  
  59.     ros::Subscriber subOdom;
  60.     std::deque<nav_msgs::Odometry> odomQueue;
  61.  
  62.     std::deque<sensor_msgs::PointCloud2> cloudQueue;
  63.     sensor_msgs::PointCloud2 currentCloudMsg;
  64.     
  65.     double *imuTime = new double[queueLength];
  66.     double *imuRotX = new double[queueLength];
  67.     double *imuRotY = new double[queueLength];
  68.     double *imuRotZ = new double[queueLength];
  69.  
  70.     int imuPointerCur;
  71.     bool firstPointFlag;
  72.     Eigen::Affine3f transStartInverse;
  73.  
  74.     pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
  75.     pcl::PointCloud<PointType>::Ptr   fullCloud;
  76.     pcl::PointCloud<PointType>::Ptr   extractedCloud;
  77.  
  78.     int deskewFlag;
  79.     cv::Mat rangeMat;
  80.  
  81.     bool odomDeskewFlag;
  82.     float odomIncreX;
  83.     float odomIncreY;
  84.     float odomIncreZ;
  85.  
  86.     //这里面有imu是否可用，odo是否可用，lidar的位姿，scan中各点对应线号，各点与光心距离的信息
  87.     lio_sam::cloud_info cloudInfo;
  88.     double timeScanCur;
  89.     double timeScanNext;
  90.     std_msgs::Header cloudHeader;
  91.  
  92.  
  93. public:
  94.     ImageProjection():
  95.     deskewFlag(0)
  96.     {
  97.         subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
  98.         subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic, 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
  99.         subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
  100.  
  101.         pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
  102.         pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 10);
  103.  
  104. 	//配置内存，重置参数
  105.         allocateMemory();
  106.         resetParameters();
  107.  
  108. 	//pcl控制台输出error信息
  109.         pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
  110.     }
  111.  
  112.     void allocateMemory()
  113.     {
  114.         laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
  115.         fullCloud.reset(new pcl::PointCloud<PointType>());
  116.         extractedCloud.reset(new pcl::PointCloud<PointType>());
  117.  
  118.         fullCloud->points.resize(N_SCAN*Horizon_SCAN);
  119.  
  120.         cloudInfo.startRingIndex.assign(N_SCAN, 0);
  121.         cloudInfo.endRingIndex.assign(N_SCAN, 0);
  122.  
  123. 	//从h文件看是16*1800
  124.         cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
  125.         cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);
  126.  
  127.         resetParameters();
  128.     }
  129.  
  130.     void resetParameters()
  131.     {
  132.         laserCloudIn->clear();
  133.         extractedCloud->clear();
  134.         // reset range matrix for range image projection 全部默认初值为最大浮点数
  135.         rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
  136.  
  137.         imuPointerCur = 0;
  138.         firstPointFlag = true;
  139.         odomDeskewFlag = false;
  140.  
  141.         for (int i = 0; i < queueLength; ++i)
  142.         {
  143.             imuTime[i] = 0;
  144.             imuRotX[i] = 0;
  145.             imuRotY[i] = 0;
  146.             imuRotZ[i] = 0;
  147.         }
  148.     }
  149.  
  150.     ~ImageProjection(){}
  151.  
  152.     void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
  153.     {
  154.         sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
  155.  
  156.         std::lock_guard<std::mutex> lock1(imuLock);
  157.         imuQueue.push_back(thisImu);
  158.  
  159.         // debug IMU data
  160.         // cout << std::setprecision(6);
  161.         // cout << "IMU acc: " << endl;
  162.         // cout << "x: " << thisImu.linear_acceleration.x << 
  163.         //       ", y: " << thisImu.linear_acceleration.y << 
  164.         //       ", z: " << thisImu.linear_acceleration.z << endl;
  165.         // cout << "IMU gyro: " << endl;
  166.         // cout << "x: " << thisImu.angular_velocity.x << 
  167.         //       ", y: " << thisImu.angular_velocity.y << 
  168.         //       ", z: " << thisImu.angular_velocity.z << endl;
  169.         // double imuRoll, imuPitch, imuYaw;
  170.         // tf::Quaternion orientation;
  171.         // tf::quaternionMsgToTF(thisImu.orientation, orientation);
  172.         // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
  173.         // cout << "IMU roll pitch yaw: " << endl;
  174.         // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
  175.     }
  176.  
  177.     void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
  178.     {
  179.         std::lock_guard<std::mutex> lock2(odoLock);
  180.         odomQueue.push_back(*odometryMsg);
  181.     }
  182.  
  183.     void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
  184.     {
  185.         //检测一下点数量是否够2个，线数通道和时间通道是否存在
  186. 	if (!cachePointCloud(laserCloudMsg))
  187.             return;
  188.  
  189. 	//IMU不是空，imu序列的第一个时间戳大于当前帧雷达时间戳，IMU最后一个时间戳小于下一帧雷达时间戳
  190.         if (!deskewInfo())
  191.             return;
  192.  
  193.         projectPointCloud();
  194.  
  195.         cloudExtraction();
  196.  
  197.         publishClouds();
  198.  
  199.         resetParameters();
  200.     }
  201.  
  202.     //检测一下点数量是否够2个，线数通道和时间通道是否存在
  203.     bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
  204.     {
  205.         // cache point cloud
  206.         cloudQueue.push_back(*laserCloudMsg);
  207.  
  208.         if (cloudQueue.size() <= 2)
  209.             return false;
  210.         else
  211.         {
  212.             //提取队列第一个做timeScanCur，之后pop出第一个，选第二个做timeScanNext。后面与imu对比时间戳。
  213. 	    currentCloudMsg = cloudQueue.front();
  214.             cloudQueue.pop_front();
  215.  
  216.             cloudHeader = currentCloudMsg.header;
  217.             timeScanCur = cloudHeader.stamp.toSec();
  218.             timeScanNext = cloudQueue.front().header.stamp.toSec();
  219.         }
  220.  
  221.         // convert cloud
  222.         pcl::fromROSMsg(currentCloudMsg, *laserCloudIn);
  223.  
  224.         // check dense flag
  225.         if (laserCloudIn->is_dense == false)
  226.         {
  227.             ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
  228.             ros::shutdown();
  229.         }
  230.  
  231.         // check ring channel
  232.         static int ringFlag = 0;
  233.         if (ringFlag == 0)
  234.         {
  235.             ringFlag = -1;
  236.             for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
  237.             {
  238.                 if (currentCloudMsg.fields[i].name == "ring")
  239.                 {
  240.                     ringFlag = 1;
  241.                     break;
  242.                 }
  243.             }
  244.             if (ringFlag == -1)
  245.             {
  246.                 ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
  247.                 ros::shutdown();
  248.             }
  249.         }   
  250.  
  251.         // check point time
  252.         if (deskewFlag == 0)
  253.         {
  254.             deskewFlag = -1;
  255.             for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
  256.             {
  257.                 if (currentCloudMsg.fields[i].name == timeField)
  258.                 {
  259.                     deskewFlag = 1;
  260.                     break;
  261.                 }
  262.             }
  263.             if (deskewFlag == -1)
  264.                 ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
  265.         }
  266.  
  267.         return true;
  268.     }
  269.  
  270.     //IMU不是空，imu序列的第一个时间戳大于当前帧雷达时间戳，IMU最后一个时间戳小于下一帧雷达时间戳
  271.     bool deskewInfo()
  272.     {
  273.         std::lock_guard<std::mutex> lock1(imuLock);
  274.         std::lock_guard<std::mutex> lock2(odoLock);
  275.  
  276.         // make sure IMU data available for the scan
  277.         if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanNext)
  278.         {
  279.             ROS_DEBUG("Waiting for IMU data ...");
  280.             return false;
  281.         }
  282.  
  283.         imuDeskewInfo();
  284.  
  285.         odomDeskewInfo();
  286.  
  287.         return true;
  288.     }
  289.  
  290.     //计算当前激光时间戳前，每个时刻imu累计出的角速度，并判定imu数据是否可用（数量够）
  291.     void imuDeskewInfo()
  292.     {
  293.         cloudInfo.imuAvailable = false;
  294.  
  295. 	//直到imu的时间戳到当前scan时间戳前0.01s以内
  296.         while (!imuQueue.empty())
  297.         {
  298.             if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
  299.                 imuQueue.pop_front();
  300.             else
  301.                 break;
  302.         }
  303.  
  304.         if (imuQueue.empty())
  305.             return;
  306.  
  307.         imuPointerCur = 0;
  308.  
  309.         for (int i = 0; i < (int)imuQueue.size(); ++i)
  310.         {
  311.             sensor_msgs::Imu thisImuMsg = imuQueue[i];
  312.             double currentImuTime = thisImuMsg.header.stamp.toSec();
  313.  
  314.             //当前imu时间戳小于当前帧点云时间戳转换，大于下一帧+0.01s退出
  315. 	    // get roll, pitch, and yaw estimation for this scan
  316.             if (currentImuTime <= timeScanCur)
  317.                 imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
  318.  
  319.             if (currentImuTime > timeScanNext + 0.01)
  320.                 break;
  321.  
  322.             //赋值0
  323. 	    if (imuPointerCur == 0){
  324.                 imuRotX[0] = 0;
  325.                 imuRotY[0] = 0;
  326.                 imuRotZ[0] = 0;
  327.                 imuTime[0] = currentImuTime;
  328.                 ++imuPointerCur;
  329.                 continue;
  330.             }
  331.  
  332.             // get angular velocity
  333.             double angular_x, angular_y, angular_z;
  334.             imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
  335.  
  336.             // integrate rotation 用a×t累加
  337.             double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
  338.             imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
  339.             imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
  340.             imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
  341.             imuTime[imuPointerCur] = currentImuTime;
  342.             ++imuPointerCur;
  343.         }
  344.  
  345.         //把赋值0后面的自加减回去
  346.         --imuPointerCur;
  347.  
  348. 	//如果数量为0直接返回，不为0，该imu属性为可用
  349.         if (imuPointerCur <= 0)
  350.             return;
  351.  
  352.         cloudInfo.imuAvailable = true;
  353.     }
  354.  
  355.     //应该是为了groundtruth对比用的，pose信息保存在cloudInfo里
  356.     void odomDeskewInfo()
  357.     {
  358.         cloudInfo.odomAvailable = false;
  359.  
  360.         while (!odomQueue.empty())
  361.         {
  362.             if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
  363.                 odomQueue.pop_front();
  364.             else
  365.                 break;
  366.         }
  367.  
  368.         if (odomQueue.empty())
  369.             return;
  370.  
  371.         if (odomQueue.front().header.stamp.toSec() > timeScanCur)
  372.             return;
  373.  
  374.         // get start odometry at the beinning of the scan
  375.         nav_msgs::Odometry startOdomMsg;
  376.  
  377.         for (int i = 0; i < (int)odomQueue.size(); ++i)
  378.         {
  379.             startOdomMsg = odomQueue[i];
  380.  
  381.             if (ROS_TIME(&startOdomMsg) < timeScanCur)
  382.                 continue;
  383.             else
  384.                 break;
  385.         }
  386.  
  387.         tf::Quaternion orientation;
  388.         tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
  389.  
  390.         double roll, pitch, yaw;
  391.         tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
  392.  
  393.         // Initial guess used in mapOptimization
  394.         cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
  395.         cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
  396.         cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
  397.         cloudInfo.initialGuessRoll  = roll;
  398.         cloudInfo.initialGuessPitch = pitch;
  399.         cloudInfo.initialGuessYaw   = yaw;
  400.         cloudInfo.imuPreintegrationResetId = round(startOdomMsg.pose.covariance[0]);
  401.  
  402.         cloudInfo.odomAvailable = true;
  403.  
  404.         // get end odometry at the end of the scan
  405.         odomDeskewFlag = false;
  406.  
  407.         if (odomQueue.back().header.stamp.toSec() < timeScanNext)
  408.             return;
  409.  
  410.         nav_msgs::Odometry endOdomMsg;
  411.  
  412.         for (int i = 0; i < (int)odomQueue.size(); ++i)
  413.         {
  414.             endOdomMsg = odomQueue[i];
  415.  
  416.             if (ROS_TIME(&endOdomMsg) < timeScanNext)
  417.                 continue;
  418.             else
  419.                 break;
  420.         }
  421.  
  422.         if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
  423.             return;
  424.  
  425.         Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
  426.  
  427.         tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
  428.         tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
  429.         Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
  430.  
  431.         Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
  432.  
  433.         float rollIncre, pitchIncre, yawIncre;
  434.         pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
  435.  
  436.         odomDeskewFlag = true;
  437.     }
  438.  
  439.     //根据点云中某点的时间戳赋予其对应插值得到的旋转量
  440.     void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
  441.     {
  442.         *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
  443.  
  444.         int imuPointerFront = 0;
  445. 	//要么imuPointerFront计数大于了imu一组的数量imuPointerCur，（异常跳出）
  446. 	//要么该次imu时间戳大于了该点时间戳（话说这函数一个点调用一次，是不是静态变量好一些）
  447.         while (imuPointerFront < imuPointerCur)
  448.         {
  449.             if (pointTime < imuTime[imuPointerFront])
  450.                 break;
  451.             ++imuPointerFront;
  452.         }
  453.  
  454.         //如果计数为0或该次imu时间戳小于了该点时间戳(异常退出)
  455.         if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
  456.         {
  457.             *rotXCur = imuRotX[imuPointerFront];
  458.             *rotYCur = imuRotY[imuPointerFront];
  459.             *rotZCur = imuRotZ[imuPointerFront];
  460.         } 
  461.         
  462.         else {
  463.             int imuPointerBack = imuPointerFront - 1;
  464. 	    //算一下该点时间戳在本组imu中的位置
  465.             double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
  466.             double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
  467. 	    //按前后百分比赋予旋转量
  468.             *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
  469.             *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
  470.             *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
  471.         }
  472.     }
  473.  
  474.     void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
  475.     {
  476.         *posXCur = 0; *posYCur = 0; *posZCur = 0;
  477.         // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.
  478. //         if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
  479. //             return;
  480. // 
  481. //         float ratio = relTime / (timeScanNext - timeScanCur);
  482. // 
  483. //         *posXCur = ratio * odomIncreX;
  484. //         *posYCur = ratio * odomIncreY;
  485. //         *posZCur = ratio * odomIncreZ;
  486.     }
  487.  
  488.     //这个类型是pcl::PointXYZI
  489.     PointType deskewPoint(PointType *point, double relTime)
  490.     {
  491.         //这个来源于上文的时间戳通道和imu可用判断，没有或是不可用则返回点
  492. 	if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
  493.             return *point;
  494.  
  495. 	//点的时间等于scan时间加relTime（后文的laserCloudIn->points[i].time）
  496.         double pointTime = timeScanCur + relTime;
  497.  
  498. 	//根据时间戳插值获取imu计算的旋转量与位置量（注意imu计算的）
  499.         float rotXCur, rotYCur, rotZCur;
  500.         findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
  501.  
  502.         float posXCur, posYCur, posZCur;
  503.         findPosition(relTime, &posXCur, &posYCur, &posZCur);
  504.  
  505. 	//这里的firstPointFlag来源于resetParameters函数，而resetParameters函数每次ros调用cloudHandler都会启动
  506. 	//也就是求扫描第一个点时lidar的世界坐标系下变换矩阵的逆
  507.         if (firstPointFlag == true)
  508.         {
  509.             transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
  510.             firstPointFlag = false;
  511.         }
  512.  
  513.         // transform points to start
  514.         //扫描当前点时lidar的世界坐标系下变换矩阵
  515.         Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
  516.         //扫描该点相对扫描本次scan第一个点的lidar变换矩阵=第一个点时lidar世界坐标系下变换矩阵的逆×当前点时lidar世界坐标系下变换矩阵
  517. 	Eigen::Affine3f transBt = transStartInverse * transFinal;
  518.  
  519. 	//根据lidar位姿变换，修正点云位置
  520.         PointType newPoint;
  521.         newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
  522.         newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
  523.         newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
  524.         newPoint.intensity = point->intensity;
  525.  
  526.         return newPoint;
  527.     }
  528.  
  529.     void projectPointCloud()
  530.     {
  531.         int cloudSize = laserCloudIn->points.size();
  532.         // range image projection
  533.         for (int i = 0; i < cloudSize; ++i)
  534.         {
  535.             PointType thisPoint;
  536.             thisPoint.x = laserCloudIn->points[i].x;
  537.             thisPoint.y = laserCloudIn->points[i].y;
  538.             thisPoint.z = laserCloudIn->points[i].z;
  539.             thisPoint.intensity = laserCloudIn->points[i].intensity;
  540.  
  541.             int rowIdn = laserCloudIn->points[i].ring;
  542. 	    
  543. 	    //0--N_SCAN内，且是整数
  544.             if (rowIdn < 0 || rowIdn >= N_SCAN)
  545.                 continue;
  546.  
  547.             if (rowIdn % downsampleRate != 0)
  548.                 continue;
  549.  
  550. 	    //仰角
  551.             float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
  552.  
  553. 	    //每线Horizon_SCAN（1800）的点，占据360度除一下
  554.             static float ang_res_x = 360.0/float(Horizon_SCAN);
  555.             int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
  556.             if (columnIdn >= Horizon_SCAN)
  557.                 columnIdn -= Horizon_SCAN;
  558.  
  559. 	    //如果线数不正确
  560.             if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
  561.                 continue;
  562.  
  563. 	    //该点云点距离光心距离
  564.             float range = pointDistance(thisPoint);
  565.             
  566.             if (range < 1.0)
  567.                 continue;
  568.  
  569. 	    //如果没有初始赋值
  570.             if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
  571.                 continue;
  572.  
  573.             // for the amsterdam dataset
  574.             // if (range < 6.0 && rowIdn <= 7 && (columnIdn >= 1600 || columnIdn <= 200))
  575.             //     continue;
  576.             // if (thisPoint.z < -2.0)
  577.             //     continue;
  578.  
  579.             thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne
  580.             // thisPoint = deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0); // Ouster
  581.  
  582. 	    //计算各点距离
  583.             rangeMat.at<float>(rowIdn, columnIdn) = pointDistance(thisPoint);
  584.  
  585.             int index = columnIdn  + rowIdn * Horizon_SCAN;
  586.             fullCloud->points[index] = thisPoint;
  587.         }
  588.     }
  589.  
  590.     void cloudExtraction()
  591.     {
  592.         int count = 0;
  593.         // extract segmented cloud for lidar odometry
  594.         for (int i = 0; i < N_SCAN; ++i)
  595.         {
  596.             cloudInfo.startRingIndex[i] = count - 1 + 5;
  597.  
  598.             for (int j = 0; j < Horizon_SCAN; ++j)
  599.             {
  600.                 if (rangeMat.at<float>(i,j) != FLT_MAX)
  601.                 {
  602.                     // mark the points' column index for marking occlusion later
  603.                     cloudInfo.pointColInd[count] = j;
  604.                     // save range info
  605.                     cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
  606.                     // save extracted cloud
  607.                     extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
  608.                     // size of extracted cloud
  609.                     ++count;
  610.                 }
  611.             }
  612.             cloudInfo.endRingIndex[i] = count -1 - 5;
  613.         }
  614.     }
  615.     
  616.     void publishClouds()
  617.     {
  618.         cloudInfo.header = cloudHeader;
  619. 	//publishCloud在h文件里
  620.         cloudInfo.cloud_deskewed  = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, "base_link");
  621.         pubLaserCloudInfo.publish(cloudInfo);
  622.     }
  623. };
  624.  
  625. int main(int argc, char** argv)
  626. {
  627.     ros::init(argc, argv, "lio_sam");
  628.  
  629.     ImageProjection IP;
  630.     
  631.     ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
  632.  
  633.     ros::MultiThreadedSpinner spinner(3);
  634.     spinner.spin();
  635.     
  636.     return 0;
  637. }

featureExtraction.cpp

  1. #include "utility.h"
  2. #include "lio_sam/cloud_info.h"
  3.  
  4. //曲率值和序号
  5. struct smoothness_t{ 
  6.     float value;
  7.     size_t ind;
  8. };
  9.  
  10. //曲率值对比
  11. struct by_value{ 
  12.     bool operator()(smoothness_t const &left, smoothness_t const &right) { 
  13.         return left.value < right.value;
  14.     }
  15. };
  16.  
  17. class FeatureExtraction : public ParamServer
  18. {
  19.  
  20. public:
  21.  
  22.     ros::Subscriber subLaserCloudInfo;
  23.  
  24.     ros::Publisher pubLaserCloudInfo;
  25.     ros::Publisher pubCornerPoints;
  26.     ros::Publisher pubSurfacePoints;
  27.  
  28.     pcl::PointCloud<PointType>::Ptr extractedCloud;
  29.     pcl::PointCloud<PointType>::Ptr cornerCloud;
  30.     pcl::PointCloud<PointType>::Ptr surfaceCloud;
  31.  
  32.     pcl::VoxelGrid<PointType> downSizeFilter;
  33.  
  34.     lio_sam::cloud_info cloudInfo;
  35.     std_msgs::Header cloudHeader;
  36.  
  37.     //曲率存放
  38.     std::vector<smoothness_t> cloudSmoothness;
  39.     float *cloudCurvature;
  40.     int *cloudNeighborPicked;
  41.     int *cloudLabel;
  42.  
  43.     FeatureExtraction()
  44.     {
  45.         subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 10, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
  46.  
  47.         pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 10);
  48.         pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);
  49.         pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);
  50.         
  51.         initializationValue();
  52.     }
  53.  
  54.     void initializationValue()
  55.     {
  56.         cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
  57.  
  58.         downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);
  59.  
  60.         extractedCloud.reset(new pcl::PointCloud<PointType>());
  61.         cornerCloud.reset(new pcl::PointCloud<PointType>());
  62.         surfaceCloud.reset(new pcl::PointCloud<PointType>());
  63.  
  64.         cloudCurvature = new float[N_SCAN*Horizon_SCAN];
  65.         cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
  66.         cloudLabel = new int[N_SCAN*Horizon_SCAN];
  67.     }
  68.  
  69.     void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
  70.     {
  71.         cloudInfo = *msgIn; // new cloud info
  72.         cloudHeader = msgIn->header; // new cloud header
  73.         pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
  74.  
  75. 	//10点算曲率
  76.         calculateSmoothness();
  77.  
  78. 	//相邻两点距光心差距大则设为不进行特征提取的状态，差距过大，周围点也设置为不进行特征提取的状态
  79.         markOccludedPoints();
  80.  
  81. 	//对于scan中的棱点和面点进行提取
  82.         extractFeatures();
  83.  
  84.         publishFeatureCloud();
  85.     }
  86.  
  87.     //10点算曲率
  88.     void calculateSmoothness()
  89.     {
  90.         int cloudSize = extractedCloud->points.size();
  91.         for (int i = 5; i < cloudSize - 5; i++)
  92.         {
  93.             float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
  94.                             + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
  95.                             + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
  96.                             + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
  97.                             + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
  98.                             + cloudInfo.pointRange[i+5];            
  99.  
  100.             cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
  101.  
  102.             cloudNeighborPicked[i] = 0;
  103.             cloudLabel[i] = 0;
  104.             // cloudSmoothness for sorting
  105.             cloudSmoothness[i].value = cloudCurvature[i];
  106.             cloudSmoothness[i].ind = i;
  107.         }
  108.     }
  109.  
  110.     //相邻两点距光心差距大则设为不进行特征提取的状态，差距过大，周围点也设置为不进行特征提取的状态
  111.     void markOccludedPoints()
  112.     {
  113.         int cloudSize = extractedCloud->points.size();
  114.         // mark occluded points and parallel beam points
  115.         for (int i = 5; i < cloudSize - 6; ++i)
  116.         {
  117.             // occluded points
  118.             float depth1 = cloudInfo.pointRange[i];
  119.             float depth2 = cloudInfo.pointRange[i+1];
  120.             int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
  121.  
  122.             if (columnDiff < 10){
  123.                 // 10 pixel diff in range image
  124.                 if (depth1 - depth2 > 0.3){
  125.                     cloudNeighborPicked[i - 5] = 1;
  126.                     cloudNeighborPicked[i - 4] = 1;
  127.                     cloudNeighborPicked[i - 3] = 1;
  128.                     cloudNeighborPicked[i - 2] = 1;
  129.                     cloudNeighborPicked[i - 1] = 1;
  130.                     cloudNeighborPicked[i] = 1;
  131.                 }else if (depth2 - depth1 > 0.3){
  132.                     cloudNeighborPicked[i + 1] = 1;
  133.                     cloudNeighborPicked[i + 2] = 1;
  134.                     cloudNeighborPicked[i + 3] = 1;
  135.                     cloudNeighborPicked[i + 4] = 1;
  136.                     cloudNeighborPicked[i + 5] = 1;
  137.                     cloudNeighborPicked[i + 6] = 1;
  138.                 }
  139.             }
  140.             // parallel beam
  141.             float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
  142.             float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
  143.  
  144.             if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
  145.                 cloudNeighborPicked[i] = 1;
  146.         }
  147.     }
  148.  
  149.     //对于scan中的棱点和面点进行提取
  150.     void extractFeatures()
  151.     {
  152.         cornerCloud->clear();
  153.         surfaceCloud->clear();
  154.  
  155.         pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
  156.         pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());
  157.  
  158. 	//和loam一样，对于每条线分为六部分，各自取20棱点和全部面点
  159.         for (int i = 0; i < N_SCAN; i++)
  160.         {
  161.             surfaceCloudScan->clear();
  162.  
  163.             for (int j = 0; j < 6; j++)
  164.             {
  165.  
  166.                 //获取六段的初始截止位置
  167. 		int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
  168.                 int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
  169.  
  170.                 if (sp >= ep)
  171.                     continue;
  172.  
  173. 		//按曲率排序
  174.                 std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
  175.  
  176.                 int largestPickedNum = 0;
  177.                 for (int k = ep; k >= sp; k--)
  178.                 {
  179.                     int ind = cloudSmoothness[k].ind;
  180. 		    //可特征提取的状态+曲率大于0.1，保存20个棱点，周围点除非光心距差距极大，不然都设置为可特征提取的状态
  181. 		    //达到均匀分布的目的
  182.                     if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
  183.                     {
  184.                         largestPickedNum++;
  185.                         if (largestPickedNum <= 20){
  186.                             cloudLabel[ind] = 1;
  187.                             cornerCloud->push_back(extractedCloud->points[ind]);
  188.                         } else {
  189.                             break;
  190.                         }
  191.  
  192.                         cloudNeighborPicked[ind] = 1;
  193.                         for (int l = 1; l <= 5; l++)
  194.                         {
  195.                             int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
  196.                             if (columnDiff > 10)
  197.                                 break;
  198.                             cloudNeighborPicked[ind + l] = 1;
  199.                         }
  200.                         for (int l = -1; l >= -5; l--)
  201.                         {
  202.                             int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
  203.                             if (columnDiff > 10)
  204.                                 break;
  205.                             cloudNeighborPicked[ind + l] = 1;
  206.                         }
  207.                     }
  208.                 }
  209.  
  210.                 //可特征提取的状态+曲率小于0.1，保存全部面点，周围点除非光心距差距极大，不然都设置为可特征提取的状态
  211. 		//达到均匀分布的目的
  212.                 for (int k = sp; k <= ep; k++)
  213.                 {
  214.                     int ind = cloudSmoothness[k].ind;
  215.                     if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
  216.                     {
  217.  
  218.                         cloudLabel[ind] = -1;
  219.                         cloudNeighborPicked[ind] = 1;
  220.  
  221.                         for (int l = 1; l <= 5; l++) {
  222.  
  223.                             int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
  224.                             if (columnDiff > 10)
  225.                                 break;
  226.  
  227.                             cloudNeighborPicked[ind + l] = 1;
  228.                         }
  229.                         for (int l = -1; l >= -5; l--) {
  230.  
  231.                             int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
  232.                             if (columnDiff > 10)
  233.                                 break;
  234.  
  235.                             cloudNeighborPicked[ind + l] = 1;
  236.                         }
  237.                     }
  238.                 }
  239.  
  240.                 for (int k = sp; k <= ep; k++)
  241.                 {
  242.                     if (cloudLabel[k] <= 0){
  243.                         surfaceCloudScan->push_back(extractedCloud->points[k]);
  244.                     }
  245.                 }
  246.             }
  247.  
  248.             surfaceCloudScanDS->clear();
  249.             downSizeFilter.setInputCloud(surfaceCloudScan);
  250.             downSizeFilter.filter(*surfaceCloudScanDS);
  251.  
  252.             *surfaceCloud += *surfaceCloudScanDS;
  253.         }
  254.     }
  255.  
  256.     void freeCloudInfoMemory()
  257.     {
  258.         cloudInfo.startRingIndex.clear();
  259.         cloudInfo.endRingIndex.clear();
  260.         cloudInfo.pointColInd.clear();
  261.         cloudInfo.pointRange.clear();
  262.     }
  263.  
  264.     void publishFeatureCloud()
  265.     {
  266.         // free cloud info memory
  267.         freeCloudInfoMemory();
  268.         // save newly extracted features
  269.         cloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, "base_link");
  270.         cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");
  271.         // publish to mapOptimization
  272.         pubLaserCloudInfo.publish(cloudInfo);
  273.     }
  274. };
  275.  
  276.  
  277. int main(int argc, char** argv)
  278. {
  279.     ros::init(argc, argv, "lio_sam");
  280.  
  281.     FeatureExtraction FE;
  282.  
  283.     ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
  284.    
  285.     ros::spin();
  286.  
  287.     return 0;
  288. }

imuPreintegration.cpp

发现已经有写的更好的博客了，引用一下他的注释

  1. #include "utility.h"
  2.  
  3. #include <gtsam/geometry/Rot3.h>
  4. #include <gtsam/geometry/Pose3.h>
  5. #include <gtsam/slam/PriorFactor.h>
  6. #include <gtsam/slam/BetweenFactor.h>
  7. #include <gtsam/navigation/GPSFactor.h>
  8. #include <gtsam/navigation/ImuFactor.h>
  9. #include <gtsam/navigation/CombinedImuFactor.h>
  10. #include <gtsam/nonlinear/NonlinearFactorGraph.h>
  11. #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
  12. #include <gtsam/nonlinear/Marginals.h>
  13. #include <gtsam/nonlinear/Values.h>
  14. #include <gtsam/inference/Symbol.h>
  15.  
  16. #include <gtsam/nonlinear/ISAM2.h>
  17. #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
  18.  
  19. using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
  20. using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
  21. using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
  22.  
  23. class IMUPreintegration : public ParamServer {
  24.  public:
  25.   IMUPreintegration() {
  26.  
  27.     // subscriber 订阅imu数据和激光Odom
  28.     // 业务逻辑都在callback里面写, 两个数据是耦合关系, imu通过激光odom给出优化后的预积分预测
  29.     // odom根据预测的位姿优化、融合出新的odom
  30.     subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &IMUPreintegration::imuHandler, this,
  31.                                             ros::TransportHints().tcpNoDelay());
  32.     subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5,
  33.                                                    &IMUPreintegration::odometryHandler, this,
  34.                                                    ros::TransportHints().tcpNoDelay());
  35.  
  36.     // publisher 发布融合后的imu path和预积分完成优化后预测的odom
  37.     pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
  38.     pubImuPath = nh.advertise<nav_msgs::Path>("lio_sam/imu/path", 1);
  39.  
  40.     map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
  41.  
  42.     // 下面是预积分使用到的gtsam的一些参数配置
  43.     boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
  44.     p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // acc white noise in continuous
  45.     p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
  46.     p->integrationCovariance =
  47.         gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); // error committed in integrating position from velocities
  48.     gtsam::imuBias::ConstantBias
  49.         prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
  50.  
  51.     priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6)
  52.         << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
  53.     priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s
  54.     priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
  55.     correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-2); // meter
  56.     noiseModelBetweenBias =
  57.         (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
  58.  
  59.     // 优化前后的imu
  60.     imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
  61.     imuIntegratorOpt_ =new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
  62.   }
  63.  
  64.   void resetOptimization() {
  65.     // gtsam相关优化参数重置
  66.     gtsam::ISAM2Params optParameters;
  67.     optParameters.relinearizeThreshold = 0.1;
  68.     optParameters.relinearizeSkip = 1;
  69.     optimizer = gtsam::ISAM2(optParameters);
  70.  
  71.     gtsam::NonlinearFactorGraph newGraphFactors;
  72.     graphFactors = newGraphFactors;
  73.  
  74.     gtsam::Values NewGraphValues;
  75.     graphValues = NewGraphValues;
  76.   }
  77.  
  78.   void resetParams() {
  79.     lastImuT_imu = -1;
  80.     doneFirstOpt = false;
  81.     systemInitialized = false;
  82.   }
  83.  
  84.   void odometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg) {
  85.     double currentCorrectionTime = ROS_TIME(odomMsg);
  86.  
  87.     // make sure we have imu data to integrate
  88.     // 保证有imu数据,两个回调函数是互有联系的,
  89.     // 在imu的回调里就强调要完成一次优化才往下执行
  90.     if (imuQueOpt.empty())
  91.       return;
  92.  
  93.     // 从雷达odom中取出位姿数据
  94.     float p_x = odomMsg->pose.pose.position.x;
  95.     float p_y = odomMsg->pose.pose.position.y;
  96.     float p_z = odomMsg->pose.pose.position.z;
  97.     float r_x = odomMsg->pose.pose.orientation.x;
  98.     float r_y = odomMsg->pose.pose.orientation.y;
  99.     float r_z = odomMsg->pose.pose.orientation.z;
  100.     float r_w = odomMsg->pose.pose.orientation.w;
  101.     int currentResetId = round(odomMsg->pose.covariance[0]);
  102.     gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z),
  103.                                           gtsam::Point3(p_x, p_y, p_z));
  104.  
  105.     // correction pose jumped, reset imu pre-integration
  106.     if (currentResetId != imuPreintegrationResetId) {
  107.       resetParams();
  108.       imuPreintegrationResetId = currentResetId;
  109.     }
  110.  
  111.  
  112.     // 0. initialize system
  113.     // 第一帧进来初始化系统
  114.     if (systemInitialized == false) {
  115.       resetOptimization(); // 重置优化参数
  116.  
  117.       // pop old IMU message
  118.       // 去掉一些比较旧的imu数据, 只需要保证雷达odom时间戳在imu队列中间
  119.       // 因为imu是高频数据, 这里是有效的
  120.       while (!imuQueOpt.empty()) {
  121.         if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t) {
  122.           lastImuT_opt = ROS_TIME(&imuQueOpt.front());
  123.           imuQueOpt.pop_front();
  124.         } else
  125.           break;
  126.       }
  127.       // initial pose
  128.       prevPose_ = lidarPose.compose(lidar2Imu); // 雷达odom转到imu系下
  129.       //PriorFactor,包括了位姿、速度和bias
  130.       //加入PriorFactor在图优化中基本都是必需的前提
  131.       gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
  132.       graphFactors.add(priorPose);
  133.       // initial velocity
  134.       prevVel_ = gtsam::Vector3(0, 0, 0);
  135.       gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
  136.       graphFactors.add(priorVel);
  137.       // initial bias
  138.       prevBias_ = gtsam::imuBias::ConstantBias();
  139.       gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
  140.       graphFactors.add(priorBias);
  141.       // add values、
  142.       // 除了因子外, 还要有节点value
  143.       graphValues.insert(X(0), prevPose_);
  144.       graphValues.insert(V(0), prevVel_);
  145.       graphValues.insert(B(0), prevBias_);
  146.       // optimize once
  147.       // 进行一次优化
  148.       optimizer.update(graphFactors, graphValues);
  149.       graphFactors.resize(0);
  150.       graphValues.clear();     //图和节点均清零, 为什么要清零?
  151.  
  152.       // 重置积分器
  153.       imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
  154.       imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
  155.  
  156.       key = 1; // 计数
  157.       systemInitialized = true;
  158.       return;
  159.     }
  160.  
  161.  
  162.     // reset graph for speed
  163.     // key超过设定的100则重置整个图
  164.     // 减小计算压力,保存最后的噪声值
  165.     if (key == 100) {
  166.       // get updated noise before reset
  167.       gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(
  168.           optimizer.marginalCovariance(X(key - 1)));
  169.       gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(
  170.           optimizer.marginalCovariance(V(key - 1)));
  171.       gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(
  172.           optimizer.marginalCovariance(B(key - 1)));
  173.       // reset graph 重置参数
  174.       resetOptimization();
  175.  
  176.       // 重置之后还有类似与初始化的过程 区别在于噪声值不同
  177.       // add pose
  178.       gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
  179.       graphFactors.add(priorPose);
  180.       // add velocity
  181.       gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
  182.       graphFactors.add(priorVel);
  183.       // add bias
  184.       gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
  185.       graphFactors.add(priorBias);
  186.       // add values
  187.       graphValues.insert(X(0), prevPose_);
  188.       graphValues.insert(V(0), prevVel_);
  189.       graphValues.insert(B(0), prevBias_);
  190.       // optimize once
  191.       // 优化一次, 相当于初始化
  192.       optimizer.update(graphFactors, graphValues);
  193.       graphFactors.resize(0);
  194.       graphValues.clear();
  195.  
  196.       key = 1;
  197.     }
  198.  
  199.  
  200.     // 1. integrate imu data and optimize
  201.     // 这里才开始主要的优化流程
  202.     while (!imuQueOpt.empty()) {
  203.       // pop and integrate imu data that is between two optimizations
  204.       // 对两次优化的之间的imu数据进行优化
  205.       sensor_msgs::Imu *thisImu = &imuQueOpt.front(); // 最新的imu数据帧
  206.       double imuTime = ROS_TIME(thisImu);
  207.       if (imuTime < currentCorrectionTime - delta_t) {
  208.         // 求dt,初始是1/500,后续是两帧imu数据的时间差
  209.         double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
  210.  
  211.         // 进行预积分得到新的状态值,注意用到的是imu数据的加速度和角速度
  212.         // 作者要求的9轴imu数据中欧拉角在本程序中没有任何用到, 全在地图优化里用到的
  213.         imuIntegratorOpt_->integrateMeasurement(
  214.             gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y,
  215.                            thisImu->linear_acceleration.z),
  216.             gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y,
  217.                            thisImu->angular_velocity.z), dt);
  218.  
  219.         //在pop出一次数据前保存上一个数据的时间戳
  220.         lastImuT_opt = imuTime;
  221.         imuQueOpt.pop_front();
  222.       } else
  223.         break;
  224.     }
  225.  
  226.     // add imu factor to graph
  227.     // 利用两帧之间的IMU数据完成了预积分后增加imu因子到因子图中
  228.     // add imu factor to graph
  229.     const gtsam::PreintegratedImuMeasurements
  230.         &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imuIntegratorOpt_);
  231.     gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
  232.     graphFactors.add(imu_factor);
  233.     // add imu bias between factor
  234.     graphFactors.add(
  235.         gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
  236.                                                            gtsam::noiseModel::Diagonal::Sigmas(
  237.                                                                sqrt(imuIntegratorOpt_->deltaTij()) *
  238.                                                                    noiseModelBetweenBias)));
  239.     // add pose factor
  240.     //  还加入了pose factor,其实对应于作者论文中的因子图结构
  241.     //  就是与imu因子一起的 Lidar odometry factor
  242.     gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
  243.     gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, correctionNoise);
  244.     graphFactors.add(pose_factor);
  245.     // insert predicted values
  246.     // 插入预测的值 即因子图中x0 x1 x2……节点
  247.     gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
  248.     graphValues.insert(X(key), propState_.pose());
  249.     graphValues.insert(V(key), propState_.v());
  250.     graphValues.insert(B(key), prevBias_);
  251.  
  252.     // optimize 优化后重置
  253.     optimizer.update(graphFactors, graphValues);
  254.     optimizer.update();
  255.     graphFactors.resize(0);
  256.     graphValues.clear();
  257.     // Overwrite the beginning of the preintegration for the next step.
  258.     // 用这次的优化结果重写或者说是覆盖相关初始值, 为下一次优化准备
  259.     gtsam::Values result = optimizer.calculateEstimate();
  260.     prevPose_ = result.at<gtsam::Pose3>(X(key));
  261.     prevVel_ = result.at<gtsam::Vector3>(V(key));
  262.     prevState_ = gtsam::NavState(prevPose_, prevVel_);
  263.     prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
  264.     // Reset the optimization preintegration object.
  265.     imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
  266.     // check optimization
  267.     // 检查是否有失败情况,如有则重置参数
  268.     if (failureDetection(prevVel_, prevBias_)) {
  269.       resetParams();
  270.       return;
  271.     }
  272.  
  273.  
  274.     // 2. after optiization, re-propagate imu odometry preintegration
  275.     // 为了维持实时性imuIntegrateImu就得在每次odom触发优化后立刻获取最新的bias,
  276.     // 同时对imu测量值imuQueImu执行bias改变的状态重传播处理, 这样可以最大限度的保证实时性和准确性?
  277.     prevStateOdom = prevState_;
  278.     prevBiasOdom = prevBias_;
  279.     // first pop imu message older than current correction data
  280.     // 去除旧的imu数据
  281.     double lastImuQT = -1;
  282.     while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t) {
  283.       lastImuQT = ROS_TIME(&imuQueImu.front());
  284.       imuQueImu.pop_front();
  285.     }
  286.     // repropogate
  287.     // 重传播？
  288.     if (!imuQueImu.empty()) {
  289.       // reset bias use the newly optimized bias
  290.       // 使用最新的优化后的bias更新bias值
  291.       imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
  292.       // integrate imu message from the beginning of this optimization
  293.       // 利用imuQueImu中的数据进行预积分,主要区别旧在于上一行的更新了bias
  294.       for (int i = 0; i < (int) imuQueImu.size(); ++i) {
  295.         sensor_msgs::Imu *thisImu = &imuQueImu[i];
  296.         double imuTime = ROS_TIME(thisImu); // 时间戳
  297.         double dt = (lastImuQT < 0) ? (1.0 / 500.0) : (imuTime - lastImuQT);
  298.  
  299.         // 进行预计分
  300.         imuIntegratorImu_->integrateMeasurement(
  301.             gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y,
  302.                            thisImu->linear_acceleration.z),
  303.             gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y,
  304.                            thisImu->angular_velocity.z), dt);
  305.         lastImuQT = imuTime;
  306.       }
  307.     }
  308.  
  309.     ++key;
  310.     doneFirstOpt = true;
  311.   }
  312.  
  313.   bool failureDetection(const gtsam::Vector3 &velCur, const gtsam::imuBias::ConstantBias &biasCur) {
  314.     // 检测预计分失败的函数, 即时爆出错误,重置积分器
  315.     Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
  316.     if (vel.norm() > 10) {
  317.       ROS_WARN("Large velocity, reset IMU-preintegration!");
  318.       return true;
  319.     }
  320.  
  321.     Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
  322.     Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
  323.     if (ba.norm() > 0.1 || bg.norm() > 0.1) {
  324.       ROS_WARN("Large bias, reset IMU-preintegration!");
  325.       return true;
  326.     }
  327.  
  328.     return false;
  329.   }
  330.  
  331.   void imuHandler(const sensor_msgs::Imu::ConstPtr &imu_raw) {
  332.     // imu数据转换到雷达坐标系下
  333.     sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
  334.     // publish static tf map->odom
  335.     tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, "map", "odom"));
  336.  
  337.     // 两个双端队列分别装着优化前后的imu数据
  338.     imuQueOpt.push_back(thisImu);
  339.     imuQueImu.push_back(thisImu);
  340.  
  341.     // 检查有没有执行过一次优化,这里需要先在odomhandler中优化一次后再进行该函数后续的工作
  342.     if (doneFirstOpt == false)
  343.       return;
  344.  
  345.     // 获得时间间隔, 第一次为1/500,之后是两次imuTime间的差
  346.     double imuTime = ROS_TIME(&thisImu);
  347.     double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
  348.     lastImuT_imu = imuTime;
  349.  
  350.     // integrate this single imu message
  351.     // 进行预积分
  352.     imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y,
  353.                                                            thisImu.linear_acceleration.z),
  354.                                             gtsam::Vector3(thisImu.angular_velocity.x,
  355.                                                            thisImu.angular_velocity.y,
  356.                                                            thisImu.angular_velocity.z), dt);
  357.  
  358.     // predict odometry
  359.     // 根据预计分结果, 预测odom
  360.     gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
  361.  
  362.     // publish odometry 发布新的odom
  363.     nav_msgs::Odometry odometry;
  364.     odometry.header.stamp = thisImu.header.stamp;
  365.     odometry.header.frame_id = "odom";
  366.     odometry.child_frame_id = "odom_imu";
  367.  
  368.     // transform imu pose to ldiar imu位姿转到雷达系
  369.     // 预测值currentState获得imu位姿, 再由imu到雷达变换, 获得雷达位姿
  370.     gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
  371.     gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
  372.  
  373.     odometry.pose.pose.position.x = lidarPose.translation().x();
  374.     odometry.pose.pose.position.y = lidarPose.translation().y();
  375.     odometry.pose.pose.position.z = lidarPose.translation().z();
  376.     odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
  377.     odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
  378.     odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
  379.     odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
  380.  
  381.     odometry.twist.twist.linear.x = currentState.velocity().x();
  382.     odometry.twist.twist.linear.y = currentState.velocity().y();
  383.     odometry.twist.twist.linear.z = currentState.velocity().z();
  384.     odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
  385.     odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
  386.     odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
  387.     odometry.pose.covariance[0] = double(imuPreintegrationResetId);
  388.     pubImuOdometry.publish(odometry);
  389.  
  390.     // publish imu path
  391.     // 预测的imu path, 只保留3s内的轨迹
  392.     static nav_msgs::Path imuPath;
  393.     static double last_path_time = -1;
  394.     if (imuTime - last_path_time > 0.1) {
  395.       last_path_time = imuTime;
  396.       geometry_msgs::PoseStamped pose_stamped;
  397.       pose_stamped.header.stamp = thisImu.header.stamp;
  398.       pose_stamped.header.frame_id = "odom";
  399.       pose_stamped.pose = odometry.pose.pose;
  400.       imuPath.poses.push_back(pose_stamped);
  401.       while (!imuPath.poses.empty() &&
  402.           abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0)
  403.         imuPath.poses.erase(imuPath.poses.begin());
  404.       if (pubImuPath.getNumSubscribers() != 0) {
  405.         imuPath.header.stamp = thisImu.header.stamp;
  406.         imuPath.header.frame_id = "odom";
  407.         pubImuPath.publish(imuPath);
  408.       }
  409.     }
  410.  
  411.     // publish transformation
  412.     // 发布odom->base_link的变换
  413.     tf::Transform tCur;
  414.     tf::poseMsgToTF(odometry.pose.pose, tCur);
  415.     tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, thisImu.header.stamp, "odom", "base_link");
  416.     tfOdom2BaseLink.sendTransform(odom_2_baselink);
  417.   }
  418.  
  419.  public:
  420.   ros::Subscriber subImu;
  421.   ros::Subscriber subOdometry;
  422.   ros::Publisher pubImuOdometry;
  423.   ros::Publisher pubImuPath;
  424.  
  425.   // map -> odom
  426.   tf::Transform map_to_odom;
  427.   tf::TransformBroadcaster tfMap2Odom;
  428.   // odom -> base_link
  429.   tf::TransformBroadcaster tfOdom2BaseLink;
  430.  
  431.   bool systemInitialized = false;
  432.  
  433.   gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
  434.   gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
  435.   gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
  436.   gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
  437.   gtsam::Vector noiseModelBetweenBias;
  438.  
  439.   gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
  440.   gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
  441.  
  442.   std::deque<sensor_msgs::Imu> imuQueOpt;
  443.   std::deque<sensor_msgs::Imu> imuQueImu;
  444.  
  445.   gtsam::Pose3 prevPose_;
  446.   gtsam::Vector3 prevVel_;
  447.   gtsam::NavState prevState_;
  448.   gtsam::imuBias::ConstantBias prevBias_;
  449.  
  450.   gtsam::NavState prevStateOdom;
  451.   gtsam::imuBias::ConstantBias prevBiasOdom;
  452.  
  453.   bool doneFirstOpt = false;
  454.   double lastImuT_imu = -1;
  455.   double lastImuT_opt = -1;
  456.  
  457.   gtsam::ISAM2 optimizer;
  458.   gtsam::NonlinearFactorGraph graphFactors;
  459.   gtsam::Values graphValues;
  460.  
  461.   const double delta_t = 0;
  462.  
  463.   int key = 1;
  464.   int imuPreintegrationResetId = 0;
  465.  
  466.   // 雷达->imu外餐
  467.   gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0),
  468.                                         gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
  469.   gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0),
  470.                                         gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));;
  471.  
  472. };
  473.  
  474. int main(int argc, char **argv) {
  475.   ros::init(argc, argv, "roboat_loam");
  476.  
  477.   IMUPreintegration ImuP;
  478.  
  479.   ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
  480.  
  481.   ros::spin();
  482.  
  483.   return 0;
  484. }

mapOptmization.cpp

  1. #include "utility.h"
  2. #include "lio_sam/cloud_info.h"
  3.  
  4. #include <gtsam/geometry/Rot3.h>
  5. #include <gtsam/geometry/Pose3.h>
  6. #include <gtsam/slam/PriorFactor.h>
  7. #include <gtsam/slam/BetweenFactor.h>
  8. #include <gtsam/navigation/GPSFactor.h>
  9. #include <gtsam/navigation/ImuFactor.h>
  10. #include <gtsam/navigation/CombinedImuFactor.h>
  11. #include <gtsam/nonlinear/NonlinearFactorGraph.h>
  12. #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
  13. #include <gtsam/nonlinear/Marginals.h>
  14. #include <gtsam/nonlinear/Values.h>
  15. #include <gtsam/inference/Symbol.h>
  16.  
  17. #include <gtsam/nonlinear/ISAM2.h>
  18.  
  19. using namespace gtsam;
  20.  
  21. using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
  22. using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
  23. using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
  24. using symbol_shorthand::G; // GPS pose
  25.  
  26. /*
  27.     * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
  28.     */
  29. struct PointXYZIRPYT {
  30.   PCL_ADD_POINT4D
  31.  
  32.   PCL_ADD_INTENSITY;                  // preferred way of adding a XYZ+padding
  33.   float roll;
  34.   float pitch;
  35.   float yaw;
  36.   double time;
  37.  
  38.   EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
  39. } EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment
  40.  
  41. POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
  42.                                    (float, x, x)(float, y, y)
  43.                                        (float, z, z)(float, intensity, intensity)
  44.                                        (float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)
  45.                                        (double, time, time))
  46.  
  47. typedef PointXYZIRPYT PointTypePose;
  48.  
  49. class mapOptimization : public ParamServer {
  50.  
  51.  public:
  52.  
  53.   mapOptimization() {
  54.     ISAM2Params parameters;
  55.     parameters.relinearizeThreshold = 0.1;
  56.     parameters.relinearizeSkip = 1;
  57.     isam = new ISAM2(parameters);
  58.  
  59.     // subscriber 主要订阅分类好的cloud_info以及gps,用于后端优化和回环检测,
  60.     // 注意gps接受的是nav_msgs::Odometry消息, 是通过robot_localization节点融合imu和gps数据得到的
  61.     //  callback里面只是装数据到队列
  62.     subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1,
  63.                                                           &mapOptimization::laserCloudInfoHandler, this,
  64.                                                           ros::TransportHints().tcpNoDelay());
  65.     subGPS = nh.subscribe<nav_msgs::Odometry>(gpsTopic, 200, &mapOptimization::gpsHandler, this,
  66.                                               ros::TransportHints().tcpNoDelay());
  67.  
  68.     // publisher 发布一些odometry之类的
  69.     pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
  70.     pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1); // 全局地图
  71.     pubOdomAftMappedROS = nh.advertise<nav_msgs::Odometry>("lio_sam/mapping/odometry", 1); // 优化后的odom
  72.     pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
  73.  
  74.     // 回环检测相关的一些历史帧
  75.     pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
  76.     pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
  77.  
  78.     //  local map
  79.     pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
  80.     pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
  81.     pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);
  82.  
  83.     // 不同的特征进行滤波
  84.     downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
  85.     downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
  86.     downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
  87.     downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity,
  88.                                                   surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
  89.  
  90.     allocateMemory();
  91.   }
  92.  
  93.   void allocateMemory() {
  94.     // 初始化一些参数
  95.     cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
  96.     cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
  97.  
  98.     kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
  99.     kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
  100.  
  101.     laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
  102.     laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
  103.     laserCloudCornerLastDS.reset(
  104.         new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
  105.     laserCloudSurfLastDS.reset(
  106.         new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
  107.  
  108.     laserCloudOri.reset(new pcl::PointCloud<PointType>());
  109.     coeffSel.reset(new pcl::PointCloud<PointType>());
  110.  
  111.     laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
  112.     coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
  113.     laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
  114.     laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
  115.     coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
  116.     laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
  117.  
  118.     std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
  119.     std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
  120.  
  121.     laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
  122.     laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
  123.     laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
  124.     laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
  125.  
  126.     kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
  127.     kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
  128.  
  129.     latestKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
  130.     nearHistoryKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
  131.  
  132.     for (int i = 0; i < 6; ++i) {
  133.       transformTobeMapped[i] = 0;
  134.     }
  135.  
  136.     matP.setZero();
  137.   }
  138.  
  139.   void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn) {
  140.     // extract time stamp
  141.     timeLaserInfoStamp = msgIn->header.stamp;
  142.     timeLaserCloudInfoLast = msgIn->header.stamp.toSec();
  143.  
  144.     // extract info and feature cloud
  145.     // corner和surface点云
  146.     cloudInfo = *msgIn;
  147.     pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
  148.     pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
  149.  
  150.     std::lock_guard<std::mutex> lock(mtx);
  151.  
  152.     // 0.15s更新一下map
  153.     if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval) {
  154.  
  155.       timeLastProcessing = timeLaserCloudInfoLast;
  156.  
  157.       updateInitialGuess(); // imu预积分更新初始位姿
  158.  
  159.       extractSurroundingKeyFrames(); // 从关键帧里面提取附近回环候选帧
  160.  
  161.       downsampleCurrentScan();  // 不同的leaf size进行下采样，主要是corner cloud和surface cloud
  162.  
  163.       scan2MapOptimization(); // 构建点到平面、点到直线的残差, 用高斯牛顿法进行优化
  164.  
  165.       saveKeyFramesAndFactor(); // 添加factor,保存key pose之类的
  166.  
  167.       correctPoses();  // 更新位姿
  168.  
  169.       // publish odom
  170.       publishOdometry(); // 发布增量平滑后的odom
  171.  
  172.       publishFrames();  // 发布一些关键帧点云之类的
  173.     }
  174.   }
  175.  
  176.   void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg) {
  177.     gpsQueue.push_back(*gpsMsg);
  178.   }
  179.  
  180.   void pointAssociateToMap(PointType const *const pi, PointType *const po) {
  181.     po->x = transPointAssociateToMap(0, 0) * pi->x + transPointAssociateToMap(0, 1) * pi->y +
  182.         transPointAssociateToMap(0, 2) * pi->z + transPointAssociateToMap(0, 3);
  183.     po->y = transPointAssociateToMap(1, 0) * pi->x + transPointAssociateToMap(1, 1) * pi->y +
  184.         transPointAssociateToMap(1, 2) * pi->z + transPointAssociateToMap(1, 3);
  185.     po->z = transPointAssociateToMap(2, 0) * pi->x + transPointAssociateToMap(2, 1) * pi->y +
  186.         transPointAssociateToMap(2, 2) * pi->z + transPointAssociateToMap(2, 3);
  187.     po->intensity = pi->intensity;
  188.   }
  189.  
  190.   pcl::PointCloud<PointType>::Ptr
  191.   transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn) {
  192.     pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  193.  
  194.     PointType *pointFrom;
  195.  
  196.     int cloudSize = cloudIn->size();
  197.     cloudOut->resize(cloudSize);
  198.  
  199.     Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z,
  200.                                                       transformIn->roll, transformIn->pitch, transformIn->yaw);
  201.  
  202.     for (int i = 0; i < cloudSize; ++i) {
  203.  
  204.       pointFrom = &cloudIn->points[i];
  205.       cloudOut->points[i].x =
  206.           transCur(0, 0) * pointFrom->x + transCur(0, 1) * pointFrom->y + transCur(0, 2) * pointFrom->z +
  207.               transCur(0, 3);
  208.       cloudOut->points[i].y =
  209.           transCur(1, 0) * pointFrom->x + transCur(1, 1) * pointFrom->y + transCur(1, 2) * pointFrom->z +
  210.               transCur(1, 3);
  211.       cloudOut->points[i].z =
  212.           transCur(2, 0) * pointFrom->x + transCur(2, 1) * pointFrom->y + transCur(2, 2) * pointFrom->z +
  213.               transCur(2, 3);
  214.       cloudOut->points[i].intensity = pointFrom->intensity;
  215.     }
  216.     return cloudOut;
  217.   }
  218.  
  219.   gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint) {
  220.     return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
  221.                         gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
  222.   }
  223.  
  224.   gtsam::Pose3 trans2gtsamPose(float transformIn[]) {
  225.     return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
  226.                         gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
  227.   }
  228.  
  229.   Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint) {
  230.     return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch,
  231.                                   thisPoint.yaw);
  232.   }
  233.  
  234.   Eigen::Affine3f trans2Affine3f(float transformIn[]) {
  235.     return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1],
  236.                                   transformIn[2]);
  237.   }
  238.  
  239.   PointTypePose trans2PointTypePose(float transformIn[]) {
  240.     PointTypePose thisPose6D;
  241.     thisPose6D.x = transformIn[3];
  242.     thisPose6D.y = transformIn[4];
  243.     thisPose6D.z = transformIn[5];
  244.     thisPose6D.roll = transformIn[0];
  245.     thisPose6D.pitch = transformIn[1];
  246.     thisPose6D.yaw = transformIn[2];
  247.     return thisPose6D;
  248.   }
  249.  
  250.   void visualizeGlobalMapThread() {
  251.     // 按一定的频率发布全局地图
  252.     ros::Rate rate(0.2);
  253.     while (ros::ok()) {
  254.       rate.sleep();
  255.       publishGlobalMap();
  256.     }
  257.  
  258.     //  下面是保存各种地图
  259.     if (savePCD == false)
  260.       return;
  261.  
  262.     cout << "****************************************************" << endl;
  263.     cout << "Saving map to pcd files ..." << endl;
  264.     // create directory and remove old files;
  265.     savePCDDirectory = std::getenv("HOME") + savePCDDirectory;
  266.     int unused = system((std::string("exec rm -r ") + savePCDDirectory).c_str());
  267.     unused = system((std::string("mkdir ") + savePCDDirectory).c_str());
  268.     // save key frame transformations
  269.     pcl::io::savePCDFileASCII(savePCDDirectory + "trajectory.pcd", *cloudKeyPoses3D);
  270.     pcl::io::savePCDFileASCII(savePCDDirectory + "transformations.pcd", *cloudKeyPoses6D);
  271.     // extract global point cloud map
  272.     pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
  273.     pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());
  274.     pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
  275.     pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
  276.     pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
  277.     for (int i = 0; i < cloudKeyPoses3D->size(); i++) {
  278.       *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
  279.       *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
  280.       cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size()
  281.            << " ...";
  282.     }
  283.     // down-sample and save corner cloud
  284.     downSizeFilterCorner.setInputCloud(globalCornerCloud);
  285.     downSizeFilterCorner.filter(*globalCornerCloudDS);
  286.     pcl::io::savePCDFileASCII(savePCDDirectory + "cloudCorner.pcd", *globalCornerCloudDS);
  287.     // down-sample and save surf cloud
  288.     downSizeFilterSurf.setInputCloud(globalSurfCloud);
  289.     downSizeFilterSurf.filter(*globalSurfCloudDS);
  290.     pcl::io::savePCDFileASCII(savePCDDirectory + "cloudSurf.pcd", *globalSurfCloudDS);
  291.     // down-sample and save global point cloud map
  292.     *globalMapCloud += *globalCornerCloud;
  293.     *globalMapCloud += *globalSurfCloud;
  294.     pcl::io::savePCDFileASCII(savePCDDirectory + "cloudGlobal.pcd", *globalMapCloud);
  295.     cout << "****************************************************" << endl;
  296.     cout << "Saving map to pcd files completed" << endl;
  297.   }
  298.  
  299.   void publishGlobalMap() {
  300.     if (pubLaserCloudSurround.getNumSubscribers() == 0)
  301.       return;
  302.  
  303.     // cloudKeyPoses3Dc存的是关键帧的位姿
  304.     if (cloudKeyPoses3D->points.empty() == true)
  305.       return;
  306.  
  307.     pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
  308.     pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
  309.     pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
  310.     pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
  311.     pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());
  312.  
  313.     // kd-tree to find near key frames to visualize
  314.     std::vector<int> pointSearchIndGlobalMap;
  315.     std::vector<float> pointSearchSqDisGlobalMap;
  316.     // search near key frames to visualize
  317.     mtx.lock();
  318.     kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
  319.     kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius,
  320.                                   pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
  321.     mtx.unlock();
  322.  
  323.     // 找到附近的点云帧并发布出来
  324.     for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i)
  325.       globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
  326.     // downsample near selected key frames
  327.     pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
  328.     downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity,
  329.                                                 globalMapVisualizationPoseDensity,
  330.                                                 globalMapVisualizationPoseDensity); // for global map visualization
  331.     downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
  332.     downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
  333.  
  334.     // extract visualized and downsampled key frames
  335.     for (int i = 0; i < globalMapKeyPosesDS->size(); ++i) {
  336.       if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) >
  337.           globalMapVisualizationSearchRadius)
  338.         continue;
  339.       int thisKeyInd = (int) globalMapKeyPosesDS->points[i].intensity;
  340.       *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],
  341.                                                   &cloudKeyPoses6D->points[thisKeyInd]);
  342.       *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],
  343.                                                   &cloudKeyPoses6D->points[thisKeyInd]);
  344.     }
  345.     // downsample visualized points
  346.     pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
  347.     downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize,
  348.                                                  globalMapVisualizationLeafSize); // for global map visualization
  349.     downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
  350.     downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
  351.     publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, "odom");
  352.   }
  353.  
  354.   void loopClosureThread() {
  355.     //  什么时候才进行回环检测？
  356.     if (loopClosureEnableFlag == false)
  357.       return;
  358.  
  359.     // 以一定的频率执行回环检测
  360.     ros::Rate rate(0.2);
  361.     while (ros::ok()) {
  362.       rate.sleep();
  363.       performLoopClosure();
  364.     }
  365.   }
  366.  
  367.   bool detectLoopClosure(int *latestID, int *closestID) {
  368.     int latestFrameIDLoopCloure;
  369.     int closestHistoryFrameID;
  370.  
  371.     latestKeyFrameCloud->clear();
  372.     nearHistoryKeyFrameCloud->clear();
  373.  
  374.     std::lock_guard<std::mutex> lock(mtx);
  375.  
  376.     // find the closest history key frame
  377.     std::vector<int> pointSearchIndLoop;
  378.     std::vector<float> pointSearchSqDisLoop;
  379.     kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
  380.     kdtreeHistoryKeyPoses->radiusSearch(cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop,
  381.                                         pointSearchSqDisLoop, 0);
  382.  
  383.     //  两帧时间差也满足最小要求
  384.     closestHistoryFrameID = -1;
  385.     for (int i = 0; i < pointSearchIndLoop.size(); ++i) {
  386.       int id = pointSearchIndLoop[i];
  387.       if (abs(cloudKeyPoses6D->points[id].time - timeLaserCloudInfoLast) > historyKeyframeSearchTimeDiff) {
  388.         closestHistoryFrameID = id;
  389.         break;
  390.       }
  391.     }
  392.  
  393.     if (closestHistoryFrameID == -1)
  394.       return false;
  395.  
  396.     if (cloudKeyPoses3D->size() - 1 == closestHistoryFrameID)
  397.       return false;
  398.  
  399.     // save latest key frames
  400.     latestFrameIDLoopCloure = cloudKeyPoses3D->size() - 1;
  401.     *latestKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure],
  402.                                                  &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
  403.     *latestKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure],
  404.                                                  &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
  405.  
  406.     // save history near key frames
  407.     bool nearFrameAvailable = false;
  408.     for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j) {
  409.       if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
  410.         continue;
  411.       *nearHistoryKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID + j],
  412.                                                         &cloudKeyPoses6D->points[closestHistoryFrameID + j]);
  413.       *nearHistoryKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID + j],
  414.                                                         &cloudKeyPoses6D->points[closestHistoryFrameID + j]);
  415.       nearFrameAvailable = true;
  416.     }
  417.  
  418.     if (nearFrameAvailable == false)
  419.       return false;
  420.  
  421.     *latestID = latestFrameIDLoopCloure;
  422.     *closestID = closestHistoryFrameID;
  423.  
  424.     return true;
  425.   }
  426.  
  427.   void performLoopClosure() {
  428.     if (cloudKeyPoses3D->points.empty() == true)
  429.       return;
  430.  
  431.     int latestFrameIDLoopCloure; // 关键帧队列中最新的关键帧id
  432.     int closestHistoryFrameID;  // 最近的关键帧id
  433.     if (detectLoopClosure(&latestFrameIDLoopCloure, &closestHistoryFrameID) == false)
  434.       return;
  435.  
  436.     //  检测到了回环进入以下流程，将两帧点云进行icp配准得到最终的trans
  437.     // ICP Settings
  438.     pcl::IterativeClosestPoint<PointType, PointType> icp;
  439.     icp.setMaxCorrespondenceDistance(100);
  440.     icp.setMaximumIterations(100);
  441.     icp.setTransformationEpsilon(1e-6);
  442.     icp.setEuclideanFitnessEpsilon(1e-6);
  443.     icp.setRANSACIterations(0);
  444.  
  445.     // Downsample map cloud
  446.     pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
  447.     downSizeFilterICP.setInputCloud(nearHistoryKeyFrameCloud);
  448.     downSizeFilterICP.filter(*cloud_temp);
  449.     *nearHistoryKeyFrameCloud = *cloud_temp;
  450.     // publish history near key frames
  451.     publishCloud(&pubHistoryKeyFrames, nearHistoryKeyFrameCloud, timeLaserInfoStamp, "odom");
  452.  
  453.     // Align clouds 将回环帧与local map进行匹配
  454.     icp.setInputSource(latestKeyFrameCloud);
  455.     icp.setInputTarget(nearHistoryKeyFrameCloud);
  456.     pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
  457.     icp.align(*unused_result);
  458.  
  459.     // 通过icp score阈值判断是否匹配成功
  460.     // std::cout << "ICP converg flag:" << icp.hasConverged() << ". Fitness score: " << icp.getFitnessScore() << std::endl;
  461.     if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
  462.       return;
  463.  
  464.     // publish corrected cloud
  465.     if (pubIcpKeyFrames.getNumSubscribers() != 0) {
  466.       pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
  467.       pcl::transformPointCloud(*latestKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
  468.       publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, "odom");
  469.     }
  470.  
  471.     // Get pose transformation
  472.     float x, y, z, roll, pitch, yaw;
  473.     Eigen::Affine3f correctionLidarFrame;
  474.     // icp得到的两帧之间的转换
  475.     correctionLidarFrame = icp.getFinalTransformation();
  476.     // transform from world origin to wrong pose
  477.     Eigen::Affine3f tWrong = pclPointToAffine3f(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
  478.     // transform from world origin to corrected pose
  479.     Eigen::Affine3f tCorrect =
  480.         correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
  481.     pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
  482.  
  483.     // gtsam中添加回环的约束
  484.     gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
  485.     gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
  486.     gtsam::Vector Vector6(6);
  487.     float noiseScore = icp.getFitnessScore();
  488.     Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
  489.     noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
  490.  
  491.     // Add pose constraint
  492.     std::lock_guard<std::mutex> lock(mtx);
  493.     gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo),
  494.                                         constraintNoise));
  495.     isam->update(gtSAMgraph);
  496.     isam->update();
  497.     isam->update();
  498.     isam->update();
  499.     isam->update();
  500.     isam->update();
  501.     gtSAMgraph.resize(0);
  502.  
  503.     aLoopIsClosed = true;
  504.   }
  505.  
  506.   void updateInitialGuess() {
  507.     // 更新初始位姿, 来源可以是GPS ODOM, 也可以是上一帧的位姿, 存在transformTobeMapped中
  508.     // initialization
  509.     if (cloudKeyPoses3D->points.empty()) {
  510.       // 第一帧点云进来
  511.       transformTobeMapped[0] = cloudInfo.imuRollInit;
  512.       transformTobeMapped[1] = cloudInfo.imuPitchInit;
  513.       transformTobeMapped[2] = cloudInfo.imuYawInit;
  514.  
  515.       if (!useImuHeadingInitialization)
  516.         transformTobeMapped[2] = 0;
  517.  
  518.       // 获取初始的transform
  519.       lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
  520.                                                      cloudInfo.imuYawInit); // save imu before return;
  521.       return;
  522.     }
  523.  
  524.     // use imu pre-integration estimation for pose guess
  525.     // odom可用的话, 使用mu odom作为初始位姿, 每个点在imuPreintexx.cpp中会实时进行预计分优化, 并存储其优化后的odom
  526.     if (cloudInfo.odomAvailable == true && cloudInfo.imuPreintegrationResetId == imuPreintegrationResetId) {
  527.       transformTobeMapped[0] = cloudInfo.initialGuessRoll;
  528.       transformTobeMapped[1] = cloudInfo.initialGuessPitch;
  529.       transformTobeMapped[2] = cloudInfo.initialGuessYaw;
  530.  
  531.       transformTobeMapped[3] = cloudInfo.initialGuessX;
  532.       transformTobeMapped[4] = cloudInfo.initialGuessY;
  533.       transformTobeMapped[5] = cloudInfo.initialGuessZ;
  534.  
  535.       lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
  536.                                                      cloudInfo.imuYawInit); // save imu before return;
  537.       return;
  538.     }
  539.  
  540.     // use imu incremental estimation for pose guess (only rotation)
  541.     // imu可用的话, 使用imu计算一个旋转增量, 这里？
  542.     if (cloudInfo.imuAvailable == true) {
  543.       Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
  544.                                                          cloudInfo.imuYawInit);
  545.       Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
  546.  
  547.       Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
  548.       Eigen::Affine3f transFinal = transTobe * transIncre;
  549.       pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4],
  550.                                         transformTobeMapped[5],
  551.                                         transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  552.  
  553.       lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
  554.                                                      cloudInfo.imuYawInit); // save imu before return;
  555.       return;
  556.     }
  557.   }
  558.  
  559.   void extractForLoopClosure() {
  560.     // 提取回环候选帧
  561.     pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
  562.     int numPoses = cloudKeyPoses3D->size();
  563.     for (int i = numPoses - 1; i >= 0; --i) {
  564.       if (cloudToExtract->size() <= surroundingKeyframeSize)
  565.         cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
  566.       else
  567.         break;
  568.     }
  569.  
  570.     extractCloud(cloudToExtract);
  571.   }
  572.  
  573.   void extractNearby() {
  574.     // 提取附近的点云帧, 包括corner和surface, cloudKeyPoses3D
  575.     pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
  576.     pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
  577.     std::vector<int> pointSearchInd;
  578.     std::vector<float> pointSearchSqDis;
  579.  
  580.     // extract all the nearby key poses and downsample them, 50m范围内的关键帧
  581.     kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
  582.     kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double) surroundingKeyframeSearchRadius,
  583.                                             pointSearchInd, pointSearchSqDis);
  584.     // 将满足要求的点云帧加到surroundingKeyPoses中
  585.     for (int i = 0; i < pointSearchInd.size(); ++i) {
  586.       int id = pointSearchInd[i];
  587.       surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
  588.     }
  589.  
  590.     downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
  591.     downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
  592.  
  593.     // also extract some latest key frames in case the robot rotates in one position
  594.     // 把10s内同方向的关键帧也加到surroundingKeyPosesDS中
  595.     int numPoses = cloudKeyPoses3D->size();
  596.     for (int i = numPoses - 1; i >= 0; --i) {
  597.       // 10s内的位姿态都加进来
  598.       if (timeLaserCloudInfoLast - cloudKeyPoses6D->points[i].time < 10.0)
  599.         surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
  600.       else
  601.         break;
  602.     }
  603.  
  604.     extractCloud(surroundingKeyPosesDS);
  605.   }
  606.  
  607.   void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract) {
  608.     // 根据pose提取点云
  609.     std::vector<pcl::PointCloud<PointType>> laserCloudCornerSurroundingVec;
  610.     std::vector<pcl::PointCloud<PointType>> laserCloudSurfSurroundingVec;
  611.  
  612.     laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
  613.     laserCloudSurfSurroundingVec.resize(cloudToExtract->size());
  614.  
  615.     // extract surrounding map
  616. #pragma omp parallel for num_threads(numberOfCores)
  617.     for (int i = 0; i < cloudToExtract->size(); ++i) {
  618.       // 遍历每个位姿
  619.       if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
  620.         continue;
  621.       int thisKeyInd = (int) cloudToExtract->points[i].intensity;
  622.       laserCloudCornerSurroundingVec[i] = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],
  623.                                                                &cloudKeyPoses6D->points[thisKeyInd]);
  624.       laserCloudSurfSurroundingVec[i] = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],
  625.                                                              &cloudKeyPoses6D->points[thisKeyInd]);
  626.     }
  627.  
  628.     // fuse the map
  629.     // 构建local map
  630.     laserCloudCornerFromMap->clear();
  631.     laserCloudSurfFromMap->clear();
  632.     for (int i = 0; i < cloudToExtract->size(); ++i) {
  633.       *laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
  634.       *laserCloudSurfFromMap += laserCloudSurfSurroundingVec[i];
  635.     }
  636.  
  637.     // Downsample the surrounding corner key frames (or map)
  638.     downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
  639.     downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
  640.     laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
  641.     // Downsample the surrounding surf key frames (or map)
  642.     downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
  643.     downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
  644.     laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
  645.   }
  646.  
  647.   void extractSurroundingKeyFrames() {
  648.     if (cloudKeyPoses3D->points.empty() == true)
  649.       return;
  650.  
  651.     // 检测到了回环就提取回环帧,否则提取附近点云
  652.     // 第一次进来loopClosureEnableFlag = false, 直接提取附近关键帧
  653.     if (loopClosureEnableFlag == true) {
  654.       extractForLoopClosure();
  655.     } else {
  656.       extractNearby();
  657.     }
  658.   }
  659.  
  660.   void downsampleCurrentScan() {
  661.     // Downsample cloud from current scan
  662.     laserCloudCornerLastDS->clear();
  663.     downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
  664.     downSizeFilterCorner.filter(*laserCloudCornerLastDS);
  665.     laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
  666.  
  667.     laserCloudSurfLastDS->clear();
  668.     downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
  669.     downSizeFilterSurf.filter(*laserCloudSurfLastDS);
  670.     laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
  671.   }
  672.  
  673.   void updatePointAssociateToMap() {
  674.     // 根据初始位姿将点云转换到Map系下
  675.     transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
  676.   }
  677.  
  678.   void cornerOptimization() {
  679.     updatePointAssociateToMap(); // 将points转到地图系
  680.  
  681. #pragma omp parallel for num_threads(numberOfCores)
  682.     // 遍历点云, 构建点到直线的约束
  683.     for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
  684.       PointType pointOri, pointSel, coeff;
  685.       std::vector<int> pointSearchInd;
  686.       std::vector<float> pointSearchSqDis;
  687.  
  688.       // 在map中搜索当前点的5个紧邻点
  689.       pointOri = laserCloudCornerLastDS->points[i];
  690.       pointAssociateToMap(&pointOri, &pointSel);
  691.       kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
  692.  
  693.       cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
  694.       cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
  695.       cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
  696.  
  697.       // 只有最近的点都在一定阈值内（1米）才进行计算
  698.       if (pointSearchSqDis[4] < 1.0) {
  699.         float cx = 0, cy = 0, cz = 0;
  700.         for (int j = 0; j < 5; j++) {
  701.           cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
  702.           cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
  703.           cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
  704.         }
  705.         // 计算其算数平均值/均值
  706.         cx /= 5;
  707.         cy /= 5;
  708.         cz /= 5;
  709.  
  710.         // 计算协方差
  711.         float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
  712.         for (int j = 0; j < 5; j++) {
  713.           float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
  714.           float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
  715.           float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
  716.  
  717.           a11 += ax * ax;
  718.           a12 += ax * ay;
  719.           a13 += ax * az;
  720.           a22 += ay * ay;
  721.           a23 += ay * az;
  722.           a33 += az * az;
  723.         }
  724.         a11 /= 5;
  725.         a12 /= 5;
  726.         a13 /= 5;
  727.         a22 /= 5;
  728.         a23 /= 5;
  729.         a33 /= 5;
  730.  
  731.         matA1.at<float>(0, 0) = a11;
  732.         matA1.at<float>(0, 1) = a12;
  733.         matA1.at<float>(0, 2) = a13;
  734.         matA1.at<float>(1, 0) = a12;
  735.         matA1.at<float>(1, 1) = a22;
  736.         matA1.at<float>(1, 2) = a23;
  737.         matA1.at<float>(2, 0) = a13;
  738.         matA1.at<float>(2, 1) = a23;
  739.         matA1.at<float>(2, 2) = a33;
  740.  
  741.         // 求协方差矩阵的特征值和特征向量, 特征值：matD1，特征向量：保存在矩阵matV1中。
  742.         cv::eigen(matA1, matD1, matV1);
  743.  
  744.         // 其中一个特征值远远大于其他两个，则呈线状
  745.         if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
  746.           float x0 = pointSel.x;
  747.           float y0 = pointSel.y;
  748.           float z0 = pointSel.z;
  749.           float x1 = cx + 0.1 * matV1.at<float>(0, 0);
  750.           float y1 = cy + 0.1 * matV1.at<float>(0, 1);
  751.           float z1 = cz + 0.1 * matV1.at<float>(0, 2);
  752.           float x2 = cx - 0.1 * matV1.at<float>(0, 0);
  753.           float y2 = cy - 0.1 * matV1.at<float>(0, 1);
  754.           float z2 = cz - 0.1 * matV1.at<float>(0, 2);
  755.  
  756.           // 与里程计的计算类似，计算到直线的距离
  757.           float a012 = sqrt(((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) *
  758.               ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))
  759.                                 + ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) *
  760.                                     ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))
  761.                                 + ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)) *
  762.                                     ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)));
  763.  
  764.           float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
  765.  
  766.           float la = ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))
  767.               + (z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / a012 / l12;
  768.  
  769.           float lb = -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1))
  770.               - (z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;
  771.  
  772.           float lc = -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))
  773.               + (y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;
  774.  
  775.           float ld2 = a012 / l12;
  776.  
  777.           // 下面涉及到一个鲁棒核函数，作者简单地设计了这个核函数。
  778.           float s = 1 - 0.9 * fabs(ld2);
  779.  
  780.           coeff.x = s * la;
  781.           coeff.y = s * lb;
  782.           coeff.z = s * lc;
  783.           coeff.intensity = s * ld2;
  784.  
  785.           // 程序末尾根据s的值来判断是否将点云点放入点云集合laserCloudOri以及coeffSel中。
  786.           if (s > 0.1) {
  787.             laserCloudOriCornerVec[i] = pointOri;
  788.             coeffSelCornerVec[i] = coeff;
  789.             laserCloudOriCornerFlag[i] = true;
  790.           }
  791.         }
  792.       }
  793.     }
  794.   }
  795.  
  796.   void surfOptimization() {
  797.     updatePointAssociateToMap();
  798.  
  799. #pragma omp parallel for num_threads(numberOfCores)
  800.     for (int i = 0; i < laserCloudSurfLastDSNum; i++) {
  801.       PointType pointOri, pointSel, coeff;
  802.       std::vector<int> pointSearchInd;
  803.       std::vector<float> pointSearchSqDis;
  804.  
  805.       // 寻找5个紧邻点, 计算其特征值和特征向量
  806.       pointOri = laserCloudSurfLastDS->points[i];
  807.       pointAssociateToMap(&pointOri, &pointSel);
  808.       kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
  809.  
  810.       Eigen::Matrix<float, 5, 3> matA0;
  811.       Eigen::Matrix<float, 5, 1> matB0;
  812.       Eigen::Vector3f matX0;
  813.  
  814.       matA0.setZero();  // 5*3 存储5个紧邻点
  815.       matB0.fill(-1);
  816.       matX0.setZero();
  817.  
  818.       // 只考虑附近1.0m内
  819.       if (pointSearchSqDis[4] < 1.0) {
  820.         for (int j = 0; j < 5; j++) {
  821.           matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
  822.           matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
  823.           matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
  824.         }
  825.  
  826.         // 求maxA0中点构成的平面法向量
  827.         matX0 = matA0.colPivHouseholderQr().solve(matB0);
  828.  
  829.         // 法向量参数 ax+by+cz +d = 0
  830.         float pa = matX0(0, 0);
  831.         float pb = matX0(1, 0);
  832.         float pc = matX0(2, 0);
  833.         float pd = 1;
  834.  
  835.         float ps = sqrt(pa * pa + pb * pb + pc * pc);
  836.         pa /= ps;
  837.         pb /= ps;
  838.         pc /= ps;
  839.         pd /= ps;
  840.  
  841.         // 这里再次判断求解的方向向量和每个点相乘，最后结果是不是在误差范围内。
  842.         bool planeValid = true;
  843.         for (int j = 0; j < 5; j++) {
  844.           if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
  845.               pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
  846.               pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
  847.             planeValid = false;
  848.             break;
  849.           }
  850.         }
  851.  
  852.         // 是有效的平面
  853.         if (planeValid) {
  854.           float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
  855.  
  856.           float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
  857.                                                         + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
  858.  
  859.           coeff.x = s * pa;
  860.           coeff.y = s * pb;
  861.           coeff.z = s * pc;
  862.           coeff.intensity = s * pd2;
  863.  
  864.           // 误差在允许的范围内的话把这个点放到点云laserCloudOri中去，把对应的向量coeff放到coeffSel中
  865.           if (s > 0.1) {
  866.             laserCloudOriSurfVec[i] = pointOri;
  867.             coeffSelSurfVec[i] = coeff;
  868.             laserCloudOriSurfFlag[i] = true;
  869.           }
  870.         }
  871.       }
  872.     }
  873.   }
  874.  
  875.   void combineOptimizationCoeffs() {
  876.     // 把两类损失和协方差丢到laserCloudOri和coeffSel中, 后续进行联合优化
  877.     // combine corner coeffs
  878.     for (int i = 0; i < laserCloudCornerLastDSNum; ++i) {
  879.       if (laserCloudOriCornerFlag[i] == true) {
  880.         laserCloudOri->push_back(laserCloudOriCornerVec[i]);
  881.         coeffSel->push_back(coeffSelCornerVec[i]);
  882.       }
  883.     }
  884.     // combine surf coeffs
  885.     for (int i = 0; i < laserCloudSurfLastDSNum; ++i) {
  886.       if (laserCloudOriSurfFlag[i] == true) {
  887.         laserCloudOri->push_back(laserCloudOriSurfVec[i]);
  888.         coeffSel->push_back(coeffSelSurfVec[i]);
  889.       }
  890.     }
  891.     // reset flag for next iteration 重置参数, 下一帧还要继续用
  892.     std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
  893.     std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
  894.   }
  895.  
  896.   bool LMOptimization(int iterCount) {
  897.     // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
  898.     // lidar <- camera      ---     camera <- lidar
  899.     // x = z                ---     x = y
  900.     // y = x                ---     y = z
  901.     // z = y                ---     z = x
  902.     // roll = yaw           ---     roll = pitch
  903.     // pitch = roll         ---     pitch = yaw
  904.     // yaw = pitch          ---     yaw = roll
  905.  
  906.     // 高斯牛顿优化, 参考LOAM
  907.     // lidar -> camera
  908.     float srx = sin(transformTobeMapped[1]);
  909.     float crx = cos(transformTobeMapped[1]);
  910.     float sry = sin(transformTobeMapped[2]);
  911.     float cry = cos(transformTobeMapped[2]);
  912.     float srz = sin(transformTobeMapped[0]);
  913.     float crz = cos(transformTobeMapped[0]);
  914.  
  915.     // 初次优化时，特征值门限设置为50，小于这个值认为是退化了，修改matX，matX=matP*matX2
  916.     int laserCloudSelNum = laserCloudOri->size();
  917.     if (laserCloudSelNum < 50) {
  918.       return false;
  919.     }
  920.  
  921.     cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
  922.     cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
  923.     cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
  924.     cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
  925.     cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
  926.     cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
  927.     cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
  928.  
  929.     PointType pointOri, coeff;
  930.  
  931.     for (int i = 0; i < laserCloudSelNum; i++) {
  932.       // lidar -> camera
  933.       pointOri.x = laserCloudOri->points[i].y;
  934.       pointOri.y = laserCloudOri->points[i].z;
  935.       pointOri.z = laserCloudOri->points[i].x;
  936.       // lidar -> camera
  937.       coeff.x = coeffSel->points[i].y;
  938.       coeff.y = coeffSel->points[i].z;
  939.       coeff.z = coeffSel->points[i].x;
  940.       coeff.intensity = coeffSel->points[i].intensity;
  941.       // in camera
  942.       // 计算雅克比
  943.       float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y - srx * sry * pointOri.z) * coeff.x
  944.           + (-srx * srz * pointOri.x - crz * srx * pointOri.y - crx * pointOri.z) * coeff.y
  945.           + (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y - cry * srx * pointOri.z) *
  946.               coeff.z;
  947.  
  948.       float ary = ((cry * srx * srz - crz * sry) * pointOri.x
  949.           + (sry * srz + cry * crz * srx) * pointOri.y + crx * cry * pointOri.z) * coeff.x
  950.           + ((-cry * crz - srx * sry * srz) * pointOri.x
  951.               + (cry * srz - crz * srx * sry) * pointOri.y - crx * sry * pointOri.z) * coeff.z;
  952.  
  953.       float arz =
  954.           ((crz * srx * sry - cry * srz) * pointOri.x + (-cry * crz - srx * sry * srz) * pointOri.y) * coeff.x
  955.               + (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y
  956.               +
  957.                   ((sry * srz + cry * crz * srx) * pointOri.x + (crz * sry - cry * srx * srz) * pointOri.y) * coeff.z;
  958.       // lidar -> camera
  959.       matA.at<float>(i, 0) = arz;
  960.       matA.at<float>(i, 1) = arx;
  961.       matA.at<float>(i, 2) = ary;
  962.       matA.at<float>(i, 3) = coeff.z;
  963.       matA.at<float>(i, 4) = coeff.x;
  964.       matA.at<float>(i, 5) = coeff.y;
  965.       matB.at<float>(i, 0) = -coeff.intensity;
  966.     }
  967.  
  968.     cv::transpose(matA, matAt);
  969.     matAtA = matAt * matA;
  970.     matAtB = matAt * matB;
  971.     cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
  972.  
  973.     if (iterCount == 0) {
  974.  
  975.       cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
  976.       cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
  977.       cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
  978.  
  979.       cv::eigen(matAtA, matE, matV);
  980.       matV.copyTo(matV2);
  981.  
  982.       isDegenerate = false;
  983.       float eignThre[6] = {100, 100, 100, 100, 100, 100};
  984.       for (int i = 5; i >= 0; i--) {
  985.         if (matE.at<float>(0, i) < eignThre[i]) {
  986.           for (int j = 0; j < 6; j++) {
  987.             matV2.at<float>(i, j) = 0;
  988.           }
  989.           isDegenerate = true;
  990.         } else {
  991.           break;
  992.         }
  993.       }
  994.       matP = matV.inv() * matV2;
  995.     }
  996.  
  997.     if (isDegenerate) {
  998.       cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
  999.       matX.copyTo(matX2);
  1000.       matX = matP * matX2;
  1001.     }
  1002.  
  1003.     transformTobeMapped[0] += matX.at<float>(0, 0);
  1004.     transformTobeMapped[1] += matX.at<float>(1, 0);
  1005.     transformTobeMapped[2] += matX.at<float>(2, 0);
  1006.     transformTobeMapped[3] += matX.at<float>(3, 0);
  1007.     transformTobeMapped[4] += matX.at<float>(4, 0);
  1008.     transformTobeMapped[5] += matX.at<float>(5, 0);
  1009.  
  1010.     float deltaR = sqrt(
  1011.         pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
  1012.             pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
  1013.             pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
  1014.     float deltaT = sqrt(
  1015.         pow(matX.at<float>(3, 0) * 100, 2) +
  1016.             pow(matX.at<float>(4, 0) * 100, 2) +
  1017.             pow(matX.at<float>(5, 0) * 100, 2));
  1018.  
  1019.     // 在判断是否是有效的优化时，要求旋转部分的模长小于0.05m，平移部分的模长也小于0.05度
  1020.     if (deltaR < 0.05 && deltaT < 0.05) {
  1021.       return true; // converged
  1022.     }
  1023.     return false; // keep optimizing
  1024.   }
  1025.  
  1026.   void scan2MapOptimization() {
  1027.     if (cloudKeyPoses3D->points.empty())
  1028.       return;
  1029.  
  1030.     //  特征需要满足一定要求才可以进行
  1031.     if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum) {
  1032.       // 构建kdtree搜索的map, 两类
  1033.       kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
  1034.       kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
  1035.  
  1036.       // 迭代30次进行优化
  1037.       for (int iterCount = 0; iterCount < 30; iterCount++) {
  1038.         laserCloudOri->clear();
  1039.         coeffSel->clear();
  1040.  
  1041.         // 点到平面, 点到直线的残差, 这里写法还与aloam有点区别
  1042.         cornerOptimization();
  1043.         surfOptimization();
  1044.  
  1045.         // 联合两类的残差
  1046.         combineOptimizationCoeffs();
  1047.  
  1048.         // 高斯牛顿法迭代优化
  1049.         if (LMOptimization(iterCount) == true)
  1050.           break;
  1051.       }
  1052.  
  1053.       // 更新transform
  1054.       transformUpdate();
  1055.     } else {
  1056.       ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum,
  1057.                laserCloudSurfLastDSNum);
  1058.     }
  1059.   }
  1060.  
  1061.   void transformUpdate() {
  1062.     // IMU可用的话更新transformTobeMapped
  1063.     if (cloudInfo.imuAvailable == true) {
  1064.       if (std::abs(cloudInfo.imuPitchInit) < 1.4) {
  1065.         double imuWeight = 0.05;
  1066.         tf::Quaternion imuQuaternion;
  1067.         tf::Quaternion transformQuaternion;
  1068.         double rollMid, pitchMid, yawMid;
  1069.  
  1070.         // slerp roll
  1071.         transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
  1072.         imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
  1073.         // 线性插值
  1074.         tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
  1075.         transformTobeMapped[0] = rollMid;
  1076.  
  1077.         // slerp pitch
  1078.         transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
  1079.         imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
  1080.         tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
  1081.         transformTobeMapped[1] = pitchMid;
  1082.       }
  1083.     }
  1084.  
  1085.     transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
  1086.     transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
  1087.     transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
  1088.   }
  1089.  
  1090.   float constraintTransformation(float value, float limit) {
  1091.     if (value < -limit)
  1092.       value = -limit;
  1093.     if (value > limit)
  1094.       value = limit;
  1095.  
  1096.     return value;
  1097.   }
  1098.  
  1099.   bool saveFrame() {
  1100.     if (cloudKeyPoses3D->points.empty())
  1101.       return true;
  1102.  
  1103.     Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
  1104.     Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4],
  1105.                                                         transformTobeMapped[5],
  1106.                                                         transformTobeMapped[0], transformTobeMapped[1],
  1107.                                                         transformTobeMapped[2]);
  1108.     Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
  1109.     float x, y, z, roll, pitch, yaw;
  1110.     pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
  1111.  
  1112.     if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
  1113.         abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
  1114.         abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
  1115.         sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
  1116.       return false;
  1117.  
  1118.     return true;
  1119.   }
  1120.  
  1121.   void addOdomFactor() {
  1122.     if (cloudKeyPoses3D->points.empty()) {
  1123.       // 第一帧进来时初始化gtsam参数
  1124.       noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
  1125.           (Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
  1126.           // 先验因子
  1127.       gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
  1128.       initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
  1129.     } else {
  1130.       noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances(
  1131.           (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
  1132.       gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
  1133.       gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
  1134.       // 二元因子
  1135.       gtSAMgraph.add(
  1136.           BetweenFactor<Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo),
  1137.                                odometryNoise));
  1138.       initialEstimate.insert(cloudKeyPoses3D->size(), poseTo); // 添加值
  1139.     }
  1140.   }
  1141.  
  1142.   void addGPSFactor() {
  1143.     if (gpsQueue.empty())
  1144.       return;
  1145.  
  1146.     // wait for system initialized and settles down
  1147.     if (cloudKeyPoses3D->points.empty())
  1148.       return;
  1149.     else {
  1150.       if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
  1151.         return;
  1152.     }
  1153.  
  1154.     // pose covariance small, no need to correct
  1155.     if (poseCovariance(3, 3) < poseCovThreshold && poseCovariance(4, 4) < poseCovThreshold)
  1156.       return;
  1157.  
  1158.     // pose的协方差比较大的时候才去添加gps factor
  1159.     while (!gpsQueue.empty()) {
  1160.       // 时间戳对齐
  1161.       if (gpsQueue.front().header.stamp.toSec() < timeLaserCloudInfoLast - 0.2) {
  1162.         // message too old
  1163.         gpsQueue.pop_front();
  1164.       } else if (gpsQueue.front().header.stamp.toSec() > timeLaserCloudInfoLast + 0.2) {
  1165.         // message too new
  1166.         break;
  1167.       } else {
  1168.         nav_msgs::Odometry thisGPS = gpsQueue.front();
  1169.         gpsQueue.pop_front();
  1170.  
  1171.         // GPS too noisy, skip
  1172.         float noise_x = thisGPS.pose.covariance[0];
  1173.         float noise_y = thisGPS.pose.covariance[7];
  1174.         float noise_z = thisGPS.pose.covariance[14];
  1175.         if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
  1176.           continue;
  1177.  
  1178.         float gps_x = thisGPS.pose.pose.position.x;
  1179.         float gps_y = thisGPS.pose.pose.position.y;
  1180.         float gps_z = thisGPS.pose.pose.position.z;
  1181.         if (!useGpsElevation) {
  1182.           gps_z = transformTobeMapped[5];  // gps的z一般不可信
  1183.           noise_z = 0.01;
  1184.         }
  1185.  
  1186.         // GPS not properly initialized (0,0,0)
  1187.         if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
  1188.           continue;
  1189.  
  1190.         // 添加GPS因子
  1191.         gtsam::Vector Vector3(3);
  1192.         Vector3 << noise_x, noise_y, noise_z;
  1193.         noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3); // 噪声定义
  1194.         gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
  1195.         gtSAMgraph.add(gps_factor);
  1196.  
  1197.         aLoopIsClosed = true;
  1198.         break;
  1199.       }
  1200.     }
  1201.   }
  1202.  
  1203.   void saveKeyFramesAndFactor() {
  1204.     if (saveFrame() == false)
  1205.       return;
  1206.     // 添加各种factor、保存关键帧
  1207.     // odom factor
  1208.     addOdomFactor();
  1209.  
  1210.     // gps factor
  1211.     addGPSFactor();
  1212.  
  1213.     // cout << "****************************************************" << endl;
  1214.     // gtSAMgraph.print("GTSAM Graph:\n");
  1215.  
  1216.     // update iSAM
  1217.     isam->update(gtSAMgraph, initialEstimate);
  1218.     isam->update();
  1219.  
  1220.     // update multiple-times till converge
  1221.     if (aLoopIsClosed == true) {
  1222.       isam->update();
  1223.       isam->update();
  1224.       isam->update();
  1225.       isam->update();
  1226.       isam->update();
  1227.     }
  1228.  
  1229.     gtSAMgraph.resize(0);
  1230.     initialEstimate.clear();
  1231.  
  1232.     //save key poses
  1233.     PointType thisPose3D;
  1234.     PointTypePose thisPose6D;
  1235.     Pose3 latestEstimate;
  1236.  
  1237.     // 最新的pose
  1238.     isamCurrentEstimate = isam->calculateEstimate();
  1239.     latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size() - 1);
  1240.     // cout << "****************************************************" << endl;
  1241.     // isamCurrentEstimate.print("Current estimate: ");
  1242.  
  1243.     // 这里不断的增加关键帧到cloudKeyPoses3D、cloudKeyPoses6D中
  1244.     thisPose3D.x = latestEstimate.translation().x();
  1245.     thisPose3D.y = latestEstimate.translation().y();
  1246.     thisPose3D.z = latestEstimate.translation().z();
  1247.     thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
  1248.     cloudKeyPoses3D->push_back(thisPose3D);
  1249.  
  1250.     thisPose6D.x = thisPose3D.x;
  1251.     thisPose6D.y = thisPose3D.y;
  1252.     thisPose6D.z = thisPose3D.z;
  1253.     thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
  1254.     thisPose6D.roll = latestEstimate.rotation().roll();
  1255.     thisPose6D.pitch = latestEstimate.rotation().pitch();
  1256.     thisPose6D.yaw = latestEstimate.rotation().yaw();
  1257.     thisPose6D.time = timeLaserCloudInfoLast;
  1258.     cloudKeyPoses6D->push_back(thisPose6D);
  1259.  
  1260.     // cout << "****************************************************" << endl;
  1261.     // cout << "Pose covariance:" << endl;
  1262.     // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
  1263.     // 边缘化得到每个变量的协方差
  1264.     poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);
  1265.  
  1266.     // save updated transform
  1267.     transformTobeMapped[0] = latestEstimate.rotation().roll();
  1268.     transformTobeMapped[1] = latestEstimate.rotation().pitch();
  1269.     transformTobeMapped[2] = latestEstimate.rotation().yaw();
  1270.     transformTobeMapped[3] = latestEstimate.translation().x();
  1271.     transformTobeMapped[4] = latestEstimate.translation().y();
  1272.     transformTobeMapped[5] = latestEstimate.translation().z();
  1273.  
  1274.     // save all the received edge and surf points
  1275.     pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
  1276.     pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
  1277.     pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
  1278.     pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
  1279.  
  1280.     // save key frame cloud
  1281.     cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
  1282.     surfCloudKeyFrames.push_back(thisSurfKeyFrame);
  1283.  
  1284.     // save path for visualization
  1285.     updatePath(thisPose6D);
  1286.   }
  1287.  
  1288.   void correctPoses() {
  1289.     if (cloudKeyPoses3D->points.empty())
  1290.       return;
  1291.  
  1292.     if (aLoopIsClosed == true) {
  1293.       // clear path
  1294.       globalPath.poses.clear();
  1295.       // update key poses 更新位姿
  1296.       int numPoses = isamCurrentEstimate.size();
  1297.       for (int i = 0; i < numPoses; ++i) {
  1298.         cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
  1299.         cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
  1300.         cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
  1301.  
  1302.         cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
  1303.         cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
  1304.         cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
  1305.         cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
  1306.         cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
  1307.         cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
  1308.  
  1309.         updatePath(cloudKeyPoses6D->points[i]);
  1310.       }
  1311.  
  1312.       aLoopIsClosed = false;
  1313.       // ID for reseting IMU pre-integration
  1314.       ++imuPreintegrationResetId;
  1315.     }
  1316.   }
  1317.  
  1318.   void updatePath(const PointTypePose &pose_in) {
  1319.     geometry_msgs::PoseStamped pose_stamped;
  1320.     pose_stamped.header.stamp = timeLaserInfoStamp;
  1321.     pose_stamped.header.frame_id = "odom";
  1322.     pose_stamped.pose.position.x = pose_in.x;
  1323.     pose_stamped.pose.position.y = pose_in.y;
  1324.     pose_stamped.pose.position.z = pose_in.z;
  1325.     tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
  1326.     pose_stamped.pose.orientation.x = q.x();
  1327.     pose_stamped.pose.orientation.y = q.y();
  1328.     pose_stamped.pose.orientation.z = q.z();
  1329.     pose_stamped.pose.orientation.w = q.w();
  1330.  
  1331.     globalPath.poses.push_back(pose_stamped);
  1332.   }
  1333.  
  1334.   void publishOdometry() {
  1335.     // Publish odometry for ROS
  1336.     nav_msgs::Odometry laserOdometryROS;
  1337.     laserOdometryROS.header.stamp = timeLaserInfoStamp;
  1338.     laserOdometryROS.header.frame_id = "odom";
  1339.     laserOdometryROS.child_frame_id = "odom_mapping";
  1340.     laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
  1341.     laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
  1342.     laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
  1343.     laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0],
  1344.                                                                                      transformTobeMapped[1],
  1345.                                                                                      transformTobeMapped[2]);
  1346.     laserOdometryROS.pose.covariance[0] = double(imuPreintegrationResetId);
  1347.     pubOdomAftMappedROS.publish(laserOdometryROS);
  1348.   }
  1349.  
  1350.   void publishFrames() {
  1351.     if (cloudKeyPoses3D->points.empty())
  1352.       return;
  1353.     // publish key poses
  1354.     publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, "odom");
  1355.     // Publish surrounding key frames
  1356.     publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");
  1357.     // publish registered key frame
  1358.     if (pubRecentKeyFrame.getNumSubscribers() != 0) {
  1359.       pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  1360.       PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
  1361.       *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
  1362.       *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
  1363.       publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, "odom");
  1364.     }
  1365.     // publish registered high-res raw cloud
  1366.     if (pubCloudRegisteredRaw.getNumSubscribers() != 0) {
  1367.       pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
  1368.       pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
  1369.       PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
  1370.       *cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
  1371.       publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, "odom");
  1372.     }
  1373.     // publish path
  1374.     if (pubPath.getNumSubscribers() != 0) {
  1375.       globalPath.header.stamp = timeLaserInfoStamp;
  1376.       globalPath.header.frame_id = "odom";
  1377.       pubPath.publish(globalPath);
  1378.     }
  1379.   }
  1380.  
  1381.  public:
  1382.   // gtsam
  1383.   NonlinearFactorGraph gtSAMgraph;
  1384.   Values initialEstimate;
  1385.   Values optimizedEstimate;
  1386.   ISAM2 *isam;
  1387.   Values isamCurrentEstimate;
  1388.   Eigen::MatrixXd poseCovariance;
  1389.  
  1390.   ros::Publisher pubLaserCloudSurround;
  1391.   ros::Publisher pubOdomAftMappedROS;
  1392.   ros::Publisher pubKeyPoses;
  1393.   ros::Publisher pubPath;
  1394.  
  1395.   ros::Publisher pubHistoryKeyFrames;
  1396.   ros::Publisher pubIcpKeyFrames;
  1397.   ros::Publisher pubRecentKeyFrames;
  1398.   ros::Publisher pubRecentKeyFrame;
  1399.   ros::Publisher pubCloudRegisteredRaw;
  1400.  
  1401.   ros::Subscriber subLaserCloudInfo;
  1402.   ros::Subscriber subGPS;
  1403.  
  1404.   std::deque<nav_msgs::Odometry> gpsQueue;
  1405.   lio_sam::cloud_info cloudInfo;
  1406.  
  1407.   vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
  1408.   vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
  1409.  
  1410.   pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
  1411.   pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
  1412.  
  1413.   pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
  1414.   pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
  1415.   pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
  1416.   pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
  1417.  
  1418.   pcl::PointCloud<PointType>::Ptr laserCloudOri;
  1419.   pcl::PointCloud<PointType>::Ptr coeffSel;
  1420.  
  1421.   std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
  1422.   std::vector<PointType> coeffSelCornerVec;
  1423.   std::vector<bool> laserCloudOriCornerFlag;
  1424.   std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
  1425.   std::vector<PointType> coeffSelSurfVec;
  1426.   std::vector<bool> laserCloudOriSurfFlag;
  1427.  
  1428.   pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
  1429.   pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
  1430.   pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
  1431.   pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
  1432.  
  1433.   pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
  1434.   pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
  1435.  
  1436.   pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
  1437.   pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
  1438.  
  1439.   pcl::PointCloud<PointType>::Ptr latestKeyFrameCloud;
  1440.   pcl::PointCloud<PointType>::Ptr nearHistoryKeyFrameCloud;
  1441.  
  1442.   pcl::VoxelGrid<PointType> downSizeFilterCorner;
  1443.   pcl::VoxelGrid<PointType> downSizeFilterSurf;
  1444.   pcl::VoxelGrid<PointType> downSizeFilterICP;
  1445.   pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
  1446.  
  1447.   ros::Time timeLaserInfoStamp;
  1448.   double timeLaserCloudInfoLast;
  1449.  
  1450.   float transformTobeMapped[6];
  1451.  
  1452.   std::mutex mtx;
  1453.  
  1454.   double timeLastProcessing = -1;
  1455.  
  1456.   bool isDegenerate = false;
  1457.   Eigen::Matrix<float, 6, 6> matP;
  1458.  
  1459.   int laserCloudCornerFromMapDSNum = 0;
  1460.   int laserCloudSurfFromMapDSNum = 0;
  1461.   int laserCloudCornerLastDSNum = 0;
  1462.   int laserCloudSurfLastDSNum = 0;
  1463.  
  1464.   bool aLoopIsClosed = false;
  1465.   int imuPreintegrationResetId = 0;
  1466.  
  1467.   nav_msgs::Path globalPath;
  1468.  
  1469.   Eigen::Affine3f transPointAssociateToMap;
  1470.  
  1471.   Eigen::Affine3f lastImuTransformation;
  1472.  
  1473. };
  1474.  
  1475. int main(int argc, char **argv) {
  1476.   ros::init(argc, argv, "lio_sam");
  1477.  
  1478.   mapOptimization MO;
  1479.  
  1480.   ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
  1481.  
  1482.   // 两个线程，一边按固定的频率进行回环检测、添加约束边，另外一边进行地图发布和保存
  1483.   std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
  1484.   std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
  1485.  
  1486.   // 这里不间断的执行callback
  1487.   ros::spin();
  1488.  
  1489.   return 0;
  1490. }

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家小花儿/article/detail/116683?site
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号