开源SLAM系统：ISCLOAM源码解析

| | | ℃ | 次阅读
字数: 3.6k | 阅读时长 ≈ 17

作者发表在ICRA2020上的论文，Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection，论文里面作者提出了一种基于scan context(18 IROS)改进的全局点云描述符，主要是编码了强度信息。ISCLOAM就是在此基础上，用ISC特征描述符做回环检测，完成的一套开源SLAM系统。本篇将从代码的角度进行梳理和分析。

项目地址：https://github.com/wh200720041/iscloam

个人注释版地址：https://github.com/JokerJohn/opensource_slam_noted

image-20200817230820894

简介

此系统的前端odometry部分还是基于作者原来开源的FLOAM，参考我的笔记

http://xchu.net/2020/08/17/49floam/#more。这里在引入了作者设计的ISC特征描述符，在回环检测的时候效率和精度都能保持在一定水平。如上图左侧可以看到根据点云实时提取的ISC特征。在下面终端输出的消息可以看到，其顺利的检测到了回环。

image-20200817231049151

其主要节点图如下：

image-20200817224759326

可以看到其前端和FLOAM一致，主要多了基于ISC的回环检测和后端优化模块。

代码结构如下：

  • lidar.cpp: 系统的参数配置，比如雷达线束数量、扫描周期等
  • laserProcessingClass.cpp、laserProcessingNode.cpp：点云预处理节点、包括边缘、平面特征提取
  • odomEstimationNode.cpp、odomEstimationClass.cpp：位姿估计节点，根据边缘点、平面点特征，构建点线、点面残差，利用ceres联合滑窗优化，估计位姿。
  • laserMappingNode.cpp、laserMappingClass.cpp：建图节点，根据优化后的odom信息拼接成最终点云地图
  • lidarOptimization.cpp：odom评价
  • iscOptimizationNode.cpp、iscOptimizationClass.cpp：后端优化节点，根据回环检测和odom信息，平滑整个位姿图。
  • iscGenerationNode.cpp、iscGenerationClass.cpp：根据odom和点云信息，进行实时的ISC特征提取并检测回环

前面的点云预处理、特征提取、位姿估计和LOAM基本一致，这里不再概述。

回环检测

此节点主要是根据预处理的点云，实时提取ISC特征，并进行回环检测，将检测到的回环消息发布出去。这里检测到了回环，会发布一个自定义的msg，其格式如下：

1
2
3
Header header
int32 current_id
int32[] matched_id

这里的cloud和odom的callback依然用来装buffer数据，回环检测的具体逻辑在单独的线程中来处理。

直接看回环检测，流程很简单。首先去除地面，这里直接采用直通滤波器对z轴进行阈值过滤，然后对非地面点进行截取，最后提取ISC特征，与之前的帧进行比对，计算得分。在比较ISC特征之前，首先根据里程距离和空间距离，进行候选帧的过滤。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
void ISCGenerationClass::loopDetection(const pcl::PointCloud<pcl::PointXYZI>::Ptr &current_pc,
                                       Eigen::Isometry3d &odom) {

  pcl::PointCloud<pcl::PointXYZI>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZI>());
  ground_filter(current_pc, pc_filtered); // 对z高度进行截取，粗略的去除地面
  ISCDescriptor desc = calculate_isc(pc_filtered); // 计算非地面点云的sc特征
  Eigen::Vector3d current_t = odom.translation(); // 当前odom位置
  //dont change push_back sequence
  if (travel_distance_arr.size() == 0) {
    travel_distance_arr.push_back(0);
  } else {
    // 这里是计算里程距离？
    double dis_temp = travel_distance_arr.back() + std::sqrt((pos_arr.back() - current_t).array().square().sum());
    travel_distance_arr.push_back(dis_temp);
  }
  pos_arr.push_back(current_t); // odom装到这里
  isc_arr.push_back(desc); // isc特征全部装到这里面

  current_frame_id = pos_arr.size() - 1;
  matched_frame_id.clear();
  
  //search for the near neibourgh pos
  int best_matched_id = 0;
  double best_score = 0.0;
  for (int i = 0; i < (int) pos_arr.size(); i++) { // 遍历之前点云帧的isc特征
    double delta_travel_distance = travel_distance_arr.back() - travel_distance_arr[i]; // 先检测是否存在距离最近的帧
    double pos_distance = std::sqrt((pos_arr[i] - pos_arr.back()).array().square().sum());  // 两帧空间距离
    // 我们认为里程距离越大，空间距离越小，越可能是回环
    if (delta_travel_distance > SKIP_NEIBOUR_DISTANCE && pos_distance < delta_travel_distance * INFLATION_COVARIANCE) {
      double geo_score = 0;
      double inten_score = 0;
      if (is_loop_pair(desc, isc_arr[i], geo_score, inten_score)) { // 计算isc的几何以及强度得分
        if (geo_score + inten_score > best_score) {  // 如果得分满足阈值，则认为是回环位置
          best_score = geo_score + inten_score;
          best_matched_id = i;
        }
      }

    }
  }
  if (best_matched_id != 0) {
    matched_frame_id.push_back(best_matched_id);
    ROS_INFO("received loop closure candidate: current: %d, history %d, total_score%f",
             current_frame_id,
             best_matched_id,
             best_score);
  }
}

