forked from Yixin-F/better_fastlio2
-
Notifications
You must be signed in to change notification settings - Fork 0
/
laserMapping.cpp
executable file
·2583 lines (2248 loc) · 121 KB
/
laserMapping.cpp
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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include "common_lib.h"
#include "preprocess.h"
#include <darknet_ros_msgs/BoundingBox.h>
#include <darknet_ros_msgs/BoundingBoxes.h>
#include <ikd-Tree/ikd_Tree.h>
#include "IMU_Processing.hpp"
#include"sc-relo/Scancontext.h"
#include"dynamic-remove/tgrs.h"
#define INIT_TIME (0.1)
#define LASER_POINT_COV (0.001)
#define MAXN (720000)
#define PUBFRAME_PERIOD (20)
// kdtree建立时间; kdtree搜索时间; kdtree删除时间;
double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;
// T1雷达初始时间戳; s_plot为整个流程耗时; 2特征点数量; 3kdtree增量时间; 4kdtree搜索耗时; 5kdtree删除点数量;
// 6kdtree删除耗时; 7kdtree初始大小; 8kdtree结束大小; 9平均消耗时间; 10添加点数量; 11点云预处理的总时间;
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN];
// 匹配时间; 求解时间; 求解H矩阵时间;
double match_time = 0, solve_time = 0, solve_const_H_time = 0;
// ikdtree获得的节点数; ikdtree结束时的节点数; 添加点的数量;删除点的数量;
int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;
// common
bool savePCD = false, saveSCD = false, saveLOG = false, map_save_en = false, time_sync_en = false;
string rootDir, savePCDDirectory, saveSCDDirectory, saveLOGDirectory;
string lid_topic, camera_topic, imu_topic; // topic;
string root_dir = ROOT_DIR; // TODO: ?
// segment
float sensor_height;
bool ground_en = false, tollerance_en = false;
float z_tollerance;
float rotation_tollerance;
// mapping
bool extrinsic_est_en = true; // 在线标定
float DET_RANGE = 300.0f; // 当前雷达系中心到各个地图边缘的距离;
// 雷达时间戳;imu时间戳;
double last_timestamp_lidar = 0, last_timestamp_imu = -1.0;
// 残差平均值;残差和;
double res_mean_last = 0.05, total_residual = 0.0;
// imu的角速度协方差;加速度协方差;角速度协方差偏置;加速度协方差偏置;
double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001;
float res_last[100000] = {0.0}; // 残差,点到面距离平方和;
const float MOV_THRESHOLD = 1.5f; // 当前雷达系中心到各个地图边缘的权重;
// 地图的最小尺寸;视野角度;
double filter_size_map_min, fov_deg = 0;
int kd_step = 0;
float mappingSurfLeafSize; // map filter
// 立方体长度;视野一半的角度;视野总角度;总距离;雷达结束时间;雷达初始时间;
double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;
// 有效特征点数;时间log计数器;接收到的激光雷达Msg的总数;接收到的IMU的Msg的总数;
int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
// 迭代次数;下采样的点数;最大迭代次数;有效点数;
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, pcd_save_interval = 1, pcd_index = 0;
bool point_selected_surf[100000] = {0}; // 是否为平面特征点
// 是否把雷达从buffer送到meas中;是否第一帧扫描;是否收到退出的中断;flg_EKF_inited判断EKF是否初始化完成;
bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited;
float keyframeAddingDistThreshold; // 判断是否为关键帧的距离阈值,yaml
float keyframeAddingAngleThreshold; // 判断是否为关键帧的角度阈值,yaml
float surroundingKeyframeDensity;
std::vector<pose> update_nokf_poses;
// loop
bool startFlag = true;
bool loopClosureEnableFlag;
float loopClosureFrequency; // 回环检测频率
float historyKeyframeSearchRadius; // 回环检测radius kdtree搜索半径
float historyKeyframeSearchTimeDiff; // 帧间时间阈值
int historyKeyframeSearchNum; // 回环时多少个keyframe拼成submap
float historyKeyframeFitnessScore; // icp匹配阈值
bool potentialLoopFlag = false;
// publish
bool path_en = true;
// 是否发布激光雷达数据;是否发布稠密数据;是否发布激光雷达数据的车体数据;
bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
mutex mtx_buffer; // 互斥锁;
condition_variable sig_buffer; // 条件变量;
vector<vector<int>> pointSearchInd_surf; // 每个点的索引,暂时没用到
vector<BoxPointType> cub_needrm; // ikdtree中,地图需要移除的包围盒序列
vector<PointVector> Nearest_Points; // 每个点的最近点序列
vector<double> extrinT(3, 0.0); // 雷达相对于IMU的外参T
vector<double> extrinR(9, 0.0); // 雷达相对于IMU的外参R
deque<double> time_buffer; // 激光雷达数据时间戳缓存队列
deque<pcl::PointCloud<PointType>::Ptr> lidar_buffer; // 记录特征提取或间隔采样后的lidar(特征)数据
deque<sensor_msgs::Imu::ConstPtr> imu_buffer; // IMU数据缓存队列
pcl::PointCloud<PointType>::Ptr featsFromMap(new pcl::PointCloud<PointType>()); // 提取地图中的特征点,ikdtree获得
pcl::PointCloud<PointType>::Ptr feats_undistort(new pcl::PointCloud<PointType>()); // 去畸变的点云,lidar系
pcl::PointCloud<PointType>::Ptr feats_undistort_copy(new pcl::PointCloud<PointType>()); // 去畸变的点云,lidar系
pcl::PointCloud<PointType>::Ptr feats_down_body(new pcl::PointCloud<PointType>()); // 畸变纠正后降采样的单帧点云,lidar系
pcl::PointCloud<PointType>::Ptr feats_down_world(new pcl::PointCloud<PointType>()); // 畸变纠正后降采样的单帧点云,world系
pcl::PointCloud<PointType>::Ptr normvec(new pcl::PointCloud<PointType>(100000, 1)); // 特征点在地图中对应点的,局部平面参数,world系
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>(100000, 1)); // 畸变纠正后降采样的单帧点云,imu(body)系
pcl::PointCloud<PointType>::Ptr corr_normvect(new pcl::PointCloud<PointType>(100000, 1)); // 对应点法相量
pcl::PointCloud<PointType>::Ptr _featsArray; // ikdtree中,map需要移除的点云
pcl::VoxelGrid<PointType> downSizeFilterSurf; // 单帧内降采样使用voxel grid
pcl::VoxelGrid<PointType> downSizeFilterMap; // 未使用
// KD_TREE ikdtree; // ikdtree old 类
KD_TREE<PointType> ikdtree; // ikdtree new 类
// TODO: 做全局sc回环其实没必要，现有版本是先最近位姿搜索再使用scan2map的sc匹配
ScanContext::SCManager scLoop; // sc 类
// giseop，Scan Context的输入格式
enum class SCInputType {
SINGLE_SCAN_FULL,
MULTI_SCAN_FEAT
};
V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0);
V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0);
V3D euler_cur; // 当前的欧拉角
V3D position_last(Zero3d); // 上一帧的位置
V3D Lidar_T_wrt_IMU(Zero3d); // T lidar to imu (imu = r * lidar + t)
M3D Lidar_R_wrt_IMU(Eye3d); // R lidar to imu (imu = r * lidar + t)
// EKF inputs and output
MeasureGroup Measures;
esekfom::esekf<state_ikfom, 12, input_ikfom> kf; // 状态,噪声维度,输入
state_ikfom state_point; // 状态
vect3 pos_lid; // world系下lidar坐标
vect3 pos_lid_copy;
// 输出的路径参数
nav_msgs::Path path; // 包含了一系列位姿
nav_msgs::Odometry odomAftMapped; // 只包含了一个位姿
geometry_msgs::Quaternion geoQuat; // 四元数
geometry_msgs::PoseStamped msg_body_pose; // 位姿
nav_msgs::Path path_imu;
geometry_msgs::PoseStamped msg_imu_pose;
geometry_msgs::Quaternion geoQuat_imu;
shared_ptr<Preprocess> p_pre(new Preprocess()); // 定义指向激光雷达数据的预处理类Preprocess的智能指针
shared_ptr<ImuProcess> p_imu(new ImuProcess()); // 定义指向IMU数据预处理类ImuProcess的智能指针
// back end
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames; // 历史所有关键帧的角点集合(降采样)
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames; // 历史所有关键帧的平面点集合(降采样)
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D(new pcl::PointCloud<PointType>()); // 历史关键帧位姿(位置)
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>()); // 历史关键帧位姿
pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>());
pcl::PointCloud<PointTypePose>::Ptr fastlio_unoptimized_cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>()); // 存储fastlio 未优化的位姿
ros::Publisher pubHistoryKeyFrames; // 发布loop history keyframe submap
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRecentKeyFrame;
ros::Publisher pubCloudRegisteredRaw;
ros::Publisher pubLoopConstraintEdge;
ros::Publisher pubLidarPCL;
std::vector<livox_ros_driver::CustomMsgConstPtr> livox_msg;
bool aLoopIsClosed = false;
map<int, int> loopIndexContainer; // from new to old
vector<pair<int, int>> loopIndexQueue; // 回环索引队列
vector<gtsam::Pose3> loopPoseQueue; // 回环位姿队列
vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue; // 回环噪声队列
deque<std_msgs::Float64MultiArray> loopInfoVec;
nav_msgs::Path globalPath;
// 局部关键帧构建的map点云,对应kdtree,用于scan-to-map找相邻点
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses(new pcl::KdTreeFLANN<PointType>());
// 降采样
pcl::VoxelGrid<PointType> downSizeFilterCorner;
// pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterICP;
float transformTobeMapped[6]; // TODO: 当前帧的位姿(world系下),x,y,z,roll,pitch,yaw + tollorance
std::mutex mtx;
std::mutex mtxLoopInfo;
TGRS remover;
// gtsam
gtsam::NonlinearFactorGraph gtSAMgraph; // 实例化一个空的因子图
gtsam::Values initialEstimate;
gtsam::Values optimizedEstimate;
gtsam::ISAM2 *isam;
gtsam::Values isamCurrentEstimate;
Eigen::MatrixXd poseCovariance;
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubOptimizedGlobalMap; // 发布最后优化的地图
bool recontructKdTree = false;
int updateKdtreeCount = 0; // 每100次更新一次
bool visulize_IkdtreeMap = false; // visual iktree submap
// global map visualization radius
float globalMapVisualizationSearchRadius;
float globalMapVisualizationPoseDensity;
float globalMapVisualizationLeafSize;
// saveMap
ros::ServiceServer srvSaveMap;
ros::ServiceServer srvSavePose;
// pose-graph saver
std::fstream pgSaveStream; // pg: pose-graph
std::vector<std::string> edges_str;
std::vector<std::string> vertices_str;
/*---------------------usb_cam---------------------*/
#define Hmax 720
#define Wmax 1280
bool camera_en;
vector<double> cam_ex(16, 0.0);
vector<double> cam_in(12, 0.0);
cv::Vec3b image_color[Hmax][Wmax]; // 每个像素点的BGR值用一个三维向量表示
cv::Mat externalMat(4, 4, cv::DataType<double>::type); // 外参旋转矩阵3*3和平移向量3*1
cv::Mat internalMatProject(3, 4, cv::DataType<double>::type); // 内参3*4的投影矩阵,最后一列是三个零
struct Box
{
int xmin;
int xmax;
int ymin;
int ymax;
};
std::vector<Box> box;
// camera话题rgb回调函数
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
for (int row = 0; row < Hmax; row++)
{
for (int col = 0; col < Wmax; col++)
{
image_color[row][col] = (cv::Vec3b)image.at<cv::Vec3b>(row, col);
}
}
// // cut images
// for (int i = 0; i < box.size(); i++)
// {
// for (int row = 0; row < Hmax; row++)
// {
// for (int col = 0; col < Wmax; col++)
// {
// if (col > box[i].xmin - 20 && col < box[i].xmax + 220 && row > box[i].ymin - 20 && row < box[i].ymax + 20)
// {
// image_color[row][col] = {0, 0, 255};
// }
// }
// }
// }
}
// 参数设置:camera内参、lidar系到camera系的外参
void paramSetting(void)
{
externalMat = (cv::Mat_<double>(4, 4) << cam_ex[0], cam_ex[1], cam_ex[2], cam_ex[3],
cam_ex[4], cam_ex[5], cam_ex[6], cam_ex[7],
cam_ex[8], cam_ex[9], cam_ex[10], cam_ex[11],
cam_ex[12], cam_ex[13], cam_ex[14], cam_ex[15]);
internalMatProject = (cv::Mat_<double>(3, 4) << cam_in[0], cam_in[1], cam_in[2], cam_in[3],
cam_in[4], cam_in[5], cam_in[6], cam_in[7],
cam_in[8], cam_in[9], cam_in[10], cam_in[11]);
}
// TODO: 目标检测投影框回调函数
void BoxCallback(const darknet_ros_msgs::BoundingBoxes::ConstPtr &msg)
{
box.clear();
for (int i = 0; i < msg->bounding_boxes.size(); i++)
{
if (msg->bounding_boxes[i].Class == "person" && msg->bounding_boxes[i].probability > 0.6)
{
Box boxi;
boxi.xmin = msg->bounding_boxes[i].xmin;
boxi.xmax = msg->bounding_boxes[i].xmax;
boxi.ymin = msg->bounding_boxes[i].ymin;
boxi.ymax = msg->bounding_boxes[i].ymax;
box.push_back(boxi);
}
}
}
// 发布彩色点云到world系, only for avia livox (not mid360)
void publish_frame_world_color(const ros::Publisher &pubLaserCloudColor)
{
if (p_pre->lidar_type == LIVOX && camera_en)
{
if (scan_pub_en) // 是否发布激光雷达数据
{
// 发布稠密点云还是降采样点云
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
sensor_msgs::PointCloud2 laserCloudFullResmsg;
pcl::PointCloud<livox_ros::Point>::Ptr livox_color_ptr(new pcl::PointCloud<livox_ros::Point>); // 带颜色的点云
pcl::PointCloud<livox_ros::Point>::Ptr livox_raw_ptr(new pcl::PointCloud<livox_ros::Point>); // 不带颜色的点云
for (int i = 0; i < laserCloudFullRes->size(); i++)
{
livox_ros::Point livox_raw_point;
livox_raw_point.x = laserCloudFullRes->points[i].x;
livox_raw_point.y = laserCloudFullRes->points[i].y;
livox_raw_point.z = laserCloudFullRes->points[i].z;
livox_raw_point.intensity = laserCloudFullRes->points[i].intensity;
livox_raw_ptr->points.push_back(livox_raw_point);
}
cv::Mat coordinateCloud(4, 1, cv::DataType<double>::type); // 点云坐标系下的x,y,z,1向量
cv::Mat coordinateCamera(3, 1, cv::DataType<double>::type); // 相机坐标系下的x,y,z向量
for (int i = 0; i < livox_raw_ptr->points.size(); i++)
{
coordinateCloud.at<double>(0, 0) = livox_raw_ptr->points[i].x;
coordinateCloud.at<double>(1, 0) = livox_raw_ptr->points[i].y;
coordinateCloud.at<double>(2, 0) = livox_raw_ptr->points[i].z;
coordinateCloud.at<double>(3, 0) = 1;
coordinateCamera = internalMatProject * externalMat * coordinateCloud; // 点云坐标转相机坐标
cv::Point pixel; // 像素坐标
pixel.x = coordinateCamera.at<double>(0, 0) / coordinateCamera.at<double>(0, 2);
pixel.y = coordinateCamera.at<double>(1, 0) / coordinateCamera.at<double>(0, 2);
if (pixel.x >= 0 && pixel.x < Wmax && pixel.y >= 0 && pixel.y < Hmax && livox_raw_ptr->points[i].x > 0) // && livox_raw_ptr->points[i].x>0去掉图像后方的点云
{
livox_ros::Point livoxPoint;
livoxPoint.x = livox_raw_ptr->points[i].x;
livoxPoint.y = livox_raw_ptr->points[i].y;
livoxPoint.z = livox_raw_ptr->points[i].z;
livoxPoint.b = image_color[pixel.y][pixel.x][0];
livoxPoint.g = image_color[pixel.y][pixel.x][1];
livoxPoint.r = image_color[pixel.y][pixel.x][2];
livox_color_ptr->points.push_back(livoxPoint);
// if (livoxPoint.b != 0 && livoxPoint.g != 0 && livoxPoint.r != 255)
// {
// livox_color_ptr->points.push_back(livoxPoint);
// }
}
}
pcl::PointCloud<livox_ros::Point>::Ptr livox_color_world_ptr(livox_color_ptr);
// 转换到world系
for (int i = 0; i < livox_color_ptr->points.size(); i++)
{
V3D p_body(livox_color_ptr->points[i].x, livox_color_ptr->points[i].y, livox_color_ptr->points[i].z);
V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);
livox_color_world_ptr->points[i].x = p_global(0);
livox_color_world_ptr->points[i].y = p_global(1);
livox_color_world_ptr->points[i].z = p_global(2);
}
sensor_msgs::PointCloud2 livox_color_world_msg;
livox_color_world_ptr->width = 1;
livox_color_world_ptr->height = livox_color_world_ptr->points.size();
pcl::toROSMsg(*livox_color_world_ptr, livox_color_world_msg);
livox_color_world_msg.header.frame_id = "camera_init";
pubLaserCloudColor.publish(livox_color_world_msg);
publish_count -= PUBFRAME_PERIOD;
}
}
}
// 更新里程计轨迹
void updatePath(const PointTypePose &pose_in)
{
string odometryFrame = "camera_init";
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose.position.x = pose_in.x;
pose_stamped.pose.position.y = pose_in.y;
pose_stamped.pose.position.z = pose_in.z;
tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
pose_stamped.pose.orientation.x = q.x();
pose_stamped.pose.orientation.y = q.y();
pose_stamped.pose.orientation.z = q.z();
pose_stamped.pose.orientation.w = q.w();
Eigen::Matrix3d R = Exp((double)pose_in.roll, (double)pose_in.pitch, (double)pose_in.yaw);
Eigen::Vector3d t((double)pose_in.x, (double)pose_in.y, (double)pose_in.z);
pose update_pose;
update_pose.R = R;
update_pose.t = t;
update_nokf_poses.emplace_back(update_pose);
// fout_update_pose << std::fixed << R(0, 0) << " " << R(0, 1) << " " << R(0, 2) << " " << t[0] << " "
// << R(1, 0) << " " << R(1, 1) << " " << R(1, 2) << " " << t[1] << " "
// << R(2, 0) << " " << R(2, 1) << " " << R(2, 2) << " " << t[2] << std::endl;
globalPath.poses.push_back(pose_stamped);
}
inline float constraintTransformation(float value, float limit)
{
if (value < -limit)
value = -limit;
if (value > limit)
value = limit;
return value;
}
// Get the Cur Pose object将更新的pose赋值到transformTobeMapped中来表示位姿（因子图要用）
void getCurPose(state_ikfom cur_state)
{
// 欧拉角是没有群的性质,所以从SO3还是一般的rotation matrix,转换过来的结果一样
Eigen::Vector3d eulerAngle = cur_state.rot.matrix().eulerAngles(2, 1, 0); // yaw-pitch-roll,单位:弧度
transformTobeMapped[0] = eulerAngle(2); // roll
transformTobeMapped[1] = eulerAngle(1); // pitch
transformTobeMapped[2] = eulerAngle(0); // yaw
transformTobeMapped[3] = cur_state.pos(0); // x
transformTobeMapped[4] = cur_state.pos(1); // y
transformTobeMapped[5] = cur_state.pos(2); // z
if(tollerance_en){ // TODO: human constraint in z and roll oitch
transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance); // roll
transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance); // pitch
transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
}
}
// TODO: rviz展示回环边, can be used for relocalization
void visualizeLoopClosure()
{
ros::Time timeLaserInfoStamp = ros::Time().fromSec(lidar_end_time); // 时间戳
string odometryFrame = "camera_init";
if (loopIndexContainer.empty())
return;
visualization_msgs::MarkerArray markerArray;
// 回环顶点
visualization_msgs::Marker markerNode;
markerNode.header.frame_id = odometryFrame;
markerNode.header.stamp = timeLaserInfoStamp;
// action对应的操作:ADD=0、MODIFY=0、DELETE=2、DELETEALL=3,即添加、修改、删除、全部删除
markerNode.action = visualization_msgs::Marker::ADD;
// 设置形状:球体
markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
markerNode.ns = "loop_nodes";
markerNode.id = 0;
markerNode.pose.orientation.w = 1;
// 尺寸
markerNode.scale.x = 0.3;
markerNode.scale.y = 0.3;
markerNode.scale.z = 0.3;
// 颜色
markerNode.color.r = 0;
markerNode.color.g = 0.8;
markerNode.color.b = 1;
markerNode.color.a = 1;
// 回环边
visualization_msgs::Marker markerEdge;
markerEdge.header.frame_id = odometryFrame;
markerEdge.header.stamp = timeLaserInfoStamp;
markerEdge.action = visualization_msgs::Marker::ADD;
// 设置形状:线
markerEdge.type = visualization_msgs::Marker::LINE_LIST;
markerEdge.ns = "loop_edges";
markerEdge.id = 1;
markerEdge.pose.orientation.w = 1;
markerEdge.scale.x = 0.1;
markerEdge.color.r = 0.9;
markerEdge.color.g = 0.9;
markerEdge.color.b = 0;
markerEdge.color.a = 1;
// 遍历回环
for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
{
int key_cur = it->first;
int key_pre = it->second;
geometry_msgs::Point p;
p.x = copy_cloudKeyPoses6D->points[key_cur].x;
p.y = copy_cloudKeyPoses6D->points[key_cur].y;
p.z = copy_cloudKeyPoses6D->points[key_cur].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
p.x = copy_cloudKeyPoses6D->points[key_pre].x;
p.y = copy_cloudKeyPoses6D->points[key_pre].y;
p.z = copy_cloudKeyPoses6D->points[key_pre].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
}
markerArray.markers.push_back(markerNode);
markerArray.markers.push_back(markerEdge);
pubLoopConstraintEdge.publish(markerArray);
}
// 计算当前帧与前一帧位姿变换,如果变化太小,不设为关键帧,反之设为关键帧
bool saveFrame()
{
if (cloudKeyPoses3D->points.empty())
return true;
// 前一帧位姿,注:最开始没有的时候,在函数extractCloud里面有
Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
// 当前帧位姿
Eigen::Affine3f transFinal = trans2Affine3f(transformTobeMapped);
// 位姿变换增量
Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
float x, y, z, roll, pitch, yaw;
// pcl::getTranslationAndEulerAngles是根据仿射矩阵计算x,y,z,roll,pitch,yaw
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); // 获取上一帧相对当前帧的位姿
// 旋转和平移量都较小,当前帧不设为关键帧
if (abs(roll) < keyframeAddingAngleThreshold &&
abs(pitch) < keyframeAddingAngleThreshold &&
abs(yaw) < keyframeAddingAngleThreshold &&
sqrt(x * x + y * y + z * z) < keyframeAddingDistThreshold)
return false;
return true;
}
// 添加激光里程计因子
void addOdomFactor()
{
// 如果是第一帧
if (cloudKeyPoses3D->points.empty())
{
// 给出一个噪声模型,也就是协方差矩阵
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12).finished());
// 加入先验因子PriorFactor,固定这个顶点,对第0个节点增加约束
gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
// 节点设置初始值,将这个顶点的值加入初始值中
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
// 变量节点设置初始值
writeVertex(0, trans2gtsamPose(transformTobeMapped), vertices_str);
}
// 不是第一帧,增加帧间约束
else
{
// 添加激光里程计因子
gtsam::noiseModel::Diagonal::shared_ptr odometryNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back()); // 上一个位姿
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped); // 当前位姿
gtsam::Pose3 relPose = poseFrom.between(poseTo);
// 参数:前一帧id;当前帧id;前一帧与当前帧的位姿变换poseFrom.between(poseTo) = poseFrom.inverse()*poseTo;噪声协方差;
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
// 变量节点设置初始值,将这个顶点的值加入初始值中
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
writeVertex(cloudKeyPoses3D->size(), poseTo, vertices_str);
writeEdge({cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size()}, relPose, edges_str);
}
}
// 添加回环因子
void addLoopFactor()
{
if (loopIndexQueue.empty())
return;
// 把队列里面所有的回环约束添加进行
for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
{
int indexFrom = loopIndexQueue[i].first; // 回环帧索引
int indexTo = loopIndexQueue[i].second; // 当前帧索引
// 两帧的位姿变换（帧间约束）
gtsam::Pose3 poseBetween = loopPoseQueue[i];
// 回环的置信度就是icp的得分？
gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
// 加入约束
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
writeEdge({indexFrom, indexTo}, poseBetween, edges_str);
}
// 清空回环相关队列
loopIndexQueue.clear(); // it's very necessary
loopPoseQueue.clear();
loopNoiseQueue.clear();
aLoopIsClosed = true;
}
// TODO: update ikdtree for better visualization at a certain frequency
void recontructIKdTree()
{
if (updateKdtreeCount == kd_step)
{
/*** if path is too large, the rviz will crash ***/
pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMapPoses(new pcl::KdTreeFLANN<PointType>());
pcl::PointCloud<PointType>::Ptr subMapKeyPoses(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr subMapKeyPosesDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr subMapKeyFrames(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr subMapKeyFramesDS(new pcl::PointCloud<PointType>());
// kdtree查找最近一帧关键帧相邻的关键帧集合
std::vector<int> pointSearchIndGlobalMap;
std::vector<float> pointSearchSqDisGlobalMap;
mtx.lock();
kdtreeGlobalMapPoses->setInputCloud(cloudKeyPoses3D);
kdtreeGlobalMapPoses->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
mtx.unlock();
for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
subMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]); // subMap的pose集合
// 降采样
pcl::VoxelGrid<PointType> downSizeFilterSubMapKeyPoses;
downSizeFilterSubMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
downSizeFilterSubMapKeyPoses.setInputCloud(subMapKeyPoses);
downSizeFilterSubMapKeyPoses.filter(*subMapKeyPosesDS); // subMap poses downsample
// 提取局部相邻关键帧对应的特征点云
for (int i = 0; i < (int)subMapKeyPosesDS->size(); ++i)
{
// 距离过大
if (pointDistance(subMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
continue;
int thisKeyInd = (int)subMapKeyPosesDS->points[i].intensity;
// *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
*subMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); // fast_lio only use surfCloud
}
// 降采样，发布
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setInputCloud(subMapKeyFrames);
downSizeFilterGlobalMapKeyFrames.filter(*subMapKeyFramesDS);
// std::cout << "subMapKeyFramesDS sizes = " << subMapKeyFramesDS->points.size() << std::endl;
ikdtree.reconstruct(subMapKeyFramesDS->points);
updateKdtreeCount = 0;
ROS_INFO("Reconstructed ikdtree ");
int featsFromMapNum = ikdtree.validnum();
kdtree_size_st = ikdtree.size();
// featsFromMap->clear(); // TODO: right?
featsFromMap->points = subMapKeyFramesDS->points;
std::cout << "featsFromMapNum = " << featsFromMapNum << "\t"
<< " kdtree_size_st = " << kdtree_size_st << std::endl;
}
updateKdtreeCount++;
}
/**
* @brief
* 设置当前帧为关键帧并执行因子图优化
* 1、计算当前帧与前一帧位姿变换,如果变化太小,不设为关键帧,反之设为关键帧
* 2、添加激光里程计因子、回环因子
* 3、执行因子图优化
* 4、得到当前帧优化后位姿,位姿协方差
* 5、添加cloudKeyPoses3D,cloudKeyPoses6D,更新transformTobeMapped,添加当前关键帧的角点、平面点集合
*/
void saveKeyFramesAndFactor()
{
// 计算当前帧与前一帧位姿变换,如果变化太小,不设为关键帧,反之设为关键帧
if (saveFrame() == false)
return;
// 激光里程计因子(from fast-lio)
addOdomFactor();
// addGPSFactor(); // TODO: GPS
// 回环因子
addLoopFactor();
// 执行优化,更新图模型
isam->update(gtSAMgraph, initialEstimate);
isam->update();
// TODO: 如果加入了回环约束,isam需要进行更多次的优化
if (aLoopIsClosed == true){
cout << "pose is upated by isam " << endl;
isam->update();
isam->update();
isam->update();
isam->update();
}
// update之后要清空一下保存的因子图,注:清空不会影响优化,ISAM保存起来了
gtSAMgraph.resize(0);
initialEstimate.clear();
PointType thisPose3D;
PointTypePose thisPose6D;
gtsam::Pose3 latestEstimate;
// 通过接口获得所以变量的优化结果
isamCurrentEstimate = isam->calculateBestEstimate();
// 取出优化后的当前帧位姿结果
latestEstimate = isamCurrentEstimate.at<gtsam::Pose3>(isamCurrentEstimate.size() - 1);
// 位移信息取出来保存进clouKeyPoses3D这个结构中
thisPose3D.x = latestEstimate.translation().x();
thisPose3D.y = latestEstimate.translation().y();
thisPose3D.z = latestEstimate.translation().z();
// 其中索引作为intensity
thisPose3D.intensity = cloudKeyPoses3D->size(); // 使用intensity作为该帧点云的index
cloudKeyPoses3D->push_back(thisPose3D); // 新关键帧帧放入队列中
// 同样6D的位姿也保存下来
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
// TODO:
thisPose3D.z = 0.0; // FIXME: right?
thisPose6D.intensity = thisPose3D.intensity;
thisPose6D.roll = latestEstimate.rotation().roll();
thisPose6D.pitch = latestEstimate.rotation().pitch();
thisPose6D.yaw = latestEstimate.rotation().yaw();
thisPose6D.time = lidar_end_time;
cloudKeyPoses6D->push_back(thisPose6D);
// 保存当前位姿的位姿协方差（置信度）
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);
// // ESKF状态和方差更新
state_ikfom state_updated = kf.get_x(); // 获取cur_pose(还没修正)
Eigen::Vector3d pos(latestEstimate.translation().x(), latestEstimate.translation().y(), latestEstimate.translation().z());
Eigen::Quaterniond q = EulerToQuat(latestEstimate.rotation().roll(), latestEstimate.rotation().pitch(), latestEstimate.rotation().yaw());
// 更新状态量
state_updated.pos = pos;
state_updated.rot = q;
state_point = state_updated; // 对state_point进行更新,state_point可视化用到
if(aLoopIsClosed == true)
kf.change_x(state_updated); // 对cur_pose进行isam2优化后的修正
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*feats_undistort, *thisSurfKeyFrame); // 存储关键帧,没有降采样的点云
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
updatePath(thisPose6D); // 可视化update后的最新位姿
// 清空局部map, reconstruct ikdtree submap
if (recontructKdTree){
recontructIKdTree();
}
}
// 更新因子图中所有变量节点的位姿,也就是所有历史关键帧的位姿,调整全局轨迹,重构ikdtree
void correctPoses()
{
if (cloudKeyPoses3D->points.empty())
return;
// 只有回环以及才会触发全局调整
if (aLoopIsClosed == true)
{
// 清空里程计轨迹
globalPath.poses.clear();
// 更新因子图中所有变量节点的位姿,也就是所有历史关键帧的位姿
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().x();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().y();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().z();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();
// 更新里程计轨迹
updatePath(cloudKeyPoses6D->points[i]);
}
// 清空局部map, reconstruct ikdtree submap
if (recontructKdTree){
recontructIKdTree();
}
ROS_INFO("ISMA2 Update");
aLoopIsClosed = false;
}
}
/**
* @brief
* 检测最新帧是否和其它帧形成回环
* 回环检测三大要素
* 1.设置最小时间差,太近没必要
* 2.控制回环的频率,避免频繁检测,每检测一次,就做一次等待
* 3.根据当前最小距离重新计算等待时间
*/
bool detectLoopClosureDistance(int *latestID, int *closestID)
{
int loopKeyCur = copy_cloudKeyPoses3D->size() - 1; // 当前关键帧索引
int loopKeyPre = -1;
// 检查一下当前帧是否和别的形成了回环,如果已经有回环就不再继续
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
// 在历史关键帧中查找与当前关键帧距离最近的关键帧集合
std::vector<int> pointSearchIndLoop; // 候选关键帧索引
std::vector<float> pointSearchSqDisLoop; // 候选关键帧距离
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D); // 把只包含关键帧位移信息的点云填充kdtree
// 根据最后一个关键帧的平移信息,寻找离他一定距离内的其它关键帧
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
// 在候选关键帧集合中,找到与当前帧时间相隔较远的帧,设为候选匹配帧
for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
{
int id = pointSearchIndLoop[i];
// 历史帧必须比当前帧间隔historyKeyframeSearchTimeDiff以上,必须满足时间阈值,才是一个有效回环,一次找一个回环帧就行了
if (abs(copy_cloudKeyPoses6D->points[id].time - lidar_end_time) > historyKeyframeSearchTimeDiff)
{
loopKeyPre = id;
break;
}
}
// 如果没有找到回环或者回环找到自己身上去了,就认为是本次回环寻找失败
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
return false;
// 赋值当前帧和历史回环帧的id
*latestID = loopKeyCur;
*closestID = loopKeyPre;
// ROS_INFO("Find loop clousre frame ");
return true;
}
/**
* @brief 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合,降采样
*
*/
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr &nearKeyframes, const int &key, const int &searchNum)
{
nearKeyframes->clear();
int cloudSize = copy_cloudKeyPoses6D->size();
// searchNum是搜索范围,遍历帧的范围
for (int i = -searchNum; i <= searchNum; ++i)
{
int keyNear = key + i;
// 超出范围,退出
if (keyNear < 0 || keyNear >= cloudSize)
continue;
// 注意:cloudKeyPoses6D存储的是T_w_b,而点云是lidar系下的,构建submap时,要把点云转到cur点云系下
if(i == 0){
*nearKeyframes += *surfCloudKeyFrames[keyNear]; // cur点云本身保持不变
}
else{
Eigen::Affine3f keyTrans = pcl::getTransformation(copy_cloudKeyPoses6D->points[key].x, copy_cloudKeyPoses6D->points[key].y, copy_cloudKeyPoses6D->points[key].z, copy_cloudKeyPoses6D->points[key].roll, copy_cloudKeyPoses6D->points[key].pitch, copy_cloudKeyPoses6D->points[key].yaw);
Eigen::Affine3f keyNearTrans = pcl::getTransformation(copy_cloudKeyPoses6D->points[keyNear].x, copy_cloudKeyPoses6D->points[keyNear].y, copy_cloudKeyPoses6D->points[keyNear].z, copy_cloudKeyPoses6D->points[keyNear].roll, copy_cloudKeyPoses6D->points[keyNear].pitch, copy_cloudKeyPoses6D->points[keyNear].yaw);
Eigen::Affine3f finalTrans = keyTrans.inverse() * keyNearTrans;
pcl::PointCloud<PointType>::Ptr tmp(new pcl::PointCloud<PointType>());
transformPointCloud(surfCloudKeyFrames[keyNear], finalTrans, tmp);
*nearKeyframes += *tmp;
// *nearKeyframes += *getBodyCloud(surfCloudKeyFrames[keyNear], copy_cloudKeyPoses6D->points[key], copy_cloudKeyPoses6D->points[keyNear]); // TODO: fast-lio没有进行特征提取,默认点云就是surf
}
}
if (nearKeyframes->empty())
return;
}
/**
* @brief 回环检测函数 // TODO：没有用SC全局回环，因为很容易出现错误匹配，与其这样不如相信odometry
* 先最近距离再sc，我们可以相信odometry 是比较准的
*
*/
void performLoopClosure()
{
ros::Time timeLaserInfoStamp = ros::Time().fromSec(lidar_end_time); // 时间戳
string odometryFrame = "camera_init";
// 没有关键帧,没法进行回环检测
if (cloudKeyPoses3D->points.empty() == true)
{
return;
}
// 把存储关键帧位姿的点云copy出来,避免线程冲突,cloudKeyPoses3D就是关键帧的位置,cloudKeyPoses6D就是关键帧的位姿
mtx.lock();
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
int loopKeyCur; // 当前关键帧索引
int loopKeyPre; // 候选回环匹配帧索引
// 根据里程计的距离来检测回环,如果还没有回环则直接返回
if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
{
return;
}
cout << "[Nearest Pose found] " << " curKeyFrame: " << loopKeyCur << " loopKeyFrame: " << loopKeyPre << endl;
// 提取scan2map
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>()); // 当前关键帧的点云
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>()); // 历史回环帧周围的点云（局部地图）
{
// 提取当前关键帧特征点集合,降采样
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, historyKeyframeSearchNum); // 将cureKeyframeCloud保持在cureKeyframeCloud系下
// 提取历史回环帧周围的点云特征点集合,降采样
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum); // 选取historyKeyframeSearchNum个keyframe拼成submap，并转换到cureKeyframeCloud系下
// 发布回环局部地图submap
if (pubHistoryKeyFrames.getNumSubscribers() != 0){
pcl::PointCloud<PointType>::Ptr pubKrevKeyframeCloud(new pcl::PointCloud<PointType>());
*pubKrevKeyframeCloud += *transformPointCloud(prevKeyframeCloud, ©_cloudKeyPoses6D->points[loopKeyCur]); // 将submap转换到world系再发布
publishCloud(&pubHistoryKeyFrames, pubKrevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}
}
// TODO: 生成sc，进行匹配，再次声明一次，这里没有使用sc的全局回环
Eigen::MatrixXd cureKeyframeSC = scLoop.makeScancontext(*cureKeyframeCloud);
Eigen::MatrixXd prevKeyframeSC = scLoop.makeScancontext(*prevKeyframeCloud);
std::pair<double, int> simScore = scLoop.distanceBtnScanContext(cureKeyframeSC, prevKeyframeSC);
double dist = simScore.first;
int align = simScore.second;
if(dist > scLoop.SC_DIST_THRES){
cout << "but they can not be detected by SC." << endl;
return ;
}
std::cout.precision(3); // TODO: 如果使用sc全局定位，必须将保存的scd精度为3
cout << "[SC Loop found]" << " curKeyFrame: " << loopKeyCur << " loopKeyFrame: " << loopKeyPre
<< " distance: " << dist << " nn_align: " << align * scLoop.PC_UNIT_SECTORANGLE << " deg." << endl;
// ICP设置
pcl::IterativeClosestPoint<PointType, PointType> icp; // 使用icp来进行帧到局部地图的配准
icp.setMaxCorrespondenceDistance(200); // 设置对应点最大欧式距离,只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对
icp.setMaximumIterations(100); // 迭代停止条件一:设置最大的迭代次数
icp.setTransformationEpsilon(1e-6); // 迭代停止条件二:设置前后两次迭代的转换矩阵的最大容差,一旦两次迭代小于最大容差,则认为收敛到最优解,迭代停止
icp.setEuclideanFitnessEpsilon(1e-6); // 迭代终止条件三:设置前后两次迭代的点对的欧式距离均值的最大容差
icp.setRANSACIterations(0); // 设置RANSAC运行次数
float com_yaw = align * scLoop.PC_UNIT_SECTORANGLE;
PointTypePose com;
com.x = 0.0;
com.y = 0.0;
com.z = 0.0;
com.yaw = -com_yaw;
com.pitch = 0.0;
com.roll = 0.0;
cureKeyframeCloud = transformPointCloud(cureKeyframeCloud, &com);
// map-to-map,调用icp匹配
icp.setInputSource(cureKeyframeCloud); // 设置原始点云
icp.setInputTarget(prevKeyframeCloud); // 设置目标点云
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result); // 进行ICP配准,输出变换后点云
// 检测icp是否收敛以及得分是否满足要求
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore){
cout << "but they can not be registered by ICP." << " icpFitnessScore: " << icp.getFitnessScore() << endl;
return;
}
std::cout.precision(3);
cout << "[ICP Regiteration success ] " << " curKeyFrame: " << loopKeyCur << " loopKeyFrame: " << loopKeyPre
<< " icpFitnessScore: " << icp.getFitnessScore() << endl;
// 发布当前关键帧经过回环优化后的位姿变换之后的特征点云供可视化使用
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
// TODO: icp.getFinalTransformation()可以用来得到精准位姿
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
closed_cloud = transformPointCloud(closed_cloud, ©_cloudKeyPoses6D->points[loopKeyCur]);
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
// 回环优化得到的当前关键帧与回环关键帧之间的位姿变换
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
correctionLidarFrame = icp.getFinalTransformation(); // 获得两个点云的变换矩阵结果
// 回环优化前的当前帧位姿
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// 回环优化后的当前帧位姿:将icp结果补偿过去,就是当前帧的更为准确的位姿结果
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
// 将回环优化后的当前帧位姿换成平移和旋转
pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
// 将回环优化后的当前帧位姿换成gtsam的形式,From和To相当于帧间约束的因子,To是历史回环帧的位姿