开源SLAM系统:ISCLOAM源码解析

作者发表在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许可协议,转载请注明出处!