这里面两个关键步骤，一是计算ISC特征，二是特征相似性计算。首先是提取ISC特征，用一张图像来表示，其中像素值存的是点云的强度？这里代码和论文中不太一致，后续再研究下。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
ISCDescriptor ISCGenerationClass::calculate_isc(const pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_pointcloud) {

  // sectors和rings是初始化参数，参考scan context
  // 初始化一个sectors*ring的图像
  ISCDescriptor isc = cv::Mat::zeros(cv::Size(sectors, rings), CV_8U);

  // 计算每一个遍历点的ring_id和sector_id
  for (int i = 0; i < (int) filtered_pointcloud->points.size(); i++) {
    ROS_WARN_ONCE(
        "intensity is %f, if intensity showed here is integer format between 1-255, please uncomment #define INTEGER_INTENSITY in iscGenerationClass.cpp and recompile",
        (double) filtered_pointcloud->points[i].intensity);
    double distance = std::sqrt(filtered_pointcloud->points[i].x * filtered_pointcloud->points[i].x
                                    + filtered_pointcloud->points[i].y * filtered_pointcloud->points[i].y);
    if (distance >= max_dis)
      continue;
    double angle = M_PI + std::atan2(filtered_pointcloud->points[i].y, filtered_pointcloud->points[i].x);
    int ring_id = std::floor(distance / ring_step);
    int sector_id = std::floor(angle / sector_step);
    if (ring_id >= rings)
      continue;
    if (sector_id >= sectors)
      continue;
#ifndef INTEGER_INTENSITY
    int intensity_temp = (int) (255 * filtered_pointcloud->points[i].intensity); // 强度是整数
#else
    int intensity_temp = (int) (filtered_pointcloud->points[i].intensity);
#endif
    if (isc.at<unsigned char>(ring_id, sector_id) < intensity_temp)
      isc.at<unsigned char>(ring_id, sector_id) = intensity_temp;  // 像素值存强度？

  }
  return isc;
}

如何比较两个ISC特征的相似性呢？这里分两步计算，第一步计算其空间距离得分，第二步计算其强度距离得分。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool ISCGenerationClass::is_loop_pair(ISCDescriptor &desc1,
                                      ISCDescriptor &desc2,
                                      double &geo_score,
                                      double &inten_score) {
  int angle = 0;
  geo_score = calculate_geometry_dis(desc1, desc2, angle); // 计算几何距离
  if (geo_score > GEOMETRY_THRESHOLD) {
    inten_score = calculate_intensity_dis(desc1, desc2, angle); // 计算强度得分
    if (inten_score > INTENSITY_THRESHOLD) {
      return true;
    }
  }
  return false;
}

