作者发表在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

简介
此系统的前端odometry部分还是基于作者原来开源的FLOAM,参考我的笔记
http://xchu.net/2020/08/17/49floam/#more
。这里在引入了作者设计的ISC特征描述符,在回环检测的时候效率和精度都能保持在一定水平。如上图左侧可以看到根据点云实时提取的ISC特征。在下面终端输出的消息可以看到,其顺利的检测到了回环。

其主要节点图如下:

可以看到其前端和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 ¤t_pc, Eigen::Isometry3d &odom) {
pcl::PointCloud<pcl::PointXYZI>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZI>()); ground_filter(current_pc, pc_filtered); ISCDescriptor desc = calculate_isc(pc_filtered); Eigen::Vector3d current_t = odom.translation(); 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); isc_arr.push_back(desc);
current_frame_id = pos_arr.size() - 1; matched_frame_id.clear(); int best_matched_id = 0; double best_score = 0.0; for (int i = 0; i < (int) pos_arr.size(); i++) { 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)) { 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) {
ISCDescriptor isc = cv::Mat::zeros(cv::Size(sectors, rings), CV_8U);
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++) { 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; 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
| 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);
odom_pub = nh.advertise<nav_msgs::Odometry>("/odom_final", 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_surf_arr.push_back(pointcloud_surf_in); pointcloud_edge_arr.push_back(pointcloud_edge_in); gtsam::Pose3 pose3_current = eigenToPose3(odom_in);
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; return false; }
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) { stop_check_loop_count--; return false; } if (matched_frame_id.size() != 0) { for (int i = 0; i < (int) matched_frame_id.size(); i++) { gtsam::Pose3 transform_pose3 = pose_optimized_arr[matched_frame_id[i]].between(pose_optimized_arr.back()); Eigen::Isometry3d transform = pose3ToEigen(pose_optimized_arr[matched_frame_id[i]].between(pose_optimized_arr.back())); 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();
if (updateStates(result, matched_frame_id[i], pointcloud_edge_arr.size() - 1)) { stop_check_loop_count = STOP_LOOP_CHECK_COUNTER; 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>());
for (int i = -20; i <= 20; i = i + 5) { if (matched_id + i >= current_id || matched_id + i < 0) continue;
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; }
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()); 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()); downSizeFilter.setInputCloud(current_scan_surf); downSizeFilter.filter(*current_scan_surf);
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); if (match_score < LOOPCLOSURE_THRESHOLD) return true; else { 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) { 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)); 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);
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;
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.remove(graph.size() - 1); return false; } 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特征描述符的表征环境的能力,可能需要全局定位来验证,系统更多细节还需要进一步深入。另外这里只用了激光雷达,改进空间还比较大。