这里进行两个阶段的检测，首先是快速的几何结构匹配，然后强度结构匹配。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
double ISCGenerationClass::calculate_geometry_dis(const ISCDescriptor &desc1, const ISCDescriptor &desc2, int &angle) {
  double similarity = 0.0;

  // 几何结构匹配
  for (int i = 0; i < sectors; i++) { // 列
    int match_count = 0;
    // 矩阵逐元素异或
    for (int p = 0; p < sectors; p++) {
      int new_col = p + i >= sectors ? p + i - sectors : p + i;
      for (int q = 0; q < rings; q++) {  // 行
        // 由于二值矩阵的列向量表示方位角，因此LiDAR的旋转可以通过矩阵的列移动反映．
        // 因此，为了检测视角变化，我们需要计算具有最大几何相似度的列移动
        if ((desc1.at<unsigned char>(q, p) == true && desc2.at<unsigned char>(q, new_col) == true)
            || (desc1.at<unsigned char>(q, p) == false && desc2.at<unsigned char>(q, new_col) == false)) {
          match_count++;
        }

      }
    }
    if (match_count > similarity) {
      similarity = match_count;
      angle = i;
    }
  }
  return similarity / (sectors * rings);
}
double ISCGenerationClass::calculate_intensity_dis(const ISCDescriptor &desc1, const ISCDescriptor &desc2, int &angle) {
  double difference = 1.0;
  // 强度结构匹配 计算上一步最佳列旋转后的和候选帧的ISC的密度相似度
  // 取ISC对应列的余弦距离的均值
  double angle_temp = angle;
  for (int i = angle_temp - 10; i < angle_temp + 10; i++) {

    int match_count = 0;
    int total_points = 0;
    for (int p = 0; p < sectors; p++) {
      int new_col = p + i;
      if (new_col >= sectors)
        new_col = new_col - sectors;
      if (new_col < 0)
        new_col = new_col + sectors;
      for (int q = 0; q < rings; q++) {
        match_count += abs(desc1.at<unsigned char>(q, p) - desc2.at<unsigned char>(q, new_col));
        total_points++;
      }
    }
    double diff_temp = ((double) match_count) / (sectors * rings * 255);
    if (diff_temp < difference)
      difference = diff_temp;
  }
  return 1 - difference;
}

回环检测完成后，发布实时的ISC特征图像以及回环信息。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// 发布isc特征图
    cv_bridge::CvImage out_msg;
    out_msg.header.frame_id = "velodyne";
    out_msg.header.stamp = pointcloud_time;
    out_msg.encoding = sensor_msgs::image_encodings::RGB8;
    out_msg.image = iscGeneration.getLastISCRGB();
    isc_pub.publish(out_msg.toImageMsg());

    // 发布回环检测的结果
    iscloam::LoopInfo loop;
    loop.header.stamp = pointcloud_time;
    loop.header.frame_id = "velodyne";
    loop.current_id = iscGeneration.current_frame_id;
    for (int i = 0; i < (int) iscGeneration.matched_frame_id.size(); i++) {
      loop.matched_id.push_back(iscGeneration.matched_frame_id[i]);
    }
    loop_info_pub.publish(loop);

后端优化

后端优化节点，主要接收当前的边缘、平面特征，以及odom和回环信息，然后进行全局一致性优化，这里采用GTSAM。

1
2
3
4
5
6
7
8
9
10
ros::Subscriber
    velodyne_edge_sub = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_edge", 10, pointCloudEdgeCallback);
ros::Subscriber
    velodyne_surf_sub = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf", 10, pointCloudSurfCallback);
ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>("/odom", 10, odomCallback);
ros::Subscriber loop_sub = nh.subscribe<iscloam::LoopInfo>("/loop_closure", 10, loopClosureCallback); // 在isc节点中检测回环

odom_pub = nh.advertise<nav_msgs::Odometry>("/odom_final", 100);
//map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map", 100);
path_pub = nh.advertise<nav_msgs::Path>("/final_path", 100);

在优化的线程里面，代码逻辑依然是时间戳对齐、构建因子图优化等，主要是addPoseToGraph函数。在这个函数中，主要是通过构建因子图，并将检测到的回环帧与当前帧的约束加入进来，进行增量平滑。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
bool ISCOptimizationClass::addPoseToGraph(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pointcloud_edge_in,
                                          const pcl::PointCloud<pcl::PointXYZI>::Ptr &pointcloud_surf_in,
                                          std::vector<int> &matched_frame_id,
                                          Eigen::Isometry3d &odom_in) {
  //pointcloud_arr.push_back(pointcloud_in);
  pointcloud_surf_arr.push_back(pointcloud_surf_in); // 更新特征点云的local map
  pointcloud_edge_arr.push_back(pointcloud_edge_in);
  // ROS_INFO("input pc size %d",(int)pointcloud_edge_in->points.size());
  gtsam::Pose3 pose3_current = eigenToPose3(odom_in);  // se3 eigen转gtsam

  // 第一帧边缘特征点云进来的时候初始化因子图
  if (pointcloud_edge_arr.size() <= 1) {
    pose_optimized_arr.push_back(pose3_current);
    // 因子图中添加一元因子，系统先验
    graph.push_back(gtsam::PriorFactor<gtsam::Pose3>(gtsam::Symbol('x', 1), pose_optimized_arr.front(), priorModel));
    initials.insert(gtsam::Symbol('x', 1), pose_optimized_arr.back());  // 初值容器初始化
    last_pose3 = pose3_current;  // 更新last_pose3
    return false;
  }

  // odom_original_arr、pose_optimized_arr中分别存储odom位姿和优化后的odom位姿
  odom_original_arr.push_back(last_pose3.between(pose3_current));
  pose_optimized_arr.push_back(pose_optimized_arr.back() * odom_original_arr.back());
  last_pose3 = pose3_current;
  initials.insert(gtsam::Symbol('x', pointcloud_edge_arr.size()), pose_optimized_arr.back()); // 加入一个变量
  // 因子图中加入二元因子，相邻节点之间边的约束，通过相邻帧边缘点来关联
  graph.push_back(gtsam::BetweenFactor<gtsam::Pose3>(gtsam::Symbol('x', pointcloud_edge_arr.size() - 1),
                                                     gtsam::Symbol('x', pointcloud_edge_arr.size()),
                                                     odom_original_arr.back(),
                                                     odomModel));

  if (stop_check_loop_count > 0) {  // 等于0表明未检测到回环帧
    stop_check_loop_count--;
    return false;
  }
  // 如果检测到回环，则在因子图中添加相关约束
  if (matched_frame_id.size() != 0) {
    //if loop closure detected
    // 对每一个回环帧构建约束并优化
    for (int i = 0; i < (int) matched_frame_id.size(); i++) {
      //get initial guess
      // 回环帧与当前帧的变换矩阵
      gtsam::Pose3 transform_pose3 = pose_optimized_arr[matched_frame_id[i]].between(pose_optimized_arr.back());
      //ROS_WARN("pose %f,%f,%f, [%f,%f,%f,%f]",transform_pose3.translation().x(),transform_pose3.translation().y(),transform_pose3.translation().z(),transform_pose3.rotation().toQuaternion().w(),transform_pose3.rotation().toQuaternion().x(),transform_pose3.rotation().toQuaternion().y(),transform_pose3.rotation().toQuaternion().z());
      Eigen::Isometry3d
          transform = pose3ToEigen(pose_optimized_arr[matched_frame_id[i]].between(pose_optimized_arr.back()));
      // 用ceres构建回环帧与当前帧的线面约束并优化，存在transform中
      if (geometryConsistencyVerification(pointcloud_edge_arr.size() - 1, matched_frame_id[i], transform)) {

        // 添加回环得到的二元因子
        gtsam::Pose3 loop_temp = eigenToPose3(transform);
        graph.push_back(gtsam::BetweenFactor<gtsam::Pose3>(gtsam::Symbol('x', matched_frame_id[i] + 1),
                                                           gtsam::Symbol('x', pointcloud_edge_arr.size()),
                                                           loop_temp,
                                                           loopModel));
        gtsam::Values result = gtsam::LevenbergMarquardtOptimizer(graph, initials).optimize(); // LM法进行优化，获取优化变量的最优解

        // 如果回环帧有效则更新因子图
        if (updateStates(result, matched_frame_id[i], pointcloud_edge_arr.size() - 1)) {
          stop_check_loop_count = STOP_LOOP_CHECK_COUNTER; // 只检测相邻40帧
          ROS_WARN("global optimization finished%d,%d with tranform %f,%f,%f, [%f,%f,%f,%f]",
                   pointcloud_edge_arr.size() - 1,
                   matched_frame_id[i],
                   loop_temp.translation().x(),
                   loop_temp.translation().y(),
                   loop_temp.translation().z(),
                   loop_temp.rotation().toQuaternion().w(),
                   loop_temp.rotation().toQuaternion().x(),
                   loop_temp.rotation().toQuaternion().y(),
                   loop_temp.rotation().toQuaternion().z());
          return true;
        } else {
          stop_check_loop_count = 2;
        }
      } else {
        stop_check_loop_count = 2;
      }
    }
  }
  return false;
}

其中geometryConsistencyVerification函数会通过构建回环帧到当前帧的点线、点面残差来优化得到两帧之间的transform。这里还有些疑问，懂的同学可以在下面评论补充。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
bool ISCOptimizationClass::geometryConsistencyVerification(int current_id,
                                                           int matched_id,
                                                           Eigen::Isometry3d &transform) {

  pcl::PointCloud<pcl::PointXYZI>::Ptr map_surf(new pcl::PointCloud<pcl::PointXYZI>());
  pcl::PointCloud<pcl::PointXYZI>::Ptr map_edge(new pcl::PointCloud<pcl::PointXYZI>());

  // 回环候选帧只有最近的40帧?
  for (int i = -20; i <= 20; i = i + 5) {
    if (matched_id + i >= current_id || matched_id + i < 0)
      continue;

    // 计算回环帧与相邻帧的转换，并把点云加到map中
    Eigen::Isometry3d transform_pose = pose3ToEigen(pose_optimized_arr[matched_id + i]);
    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_temp(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::transformPointCloud(*pointcloud_surf_arr[matched_id + i], *transformed_temp, transform_pose.cast<float>());
    *map_surf += *transformed_temp;
    pcl::transformPointCloud(*pointcloud_edge_arr[matched_id + i], *transformed_temp, transform_pose.cast<float>());
    *map_edge += *transformed_temp;
  }

  // 回环帧的pose
  Eigen::Isometry3d transform_pose = pose3ToEigen(pose_optimized_arr[matched_id]);
  pcl::transformPointCloud(*map_edge, *map_edge, transform_pose.cast<float>().inverse());
  pcl::transformPointCloud(*map_surf, *map_surf, transform_pose.cast<float>().inverse());
  //downsample
  downSizeFilter.setInputCloud(map_surf);
  downSizeFilter.filter(*map_surf);

  pcl::PointCloud<pcl::PointXYZI>::Ptr current_scan_surf(new pcl::PointCloud<pcl::PointXYZI>());
  pcl::PointCloud<pcl::PointXYZI>::Ptr current_scan_edge(new pcl::PointCloud<pcl::PointXYZI>());
  for (int i = 0; i <= 0; i = i + 3) {
    if (current_id + i < 0)
      continue;
    // 当前帧与相邻帧的转换
    Eigen::Isometry3d transform_pose = pose3ToEigen(pose_optimized_arr[current_id + i]);
    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_temp(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::transformPointCloud(*pointcloud_surf_arr[current_id + i], *transformed_temp, transform_pose.cast<float>());
    *current_scan_surf += *transformed_temp;
    pcl::transformPointCloud(*pointcloud_edge_arr[current_id + i], *transformed_temp, transform_pose.cast<float>());
    *current_scan_edge += *transformed_temp;
  }

  transform_pose = pose3ToEigen(pose_optimized_arr[current_id]);
  pcl::transformPointCloud(*current_scan_edge, *current_scan_edge, transform_pose.cast<float>().inverse());
  pcl::transformPointCloud(*current_scan_surf, *current_scan_surf, transform_pose.cast<float>().inverse());
  //downsample
  downSizeFilter.setInputCloud(current_scan_surf);
  downSizeFilter.filter(*current_scan_surf);

  //this is for visualization
  transform_pose = transform;
  transform_pose.translation() = Eigen::Vector3d(0, 0, 10);
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZI>());
  pcl::transformPointCloud(*current_scan_surf, *loop_candidate_pc, transform_pose.cast<float>());
  pcl::transformPointCloud(*current_scan_edge, *cloud_temp, transform_pose.cast<float>());
  *loop_candidate_pc += *cloud_temp;
  loop_map_pc = map_surf;
  *loop_map_pc += *map_edge;

  double match_score = estimateOdom(map_edge, map_surf, current_scan_edge, current_scan_surf, transform);
  //ROS_WARN("matched score %f",match_score);
  if (match_score < LOOPCLOSURE_THRESHOLD)
    return true;
  else {
    //ROS_INFO("loop rejected due to geometry verification current_id%d matched_id %d, score: %f",current_id,matched_id,match_score);
    return false;
  }

}

updateStates函数会对GSTAM优化的结果，计算的偏移进行校验，通过阈值判定是否有效。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
bool ISCOptimizationClass::updateStates(gtsam::Values &result, int matched_id, int current_id) {
  //verify states first
  double sum_residual_q = 0.0;
  double sum_residual_t = 0.0;
  int total_counter = 0;
  for (int i = current_id - STOP_LOOP_CHECK_COUNTER - 10; i < current_id; i++) {
    if (i < 0) continue;
    total_counter++;
    gtsam::Pose3 pose_temp1 = result.at<gtsam::Pose3>(gtsam::Symbol('x', current_id + 1)); // 当前相邻帧的pose
    gtsam::Pose3 pose_temp2 = result.at<gtsam::Pose3>(gtsam::Symbol('x', i + 1));
    gtsam::Pose3 tranform1 = pose_temp2.between(pose_temp1);
    gtsam::Pose3 tranform2 = pose_optimized_arr[i].between(pose_optimized_arr[current_id]);
    gtsam::Pose3 tranform = tranform1.between(tranform2);

    // 计算t q偏移量?
    sum_residual_t += std::abs(tranform.translation().x()) + std::abs(tranform.translation().y())
        + std::abs(tranform.translation().z());
    sum_residual_q +=
        std::abs(tranform.rotation().toQuaternion().w() - 1) + std::abs(tranform.rotation().toQuaternion().x())
            + std::abs(tranform.rotation().toQuaternion().y()) + std::abs(tranform.rotation().toQuaternion().z());
  }
  for (int i = matched_id - STOP_LOOP_CHECK_COUNTER - 10; i < matched_id; i++) {
    if (i < 0) continue;
    total_counter++;
    gtsam::Pose3 pose_temp1 = result.at<gtsam::Pose3>(gtsam::Symbol('x', matched_id + 1));
    gtsam::Pose3 pose_temp2 = result.at<gtsam::Pose3>(gtsam::Symbol('x', i + 1));
    gtsam::Pose3 tranform1 = pose_temp2.between(pose_temp1);
    gtsam::Pose3 tranform2 = pose_optimized_arr[i].between(pose_optimized_arr[matched_id]);
    gtsam::Pose3 tranform = tranform1.between(tranform2);
    sum_residual_t += std::abs(tranform.translation().x()) + std::abs(tranform.translation().y())
        + std::abs(tranform.translation().z());
    sum_residual_q +=
        std::abs(tranform.rotation().toQuaternion().w() - 1) + std::abs(tranform.rotation().toQuaternion().x())
            + std::abs(tranform.rotation().toQuaternion().y()) + std::abs(tranform.rotation().toQuaternion().z());
  }
  sum_residual_q = sum_residual_q / total_counter;
  sum_residual_t = sum_residual_t / total_counter;
  //ROS_INFO("optimization discard due to frame unaligned, residual_q:%f, residual_t:%f",sum_residual_q,sum_residual_t);

  if (sum_residual_q > 0.02 || sum_residual_t > 0.5) {
    ROS_INFO("optimization discard due to frame unaligned, residual_q:%f, residual_t:%f",
             sum_residual_q,
             sum_residual_t);
    //graph.pop_back();
    graph.remove(graph.size() - 1);
    return false;
  }
  //update states
  initials.clear();
  for (int i = 0; i < (int) result.size(); i++) {
    pose_optimized_arr[i] = result.at<gtsam::Pose3>(gtsam::Symbol('x', i + 1));
    initials.insert(gtsam::Symbol('x', i + 1), pose_optimized_arr[i]);
  }
  return true;

}

总结

核心部分在于回环检测，ISC特征对于初步的回环帧的筛选还比较有用，其精度保证主要在ceres和GTSAM优化的结果。真正的判定ISC特征描述符的表征环境的能力，可能需要全局定位来验证，系统更多细节还需要进一步深入。另外这里只用了激光雷达，改进空间还比较大。

-------------　　　　本文结束　　感谢您的阅读　　　　-------------
胡想成 wechat
欢迎关注个人公众号！
您的支持，是我装逼的最大动力！

本文标题:开源SLAM系统：ISCLOAM源码解析

文章作者:胡想成

发布时间:2020年08月17日 - 22:08

最后更新:2020年08月18日 - 12:08

原始链接:xchu.net/2020/08/17/50iscloam/

版权声明:本博客所有文章除特别声明外，均采用 CC BY-NC-SA 4.0许可协议,转载请注明出处！