diff --git a/.gitignore b/.gitignore index 977d3b9..c04a224 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,5 @@ build -FAST-LIO/Log/*.png -FAST-LIO/Log/*.txt -FAST-LIO/Log/*.csv -FAST-LIO/Log/*.pdf +Log/ .vscode/c_cpp_properties.json .vscode/settings.json PCD/*.pcd diff --git a/FAST-LIO/config/velodyne16.yaml b/FAST-LIO/config/velodyne16.yaml new file mode 100644 index 0000000..0335f7f --- /dev/null +++ b/FAST-LIO/config/velodyne16.yaml @@ -0,0 +1,28 @@ +common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + keyFrame_topic: "/aft_pgo_path" + keyFrame_id_topic: "/key_frames_ids" + time_sync_en: true # ONLY turn on when external time synchronization is really not possible + +preprocess: + lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 16 + blind: 1 # blind是min_radius^2 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 100.0 + extrinsic_T: [ 0, 0, 0.28] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + scan_publish_en: true # 'false' will close all the point cloud output + dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame \ No newline at end of file diff --git a/FAST-LIO/include/ikd-Tree/ikd_Tree.cpp b/FAST-LIO/include/ikd-Tree/ikd_Tree.cpp index 4912902..a652df6 100644 --- a/FAST-LIO/include/ikd-Tree/ikd_Tree.cpp +++ b/FAST-LIO/include/ikd-Tree/ikd_Tree.cpp @@ -1292,6 +1292,23 @@ bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x;} bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y;} bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z;} +/** + * @brief 删除当前所有的点云缓存，根据输入的点云，重新构造ikdtree + * @param point_cloud 输入的点云 + */ +void KD_TREE::reconstruct(PointVector point_cloud){ + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector ().swap(PCL_Storage); + Rebuild_Logger.clear(); + + if(Root_Node == nullptr){ + Build(point_cloud); + } else { + Add_Points(point_cloud, true); + } +} + // Manual heap MANUAL_HEAP::MANUAL_HEAP(int max_capacity){ cap = max_capacity; diff --git a/FAST-LIO/include/ikd-Tree/ikd_Tree.h b/FAST-LIO/include/ikd-Tree/ikd_Tree.h index 9ed41bc..c603310 100644 --- a/FAST-LIO/include/ikd-Tree/ikd_Tree.h +++ b/FAST-LIO/include/ikd-Tree/ikd_Tree.h @@ -180,6 +180,7 @@ class KD_TREE int Delete_Point_Boxes(vector & BoxPoints); void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type); void acquire_removed_points(PointVector & removed_points); + void reconstruct(PointVector point_cloud); BoxPointType tree_range(); PointVector PCL_Storage; KD_TREE_NODE * Root_Node = nullptr; diff --git a/FAST-LIO/launch/mapping_velodyne.launch b/FAST-LIO/launch/mapping_velodyne.launch index f422436..ffeced4 100644 --- a/FAST-LIO/launch/mapping_velodyne.launch +++ b/FAST-LIO/launch/mapping_velodyne.launch @@ -3,22 +3,33 @@ - + + + - - + + + + + + + + + + + - + \ No newline at end of file diff --git a/FAST-LIO/rviz_cfg/loam_livox.rviz b/FAST-LIO/rviz_cfg/loam_livox.rviz index 0d3c126..363c032 100644 --- a/FAST-LIO/rviz_cfg/loam_livox.rviz +++ b/FAST-LIO/rviz_cfg/loam_livox.rviz @@ -5,18 +5,11 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /mapping1 - - /mapping1/surround1 - - /mapping1/currPoints1 - /mapping1/currPoints1/Autocompute Value Bounds1 - - /Odometry1/Odometry1 - - /Odometry1/Odometry1/Shape1 - - /Odometry1/Odometry1/Covariance1 - /Odometry1/Odometry1/Covariance1/Position1 - /Odometry1/Odometry1/Covariance1/Orientation1 - - /MarkerArray1/Namespaces1 Splitter Ratio: 0.6432291865348816 - Tree Height: 1137 + Tree Height: 424 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -61,8 +54,7 @@ Visualization Manager: Plane Cell Count: 40 Reference Frame: Value: false - - Alpha: 1 - Class: rviz/Axes + - Class: rviz/Axes Enabled: false Length: 0.699999988079071 Name: Axes @@ -143,12 +135,12 @@ Visualization Manager: Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 - Name: PointCloud2 + Name: Laser_map_from_kdtree Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.029999999329447746 Style: Flat Squares Topic: /Laser_map Unreliable: false @@ -180,7 +172,6 @@ Visualization Manager: Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -194,106 +185,46 @@ Visualization Manager: Topic: /Odometry Unreliable: false Value: true + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true Enabled: true Name: Odometry - - Alpha: 1 - Class: rviz/Axes + - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: true - - Alpha: 0 - Buffer Length: 2 - Class: rviz/Path - Color: 25; 255; 255 - Enabled: true - Head Diameter: 0 - Head Length: 0 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.20000000298023224 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 25; 255; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.4000000059604645 - Shaft Length: 0.4000000059604645 - Topic: /path - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 239; 41; 41 - Max Intensity: 0 - Min Color: 239; 41; 41 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 4 - Size (m): 0.30000001192092896 - Style: Spheres - Topic: /cloud_effected - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 13.139549255371094 - Min Value: -32.08251953125 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 138; 226; 52 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 138; 226; 52 - Min Color: 138; 226; 52 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /Laser_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /MarkerArray - Name: MarkerArray + Enabled: true + Marker Topic: /loop_closure_constraints + Name: loop Namespaces: {} Queue Size: 100 - Value: false + Value: true Enabled: true Global Options: Background Color: 0; 0; 0 @@ -321,34 +252,34 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/ThirdPersonFollower - Distance: 76.899658203125 + Class: rviz/Orbit + Distance: 47.928932189941406 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false + X: 14.201390266418457 + Y: 8.872049331665039 + Z: 3.232368230819702 + Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.5697963237762451 Target Frame: global - Yaw: 2.980398416519165 + Value: Orbit (rviz) + Yaw: 4.031837463378906 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 1376 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004aefc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004ae000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000052fc0100000002fb0000000800540069006d00650100000000000009b8000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000692000004ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + collapsed: true + Height: 1050 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001c80000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000560000039e0000012600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000000000010000015200000368fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000368000000ec00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a400000052fc0100000002fb0000000800540069006d00650000000000000003a4000006d800fffffffb0000000800540069006d006501000000000000045000000000000000000000039f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -356,7 +287,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 2488 - X: 72 - Y: 27 + collapsed: true + Width: 927 + X: 2627 + Y: 0 diff --git a/FAST-LIO/src/IMU_Processing.hpp b/FAST-LIO/src/IMU_Processing.hpp index 7695839..560e021 100644 --- a/FAST-LIO/src/IMU_Processing.hpp +++ b/FAST-LIO/src/IMU_Processing.hpp @@ -192,7 +192,12 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekforientation; + // 如果orientation消息可用的话,以IMU的orientaion作为初始姿态 + if( orientation.x != 0 || orientation.y != 0 || orientation.z != 0 || orientation.w != 0 ){ + Eigen::Quaterniond qua(orientation.w, orientation.x, orientation.y, orientation.z); + init_state.rot = qua; + } //state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity))); init_state.bg = mean_gyr; init_state.offset_T_L_I = Lidar_T_wrt_IMU; @@ -253,7 +258,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekflinear_acceleration.y + tail->linear_acceleration.y), 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); - // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; + fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; diff --git a/FAST-LIO/src/laserMapping.cpp b/FAST-LIO/src/laserMapping.cpp index e9fb1ed..859f3cb 100644 --- a/FAST-LIO/src/laserMapping.cpp +++ b/FAST-LIO/src/laserMapping.cpp @@ -32,6 +32,17 @@ // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +/** + * @file laserMapping.cpp + * @author Yanliang wang (wyl410922@qq.com) + * @brief Modification: + * subscribe the keyframe and update the corresponding odometry of Fast LIO + * @version 0.1 + * @date 2022-03-11 + * + * @copyright Copyright (c) 2022 + * + */ #include #include #include @@ -59,6 +70,10 @@ #include #include "preprocess.h" #include +#include +#include +#include +#include #define INIT_TIME (0.1) #define LASER_POINT_COV (0.001) @@ -81,7 +96,7 @@ mutex mtx_buffer; condition_variable sig_buffer; string root_dir = ROOT_DIR; -string map_file_path, lid_topic, imu_topic; +string map_file_path, lid_topic, imu_topic, keyFrame_topic, keyFrame_id_topic; double res_mean_last = 0.05, total_residual = 0.0; double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; @@ -93,6 +108,12 @@ int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudVal bool point_selected_surf[100000] = {0}; bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; +bool recontructKdTree = false; +bool updateState = false; +int updateFrequency = 100; + +// visualize +bool visulize_map = false; vector> pointSearchInd_surf; vector cub_needrm; @@ -130,14 +151,29 @@ esekfom::esekf kf; state_ikfom state_point; vect3 pos_lid; -nav_msgs::Path path; +nav_msgs::Path path, path_updated/*发布更新的状态路径*/; nav_msgs::Odometry odomAftMapped; geometry_msgs::Quaternion geoQuat; -geometry_msgs::PoseStamped msg_body_pose; +geometry_msgs::PoseStamped msg_body_pose, msg_body_pose_updated; shared_ptr p_pre(new Preprocess()); shared_ptr p_imu(new ImuProcess()); +/*** 维护关键帧机制 ***/ +// 思路：缓存历史的lidar帧，根据订阅的seq来判断哪一帧是关键帧 +vector cloudKeyFrames; // 存放历史的关键帧点云 +queue< pair > cloudBuff; // 缓存部分历史的lidar帧，用于提取出关键帧点云 +vector idKeyFrames; // keyframes 的 id +queue idKeyFramesBuff; // keyframes 的 id buffer +nav_msgs::Path pathKeyFrames; // keyframes +uint32_t data_seq; // 数据的序号 +uint32_t lastKeyFramesId; // 最新关键帧对应里程计的ID +geometry_msgs::Pose lastKeyFramesPose; // 最新关键帧的位姿（世界到imu） +vector odoms; +/*** 维护submap ***/ +pcl::KdTreeFLANN::Ptr kdtreeSurroundingKeyPoses(new pcl::KdTreeFLANN()); // 周围关键帧pose的kdtree +pcl::VoxelGrid downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization + void SigHandle(int sig) { flg_exit = true; @@ -161,6 +197,14 @@ inline void dump_lio_state_to_log(FILE *fp) fflush(fp); } +/** + * 两点之间距离 +*/ +float pointDistance(pcl::PointXYZ p1, pcl::PointXYZ p2) +{ + return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); +} + void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s) { V3D p_body(pi->x, pi->y, pi->z); @@ -360,6 +404,19 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) sig_buffer.notify_all(); } +void keyFrame_cbk(const nav_msgs::Path::ConstPtr &msg_keyframes){ + // 更新关键帧 + pathKeyFrames = *msg_keyframes; +} + +void keyFrame_id_cbk(const std_msgs::Header::ConstPtr &msg_keyframe_id){ + // 将订阅到的关键帧id先加到idKeyFramesBuff中 + idKeyFramesBuff.push(msg_keyframe_id->seq); +} + +/* + * 获得同步的lidar和imu数据 +*/ bool sync_packages(MeasureGroup &meas) { if (lidar_buffer.empty() || imu_buffer.empty()) { @@ -510,7 +567,9 @@ void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body) pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.frame_id = "body"; + laserCloudmsg.header.seq = data_seq; pubLaserCloudFull_body.publish(laserCloudmsg); + cloudBuff.push( pair(data_seq ,laserCloudIMUBody) ); // 缓存所有发给后端的点云 publish_count -= PUBFRAME_PERIOD; } @@ -565,9 +624,14 @@ void set_posestamp(T & out) void publish_odometry(const ros::Publisher & pubOdomAftMapped) { odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.header.seq = data_seq; odomAftMapped.child_frame_id = "body"; odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time); set_posestamp(odomAftMapped.pose); + + // odoms[data_seq] = odomAftMapped.pose.pose; // 保存历史的odom + odoms.push_back(odomAftMapped.pose.pose); + pubOdomAftMapped.publish(odomAftMapped); auto P = kf.get_P(); for (int i = 0; i < 6; i ++) @@ -604,7 +668,7 @@ void publish_path(const ros::Publisher pubPath) /*** if path is too large, the rvis will crash ***/ static int jjj = 0; jjj++; - if (jjj % 10 == 0) + // if (jjj % 10 == 0) { path.poses.push_back(msg_body_pose); pubPath.publish(path); @@ -720,13 +784,20 @@ int main(int argc, char** argv) ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; + data_seq = 0; nh.param("publish/scan_publish_en",scan_pub_en,1); nh.param("publish/dense_publish_en",dense_pub_en,0); nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en,1); + nh.param("recontructKdTree",recontructKdTree,false); + nh.param("updateState",updateState,false); + nh.param("updateFrequency",updateFrequency,100); + nh.param("max_iteration",NUM_MAX_ITERATIONS,4); nh.param("map_file_path",map_file_path,""); nh.param("common/lid_topic",lid_topic,"/livox/lidar"); nh.param("common/imu_topic", imu_topic,"/livox/imu"); + nh.param("common/keyFrame_topic", keyFrame_topic,"/aft_pgo_path"); + nh.param("common/keyFrame_id_topic", keyFrame_id_topic,"/key_frames_ids"); nh.param("common/time_sync_en", time_sync_en, false); nh.param("filter_size_corner",filter_size_corner_min,0.5); nh.param("filter_size_surf",filter_size_surf_min,0.5); @@ -749,8 +820,12 @@ int main(int argc, char** argv) nh.param>("mapping/extrinsic_R", extrinR, vector()); cout<<"p_pre->lidar_type "<lidar_type<("visulize_map", visulize_map, false); + path.header.stamp = ros::Time::now(); path.header.frame_id ="camera_init"; + path_updated.header.stamp = ros::Time::now(); + path_updated.header.frame_id ="camera_init"; /*** variables definition ***/ int effect_feat_num = 0, frame_num = 0; @@ -766,6 +841,7 @@ int main(int argc, char** argv) memset(res_last, -1000.0f, sizeof(res_last)); downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + downSizeFilterSurroundingKeyPoses.setLeafSize(0.2,0.2,0.2); memset(point_selected_surf, true, sizeof(point_selected_surf)); memset(res_last, -1000.0f, sizeof(res_last)); @@ -800,6 +876,8 @@ int main(int argc, char** argv) nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ nh.subscribe(lid_topic, 200000, standard_pcl_cbk); ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); + ros::Subscriber sub_keyframes = nh.subscribe(keyFrame_topic, 10, keyFrame_cbk); + ros::Subscriber sub_keyframes_id = nh.subscribe(keyFrame_id_topic, 10, keyFrame_id_cbk); ros::Publisher pubLaserCloudFull = nh.advertise ("/cloud_registered", 100000); ros::Publisher pubLaserCloudFull_body = nh.advertise @@ -810,18 +888,48 @@ int main(int argc, char** argv) ("/cloud_effected", 100000); ros::Publisher pubLaserCloudMap = nh.advertise ("/Laser_map", 100000); + ros::Publisher pubKeyFramesMap = nh.advertise + ("/Keyframes_map", 100000); ros::Publisher pubOdomAftMapped = nh.advertise ("/Odometry", 100000); ros::Publisher pubPath = nh.advertise ("/path", 100000); + ros::Publisher pubPath_updated = nh.advertise + ("/path_updated", 100000); //------------------------------------------------------------------------------------------------------ signal(SIGINT, SigHandle); ros::Rate rate(5000); bool status = ros::ok(); + uint32_t count = 1; while (status) { if (flg_exit) break; ros::spinOnce(); + // 接收关键帧, 一直循环直到其中一个为空（理论上应该是idKeyFramesBuff先空） + { + while( !cloudBuff.empty() && !idKeyFramesBuff.empty() ){ + while( idKeyFramesBuff.front() > cloudBuff.front().first ) + { + cloudBuff.pop(); + } + // 此时idKeyFramesBuff.front() == cloudBuff.front().first + assert(idKeyFramesBuff.front() == cloudBuff.front().first); + idKeyFrames.push_back(idKeyFramesBuff.front()); + cloudKeyFrames.push_back( cloudBuff.front().second ); + idKeyFramesBuff.pop(); + cloudBuff.pop(); + } + assert(pathKeyFrames.poses.size() <= cloudKeyFrames.size() ); // 有可能id发过来了，但是节点还未更新 + + // 记录最新关键帧的信息 + if(pathKeyFrames.poses.size() >= 1){ + lastKeyFramesId = idKeyFrames[pathKeyFrames.poses.size() - 1]; + lastKeyFramesPose = pathKeyFrames.poses.back().pose; + } + } + + + if(sync_packages(Measures)) { if (flg_reset) @@ -856,6 +964,161 @@ int main(int argc, char** argv) flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \ false : true; + // todo:改为多线程ikdtree更新 + if(count % updateFrequency == 0 ){ + count = 1; + if(recontructKdTree && pathKeyFrames.poses.size() > 20){ + /*** 所有关键帧的地图 ***/ + // PointCloudXYZI::Ptr keyFramesMap(new PointCloudXYZI()); + // PointCloudXYZI::Ptr keyframesTmp(new PointCloudXYZI()); + // Eigen::Isometry3d poseTmp; + // assert(pathKeyFrames.poses.size() <= cloudKeyFrames.size() ); // 有可能id发过来了，但是节点还未更新 + // int keyFramesNum = pathKeyFrames.poses.size(); + // for(int i = 0; i < keyFramesNum; ++i){ + // downSizeFilterMap.setInputCloud(cloudKeyFrames[i]); + // downSizeFilterMap.filter(*keyframesTmp); + // tf::poseMsgToEigen(pathKeyFrames.poses[i].pose,poseTmp); + // pcl::transformPointCloud(*keyframesTmp , *keyframesTmp, poseTmp.matrix()); + // *keyFramesMap += *keyframesTmp; + // } + // downSizeFilterMap.setInputCloud(keyFramesMap); + // downSizeFilterMap.filter(*keyFramesMap); + + // ikdtree.reconstruct(keyFramesMap->points); + + /*** 距离近的关键帧构成的子图 ***/ + pcl::PointCloud::Ptr cloudKeyPoses3D(new pcl::PointCloud()); // 历史关键帧位姿（位置） + pcl::PointCloud::Ptr surroundingKeyPoses(new pcl::PointCloud()); + pcl::PointCloud::Ptr surroundingKeyPosesDS(new pcl::PointCloud()); + + for(auto keyFramePose:pathKeyFrames.poses){ + cloudKeyPoses3D->points.emplace_back(keyFramePose.pose.position.x, + keyFramePose.pose.position.y, + keyFramePose.pose.position.z); + } + double surroundingKeyframeSearchRadius = 5; + std::vector pointSearchInd; + std::vector pointSearchSqDis; + kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); + kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis); + // 遍历搜索结果，pointSearchInd存的是结果在cloudKeyPoses3D下面的索引 + unordered_map keyFramePoseMap; // 以pose的x坐标为哈希表的key + for (int i = 0; i < (int)pointSearchInd.size(); ++i) + { + int id = pointSearchInd[i]; + // 加入相邻关键帧位姿集合中 + surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]); + keyFramePoseMap[cloudKeyPoses3D->points[id].x] = id; + } + + // 降采样一下 + downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses); + downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS); + + // 加入与当前关键帧靠近的offset个帧，这些帧加进来是合理的 + int numPoses = cloudKeyPoses3D->size(); + int offset = 10; + for (int i = numPoses-1; i >= numPoses-1 - offset && i >= 0; --i) + { + surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]); + keyFramePoseMap[cloudKeyPoses3D->points[i].x] = i; + } + + // 将相邻关键帧集合对应的点加入到局部map中，作为scan-to-map匹配的局部点云地图 + // PointCloudXYZI::Ptr keyFramesSubmap = extractCloud(surroundingKeyPosesDS, keyFramePoseMap); + + PointCloudXYZI::Ptr keyFramesSubmap(new PointCloudXYZI()); + // 遍历当前帧（实际是取最近的一个关键帧来找它相邻的关键帧集合）时空维度上相邻的关键帧集合 + for (int i = 0; i < (int)surroundingKeyPosesDS->size(); ++i) + { + ROS_INFO("surroundingKeyPosesDS->points[i].x: %f", surroundingKeyPosesDS->points[i].x); + ROS_INFO("surroundingKeyPosesDS->points[i].x: %d", keyFramePoseMap.size()); + // assert(keyFramePoseMap.count(surroundingKeyPosesDS->points[i].x) != 0); + if(keyFramePoseMap.count(surroundingKeyPosesDS->points[i].x) == 0) + continue; + + // 距离超过阈值，丢弃 + if (pointDistance(surroundingKeyPosesDS->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius) // 丢弃那些满足时间临近，不满足空间临近的点 + continue; + + // 相邻关键帧索引 + int thisKeyInd = keyFramePoseMap[ surroundingKeyPosesDS->points[i].x ]; // 以intensity作为红黑树的索引 + + PointCloudXYZI::Ptr keyframesTmp(new PointCloudXYZI()); + Eigen::Isometry3d poseTmp; + assert(pathKeyFrames.poses.size() <= cloudKeyFrames.size() ); // 有可能id发过来了，但是节点还未更新 + int keyFramesNum = pathKeyFrames.poses.size(); + + downSizeFilterMap.setInputCloud(cloudKeyFrames[thisKeyInd]); + downSizeFilterMap.filter(*keyframesTmp); + tf::poseMsgToEigen(pathKeyFrames.poses[thisKeyInd].pose,poseTmp); + pcl::transformPointCloud(*keyframesTmp , *keyframesTmp, poseTmp.matrix()); + *keyFramesSubmap += *keyframesTmp; + } + downSizeFilterMap.setInputCloud(keyFramesSubmap); + downSizeFilterMap.filter(*keyFramesSubmap); + + ikdtree.reconstruct(keyFramesSubmap->points); + } + + // 更新状态 + if(updateState) + { + state_ikfom state_updated = kf.get_x(); + Eigen::Isometry3d lastPose(state_updated.rot); + lastPose.pretranslate(state_updated.pos); + + + Eigen::Isometry3d lastKeyFramesPoseEigen; // 最新的关键帧位姿 + tf::poseMsgToEigen(lastKeyFramesPose, lastKeyFramesPoseEigen); + + Eigen::Isometry3d lastKeyFrameOdomPoseEigen; // 最新的关键帧对应的odom的位姿 + tf::poseMsgToEigen(odoms[lastKeyFramesId], lastKeyFrameOdomPoseEigen); + + // lastPose表示世界坐标系到当前坐标系的变换，下面两个公式等价 + // lastPose = (lastKeyFramesPoseEigen.inverse() * lastKeyFrameOdomPoseEigen* lastPose.inverse()).inverse(); + lastPose = lastPose * lastKeyFrameOdomPoseEigen.inverse() * lastKeyFramesPoseEigen; + + Eigen::Quaterniond lastPoseQuat( lastPose.rotation() ); + Eigen::Vector3d lastPoseQuatPos( lastPose.translation() ); + state_updated.rot = lastPoseQuat; + state_updated.pos = lastPoseQuatPos; + kf.change_x(state_updated); + + esekfom::esekf::cov P_updated = kf.get_P(); // 获取当前的状态估计的协方差矩阵 + P_updated.setIdentity(); + // QUESTION: 状态的协方差矩阵是否要更新为一个比较的小的值？ + // init_P(0,0) = init_P(1,1) = init_P(2,2) = 0.00001; + // init_P(3,3) = init_P(4,4) = init_P(5,5) = 0.00001; + P_updated(6,6) = P_updated(7,7) = P_updated(8,8) = 0.00001; + P_updated(9,9) = P_updated(10,10) = P_updated(11,11) = 0.00001; + P_updated(15,15) = P_updated(16,16) = P_updated(17,17) = 0.0001; + P_updated(18,18) = P_updated(19,19) = P_updated(20,20) = 0.001; + P_updated(21,21) = P_updated(22,22) = 0.00001; + kf.change_P(P_updated); + + msg_body_pose_updated.pose.position.x = state_updated.pos(0); + msg_body_pose_updated.pose.position.y = state_updated.pos(1); + msg_body_pose_updated.pose.position.z = state_updated.pos(2); + msg_body_pose_updated.pose.orientation.x = state_updated.rot.x(); + msg_body_pose_updated.pose.orientation.y = state_updated.rot.y(); + msg_body_pose_updated.pose.orientation.z = state_updated.rot.z(); + msg_body_pose_updated.pose.orientation.w = state_updated.rot.w(); + msg_body_pose_updated.header.stamp = ros::Time().fromSec(lidar_end_time); + msg_body_pose_updated.header.frame_id = "camera_init"; + + /*** if path is too large, the rvis will crash ***/ + static int jjj = 0; + jjj++; + // if (jjj % 10 == 0) + { + path_updated.poses.push_back(msg_body_pose_updated); + pubPath_updated.publish(path_updated); + } + } + } + ++count; + /*** Segment the map in lidar FOV ***/ lasermap_fov_segment(); @@ -892,12 +1155,13 @@ int main(int argc, char** argv) fout_pre<clear(); featsFromMap->points = ikdtree.PCL_Storage; + publish_map(pubLaserCloudMap); } pointSearchInd_surf.resize(feats_down_size); @@ -936,8 +1200,8 @@ int main(int argc, char** argv) publish_frame_body(pubLaserCloudFull_body); publish_frame_lidar(pubLaserCloudFull_lidar); } + ++data_seq; // publish_effect_world(pubLaserCloudEffect); - // publish_map(pubLaserCloudMap); /*** Debug variables ***/ if (runtime_pos_log) diff --git a/SC-PGO/include/aloam_velodyne/common.h b/SC-PGO/include/aloam_velodyne/common.h index a73adb4..459ea14 100644 --- a/SC-PGO/include/aloam_velodyne/common.h +++ b/SC-PGO/include/aloam_velodyne/common.h @@ -59,4 +59,5 @@ struct Pose6D { double roll; double pitch; double yaw; + uint32_t seq; }; \ No newline at end of file diff --git a/SC-PGO/launch/fastlio_ouster64.launch b/SC-PGO/launch/fastlio_ouster64.launch index 3b7dbd1..8536344 100644 --- a/SC-PGO/launch/fastlio_ouster64.launch +++ b/SC-PGO/launch/fastlio_ouster64.launch @@ -21,6 +21,16 @@ + + + + + + + + + + diff --git a/SC-PGO/launch/fastlio_velodyne_VLP_16.launch b/SC-PGO/launch/fastlio_velodyne_VLP_16.launch new file mode 100644 index 0000000..9c8af7a --- /dev/null +++ b/SC-PGO/launch/fastlio_velodyne_VLP_16.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/SC-PGO/rviz_cfg/aloam_velodyne.rviz b/SC-PGO/rviz_cfg/aloam_velodyne.rviz index f0df56d..4f6d144 100644 --- a/SC-PGO/rviz_cfg/aloam_velodyne.rviz +++ b/SC-PGO/rviz_cfg/aloam_velodyne.rviz @@ -1,16 +1,16 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 104 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - - /odometry1/odomPath1 - - /pgoOdom1/Shape1 - - /pgoMap1 - Splitter Ratio: 0.440559446811676 - Tree Height: 787 + - /mapping1/currentPC1 + - /path1 + - /loop1 + Splitter Ratio: 0.6134663224220276 + Tree Height: 736 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -125,27 +125,64 @@ Visualization Manager: - Class: rviz/Group Displays: - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 204; 0; 0 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: curvature + Class: rviz/PointCloud2 + Color: 46; 52; 54 + Color Transformer: FlatColor + Decay Time: 0 Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.20000000298023224 - Name: odomPath - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /laser_odom_path + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 300 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: pgoMap + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 1 + Size (m): 0.4000000059604645 + Style: Points + Topic: /aft_pgo_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 300 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Laser_map + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.4000000059604645 + Style: Points + Topic: /Laser_map Unreliable: false + Use Fixed Frame: true + Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -157,35 +194,29 @@ Visualization Manager: Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 - Color Transformer: FlatColor + Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 63.10047912597656 Min Color: 0; 0; 0 - Min Intensity: -0.0067862942814826965 - Name: PointCloud2 + Name: Keyframes_map Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.05000000074505806 + Size (m): 0.10000000149011612 Style: Flat Squares - Topic: /velodyne_cloud_2 + Topic: /Keyframes_map Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Enabled: true - Name: odometry - - Class: rviz/Group - Displays: - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 4; 69; 246 - Enabled: true + Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 @@ -203,7 +234,7 @@ Visualization Manager: Shaft Length: 0.10000000149011612 Topic: /aft_mapped_path Unreliable: false - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -219,9 +250,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 50.14332962036133 Min Color: 0; 0; 0 - Min Intensity: -0.04392780363559723 Name: allMapCloud Position Transformer: XYZ Queue Size: 10 @@ -249,9 +278,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 15 Min Color: 0; 0; 0 - Min Intensity: 0 Name: surround Position Transformer: XYZ Queue Size: 1 @@ -279,9 +306,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 50.141441345214844 Min Color: 0; 0; 0 - Min Intensity: -0.024373823776841164 Name: currPoints Position Transformer: XYZ Queue Size: 10 @@ -342,155 +367,290 @@ Visualization Manager: {} Update Interval: 0 Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currentPC + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Flat Squares + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Name: mapping - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.4000000059604645 - Name: pgoPath - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Axes - Radius: 0.10000000149011612 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /aft_pgo_path - Unreliable: false - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 239; 41; 41 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: fastlio-Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path + Unreliable: false Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 32; 74; 135 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: fastlio-Pathp-updated + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path_updated + Unreliable: false Value: true - Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: pgoPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.05000000074505806 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /aft_pgo_path + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 239; 41; 41 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: fastlio-Path-c + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: pgoPath-c + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Radius: 0.05000000074505806 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /aft_pgo_path + Unreliable: false + Value: false Enabled: true - Keep: 1 - Name: pgoOdom - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 2 - Axes Radius: 0.699999988079071 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /aft_pgo_odom - Unreliable: false - Value: true - - Alpha: 0.10000000149011612 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + Name: path + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /loop_closure_constraints + Name: loop + Namespaces: + loop_edges: true + loop_nodes: true + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: loop scan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Spheres + Topic: /loop_scan_local + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 136; 138; 133 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: loop scan registed + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Spheres + Topic: /loop_scan_local_registed + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 32.84956741333008 + Min Value: -16.756437301635742 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: loop submap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.20000000298023224 + Style: Spheres + Topic: /loop_submap_local + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 300 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: pgoMap - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 2 - Size (m): 0.4000000059604645 - Style: Points - Topic: /aft_pgo_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 20.06364631652832 - Min Color: 0; 0; 0 - Min Intensity: 3.003571033477783 - Name: loop scan - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.5 - Style: Spheres - Topic: /loop_scan_local - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 32.84956741333008 - Min Value: -16.756437301635742 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + Name: loop + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: pgoOdom + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 2 + Axes Radius: 0.699999988079071 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_pgo_odom + Unreliable: false + Value: true Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 20.100271224975586 - Min Color: 0; 0; 0 - Min Intensity: 3.0008726119995117 - Name: loop submap - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.20000000298023224 - Style: Spheres - Topic: /loop_submap_local - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false + Name: Odom Enabled: true Global Options: Background Color: 255; 255; 255 @@ -519,33 +679,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 638.897705078125 + Distance: 90.50765991210938 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 191.27947998046875 - Y: 248.57839965820312 - Z: -12.774103164672852 + X: 20.914894104003906 + Y: 14.743096351623535 + Z: 6.0498809814453125 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: aft_pgo + Pitch: 1.3347965478897095 + Target Frame: camera_init Value: Orbit (rviz) - Yaw: 3.1635923385620117 + Yaw: 3.9054203033447266 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false + collapsed: true + Height: 1050 + Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001af0000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002ef000000c30000000000000000fb0000001200700067006f00200056006900650077007300000002db000000d7000000a400ffffff000000010000010f00000267fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000002670000000000fffffffaffffffff0100000002fb0000000a005600690065007700730000000000ffffffff0000010000fffffffb0000000a0056006900650077007300000006710000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007530000003efc0100000002fb0000000800540069006d0065000000000000000753000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000059e0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001930000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000560000039e0000012600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002ef000000c30000000000000000fb0000001200700067006f00200056006900650077007300000002db000000d7000000ec00ffffff000000010000010f000003a5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000003a50000000000fffffffaffffffff0100000002fb0000000a005600690065007700730000000000ffffffff0000011f00fffffffb0000000a0056006900650077007300000006710000010f0000011f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007530000003efc0100000002fb0000000800540069006d0065000000000000000753000006d800fffffffb0000000800540069006d006501000000000000045000000000000000000000039f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -554,8 +714,8 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1875 - X: 1965 - Y: 27 + Width: 927 + X: 3553 + Y: 0 pgo Views: collapsed: false diff --git a/SC-PGO/src/laserPosegraphOptimization.cpp b/SC-PGO/src/laserPosegraphOptimization.cpp index 16bd336..5e10a60 100644 --- a/SC-PGO/src/laserPosegraphOptimization.cpp +++ b/SC-PGO/src/laserPosegraphOptimization.cpp @@ -1,3 +1,16 @@ +/** + * @file laserPosegraphOptimization.cpp + * @author Yanliang Wang (wyl410922@qq.com) + * @brief + * 1. Detect the keyframes + * 2. Maintain the Gtsam-based pose graph + * 3. Detect the radius-search-based loop closure, and add them to the pose graph + * @version 0.1 + * @date 2022-03-11 + * + * @copyright Copyright (c) 2022 + * + */ #include #include #include @@ -57,6 +70,9 @@ #include "scancontext/Scancontext.h" +#include +#include + using namespace gtsam; using std::cout; @@ -91,6 +107,11 @@ std::vector keyframePoses; std::vector keyframePosesUpdated; std::vector keyframeTimes; int recentIdxUpdated = 0; +// for loop closure detection +std::map loopIndexContainer; // 记录存在的回环对 +pcl::KdTreeFLANN::Ptr kdtreeHistoryKeyPoses(new pcl::KdTreeFLANN()); +ros::Publisher pubLoopConstraintEdge; + gtsam::NonlinearFactorGraph gtSAMgraph; bool gtSAMgraphMade = false; @@ -134,6 +155,23 @@ std::string pgKITTIformat, pgScansDirectory; std::string odomKITTIformat; std::fstream pgTimeSaveStream; +// for front_end +ros::Publisher pubKeyFramesId; + +// for loop closure +double historyKeyframeSearchRadius; +double historyKeyframeSearchTimeDiff; +int historyKeyframeSearchNum; +double loopClosureFrequency; +int graphUpdateTimes; +double graphUpdateFrequency; +double loopNoiseScore; +double vizmapFrequency; +double vizPathFrequency; +double speedFactor; +ros::Publisher pubLoopScanLocalRegisted; +double loopFitnessScoreThreshold; + std::string padZeros(int val, int num_digits = 6) { std::ostringstream out; out << std::internal << std::setfill('0') << std::setw(num_digits) << val; @@ -222,7 +260,7 @@ void initNoises( void ) odomNoiseVector6 << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4; odomNoise = noiseModel::Diagonal::Variances(odomNoiseVector6); - double loopNoiseScore = 0.5; // constant is ok... + // double loopNoiseScore = 0.5; // constant is ok... gtsam::Vector robustNoiseVector6(6); // gtsam::Pose3 factor has 6 elements (6D) robustNoiseVector6 << loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore; robustLoopNoise = gtsam::noiseModel::Robust::Create( @@ -249,7 +287,7 @@ Pose6D getOdom(nav_msgs::Odometry::ConstPtr _odom) geometry_msgs::Quaternion quat = _odom->pose.pose.orientation; tf::Matrix3x3(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(roll, pitch, yaw); - return Pose6D{tx, ty, tz, roll, pitch, yaw}; + return Pose6D{tx, ty, tz, roll, pitch, yaw, _odom->header.seq}; } // getOdom Pose6D diffTransformation(const Pose6D& _p1, const Pose6D& _p2) @@ -365,12 +403,16 @@ void runISAM2opt(void) // called when a variable added isam->update(gtSAMgraph, initialEstimate); isam->update(); + for(int i = graphUpdateTimes; i > 0; --i){ + isam->update(); + } gtSAMgraph.resize(0); initialEstimate.clear(); isamCurrentEstimate = isam->calculateEstimate(); updatePoses(); + pubPath(); // 每优化一次就输出一次优化后的位姿 } pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, gtsam::Pose3 transformIn) @@ -404,12 +446,12 @@ void loopFindNearKeyframesCloud( pcl::PointCloud::Ptr& nearKeyframes, // extract and stacking near keyframes (in global coord) nearKeyframes->clear(); for (int i = -submap_size; i <= submap_size; ++i) { - int keyNear = key + i; + int keyNear = root_idx + i; if (keyNear < 0 || keyNear >= int(keyframeLaserClouds.size()) ) continue; mKF.lock(); - *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]); + *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]); mKF.unlock(); } @@ -423,15 +465,64 @@ void loopFindNearKeyframesCloud( pcl::PointCloud::Ptr& nearKeyframes, *nearKeyframes = *cloud_temp; } // loopFindNearKeyframesCloud +/** + * 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合，降采样 +*/ +void loopFindNearKeyframes(pcl::PointCloud::Ptr& nearKeyframes, const int& key, const int& searchNum) +{ + // 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合 + nearKeyframes->clear(); + int cloudSize = keyframeLaserClouds.size(); + for (int i = -searchNum; i <= searchNum; ++i) + { + int keyNear = key + i; + if (keyNear < 0 || keyNear >= cloudSize ) + continue; + // *nearKeyframes += *transformPointCloud(keyframeLaserClouds[keyNear], ©_cloudKeyPoses6D->points[keyNear]); + mKF.lock(); + *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[keyNear]); + mKF.unlock(); + } + + if (nearKeyframes->empty()) + return; + + // 降采样 + pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); + downSizeFilterICP.setInputCloud(nearKeyframes); + downSizeFilterICP.filter(*cloud_temp); + *nearKeyframes = *cloud_temp; +} + +/** + * Eigen格式的位姿变换 +*/ +Eigen::Affine3f Pose6dToAffine3f(Pose6D pose) +{ + return pcl::getTransformation(pose.x, pose.y, pose.z, pose.roll, pose.pitch, pose.yaw); +} -std::optional doICPVirtualRelative( int _loop_kf_idx, int _curr_kf_idx ) +/** + * 位姿格式变换 +*/ +gtsam::Pose3 Pose6dTogtsamPose3(Pose6D pose) +{ + return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(pose.roll), double(pose.pitch), double(pose.yaw)), + gtsam::Point3(double(pose.x), double(pose.y), double(pose.z))); +} + +gtsam::Pose3 doICPVirtualRelative( int _loop_kf_idx, int _curr_kf_idx ) { // parse pointclouds - int historyKeyframeSearchNum = 25; // enough. ex. [-25, 25] covers submap length of 50x1 = 50m if every kf gap is 1m + // int historyKeyframeSearchNum = 25; // enough. ex. [-25, 25] covers submap length of 50x1 = 50m if every kf gap is 1m pcl::PointCloud::Ptr cureKeyframeCloud(new pcl::PointCloud()); pcl::PointCloud::Ptr targetKeyframeCloud(new pcl::PointCloud()); - loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx); // use same root of loop kf idx - loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum, _loop_kf_idx); + // loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx); // use same root of loop kf idx + // loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum, _loop_kf_idx); + // 提取当前关键帧特征点集合，降采样 + loopFindNearKeyframes(cureKeyframeCloud, _curr_kf_idx, 0); + // 提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样 + loopFindNearKeyframes(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum); // loop verification sensor_msgs::PointCloud2 cureKeyframeCloudMsg; @@ -457,11 +548,16 @@ std::optional doICPVirtualRelative( int _loop_kf_idx, int _curr_kf icp.setInputTarget(targetKeyframeCloud); pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); icp.align(*unused_result); - - float loopFitnessScoreThreshold = 0.3; // user parameter but fixed low value is safe. + + sensor_msgs::PointCloud2 cureKeyframeCloudRegMsg; + pcl::toROSMsg(*unused_result, cureKeyframeCloudRegMsg); + cureKeyframeCloudRegMsg.header.frame_id = "camera_init"; + pubLoopScanLocalRegisted.publish(cureKeyframeCloudRegMsg); + + // float loopFitnessScoreThreshold = 0.3; // user parameter but fixed low value is safe. if (icp.hasConverged() == false || icp.getFitnessScore() > loopFitnessScoreThreshold) { std::cout << "[SC loop] ICP fitness test failed (" << icp.getFitnessScore() << " > " << loopFitnessScoreThreshold << "). Reject this SC loop." << std::endl; - return std::nullopt; + return gtsam::Pose3::identity(); } else { std::cout << "[SC loop] ICP fitness test passed (" << icp.getFitnessScore() << " < " << loopFitnessScoreThreshold << "). Add this SC loop." << std::endl; } @@ -470,9 +566,15 @@ std::optional doICPVirtualRelative( int _loop_kf_idx, int _curr_kf float x, y, z, roll, pitch, yaw; Eigen::Affine3f correctionLidarFrame; correctionLidarFrame = icp.getFinalTransformation(); - pcl::getTranslationAndEulerAngles (correctionLidarFrame, x, y, z, roll, pitch, yaw); + + Eigen::Affine3f tWrong = Pose6dToAffine3f(keyframePosesUpdated[_curr_kf_idx]); + + // 闭环优化后当前帧位姿 + Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong; + pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw); gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); - gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + // 闭环匹配帧的位姿 + gtsam::Pose3 poseTo = Pose6dTogtsamPose3(keyframePosesUpdated[_loop_kf_idx]); return poseFrom.between(poseTo); } // doICPVirtualRelative @@ -498,7 +600,6 @@ void process_pg() // Time equal check timeLaserOdometry = odometryBuf.front()->header.stamp.toSec(); timeLaser = fullResBuf.front()->header.stamp.toSec(); - // TODO laserCloudFullRes->clear(); pcl::PointCloud::Ptr thisKeyFrame(new pcl::PointCloud()); @@ -535,6 +636,7 @@ void process_pg() translationAccumulated += delta_translation; rotaionAccumulated += (dtf.roll + dtf.pitch + dtf.yaw); // sum just naive approach. + // 关键帧选择 if( translationAccumulated > keyframeMeterGap || rotaionAccumulated > keyframeRadGap ) { isNowKeyFrame = true; translationAccumulated = 0.0; // reset @@ -563,6 +665,13 @@ void process_pg() mKF.lock(); keyframeLaserClouds.push_back(thisKeyFrameDS); keyframePoses.push_back(pose_curr); + { + // 发布关键帧id + std_msgs::Header keyFrameHeader; + keyFrameHeader.seq = pose_curr.seq; + keyFrameHeader.stamp = ros::Time::now(); + pubKeyFramesId.publish(keyFrameHeader); + } keyframePosesUpdated.push_back(pose_curr); // init keyframeTimes.push_back(timeLaserOdometry); @@ -653,15 +762,141 @@ void performSCLoopClosure(void) } } // performSCLoopClosure +pcl::PointCloud::Ptr vector2pc(const std::vector vectorPose6d){ + pcl::PointCloud::Ptr res( new pcl::PointCloud ) ; + for( auto p : vectorPose6d){ + res->points.emplace_back(p.x, p.y, p.z); + } + return res; +} + +/** + * 在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧 +*/ +bool detectLoopClosureDistance(int *loopKeyCur, int *loopKeyPre) +{ + // 当前关键帧 + // int loopKeyCur = keyframePoses.size() - 1; + // int loopKeyPre = -1; + + // 当前帧已经添加过闭环对应关系，不再继续添加 + auto it = loopIndexContainer.find(*loopKeyCur); + if (it != loopIndexContainer.end()) + return false; + + // 在历史关键帧中查找与当前关键帧距离最近的关键帧集合 + pcl::PointCloud::Ptr copy_cloudKeyPoses3D = vector2pc(keyframePoses); + std::vector pointSearchIndLoop; + std::vector pointSearchSqDisLoop; + kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D); + kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0); + + // 在候选关键帧集合中，找到与当前帧时间相隔较远的帧，设为候选匹配帧 + for(int i = 0; i < pointSearchIndLoop.size(); ++i) + { + int id = pointSearchIndLoop[i]; + if ( abs( keyframeTimes[id] - keyframeTimes[*loopKeyCur] ) > historyKeyframeSearchTimeDiff ) + { + *loopKeyPre = id; + break; + } + } + + if (*loopKeyPre == -1 || *loopKeyCur == *loopKeyPre) + return false; + + // *latestID = loopKeyCur; + // *closestID = loopKeyPre; + + return true; +} + +void performRSLoopClosure(void) +{ + if( keyframePoses.empty() ) // 如果历史关键帧为空 + return; + + // 当前关键帧索引，候选闭环匹配帧索引 + int loopKeyCur = keyframePoses.size() - 1; + int loopKeyPre = -1; + if ( detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) ){ + cout << "Loop detected! - between " << loopKeyPre << " and " << loopKeyCur << "" << endl; + mBuf.lock(); + scLoopICPBuf.push(std::pair(loopKeyPre, loopKeyCur)); + loopIndexContainer[loopKeyCur] = loopKeyPre ; + // addding actual 6D constraints in the other thread, icp_calculation. + mBuf.unlock(); + } else + return; +} // performRSLoopClosure + +/** + * rviz展示闭环边 +*/ +void visualizeLoopClosure() +{ + if (loopIndexContainer.empty()) + return; + + visualization_msgs::MarkerArray markerArray; + // 闭环顶点 + visualization_msgs::Marker markerNode; + markerNode.header.frame_id = "camera_init"; // camera_init + markerNode.header.stamp = ros::Time().fromSec( keyframeTimes.back() ); + 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 = "camera_init"; + markerEdge.header.stamp = ros::Time().fromSec( keyframeTimes.back() ); + 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 = keyframePosesUpdated[key_cur].x; + p.y = keyframePosesUpdated[key_cur].y; + p.z = keyframePosesUpdated[key_cur].z; + markerNode.points.push_back(p); + markerEdge.points.push_back(p); + p.x = keyframePosesUpdated[key_pre].x; + p.y = keyframePosesUpdated[key_pre].y; + p.z = keyframePosesUpdated[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); +} + void process_lcd(void) { - float loopClosureFrequency = 1.0; // can change + // float loopClosureFrequency = 3.0; // can change ros::Rate rate(loopClosureFrequency); while (ros::ok()) { rate.sleep(); - performSCLoopClosure(); - // performRSLoopClosure(); // TODO + // performSCLoopClosure(); + performRSLoopClosure(); // TODO + visualizeLoopClosure(); } } // process_lcd @@ -682,11 +917,12 @@ void process_icp(void) const int prev_node_idx = loop_idx_pair.first; const int curr_node_idx = loop_idx_pair.second; - auto relative_pose_optional = doICPVirtualRelative(prev_node_idx, curr_node_idx); - if(relative_pose_optional) { - gtsam::Pose3 relative_pose = relative_pose_optional.value(); + auto relative_pose = doICPVirtualRelative(prev_node_idx, curr_node_idx); + // if( !gtsam::Pose3::equals(relative_pose, gtsam::Pose3::identity()) ) { + if( !relative_pose.equals( gtsam::Pose3::identity() )) { + // gtsam::Pose3 relative_pose = relative_pose_optional.value(); mtxPosegraph.lock(); - gtSAMgraph.add(gtsam::BetweenFactor(prev_node_idx, curr_node_idx, relative_pose, robustLoopNoise)); + gtSAMgraph.add(gtsam::BetweenFactor(curr_node_idx, prev_node_idx, relative_pose, robustLoopNoise)); // runISAM2opt(); mtxPosegraph.unlock(); } @@ -700,7 +936,7 @@ void process_icp(void) void process_viz_path(void) { - float hz = 10.0; + float hz = vizPathFrequency; ros::Rate rate(hz); while (ros::ok()) { rate.sleep(); @@ -712,14 +948,14 @@ void process_viz_path(void) void process_isam(void) { - float hz = 1; + float hz = graphUpdateFrequency; ros::Rate rate(hz); while (ros::ok()) { rate.sleep(); if( gtSAMgraphMade ) { mtxPosegraph.lock(); runISAM2opt(); - cout << "running isam2 optimization ..." << endl; + // cout << "running isam2 optimization ..." << endl; mtxPosegraph.unlock(); saveOptimizedVerticesKITTIformat(isamCurrentEstimate, pgKITTIformat); // pose @@ -730,7 +966,7 @@ void process_isam(void) void pubMap(void) { - int SKIP_FRAMES = 2; // sparse map visulalization to save computations + int SKIP_FRAMES = 1; // sparse map visulalization to save computations int counter = 0; laserCloudMapPGO->clear(); @@ -756,7 +992,7 @@ void pubMap(void) void process_viz_map(void) { - float vizmapFrequency = 0.1; // 0.1 means run onces every 10s + // float vizmapFrequency = 0.1; // 0.1 means run onces every 10s ros::Rate rate(vizmapFrequency); while (ros::ok()) { rate.sleep(); @@ -788,6 +1024,29 @@ int main(int argc, char **argv) nh.param("sc_dist_thres", scDistThres, 0.2); nh.param("sc_max_radius", scMaximumRadius, 80.0); // 80 is recommended for outdoor, and lower (ex, 20, 40) values are recommended for indoor + // for loop closure detection + nh.param("historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0); + nh.param("historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0); + nh.param("historyKeyframeSearchNum", historyKeyframeSearchNum, 25); + nh.param("loopNoiseScore", loopNoiseScore, 0.5); + nh.param("graphUpdateTimes", graphUpdateTimes, 2); + nh.param("loopFitnessScoreThreshold", loopFitnessScoreThreshold, 0.3); + + nh.param("speedFactor", speedFactor, 1); + { + nh.param("loopClosureFrequency", loopClosureFrequency, 2); + loopClosureFrequency *= speedFactor; + nh.param("graphUpdateFrequency", graphUpdateFrequency, 1.0); + graphUpdateFrequency *= speedFactor; + nh.param("vizmapFrequency", vizmapFrequency, 0.1); + vizmapFrequency *= speedFactor; + nh.param("vizPathFrequency", vizPathFrequency, 10); + vizPathFrequency *= speedFactor; + + } + + + ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; parameters.relinearizeSkip = 1; @@ -811,6 +1070,14 @@ int main(int argc, char **argv) pubOdomAftPGO = nh.advertise("/aft_pgo_odom", 100); pubOdomRepubVerifier = nh.advertise("/repub_odom", 100); + + // for front-end + pubKeyFramesId = nh.advertise("/key_frames_ids", 10); + + // for loop closure + pubLoopConstraintEdge = nh.advertise("/loop_closure_constraints", 1); + pubLoopScanLocalRegisted = nh.advertise("/loop_scan_local_registed", 100); + pubPathAftPGO = nh.advertise("/aft_pgo_path", 100); pubMapAftPGO = nh.advertise("/aft_pgo_map", 100); @@ -823,9 +1090,9 @@ int main(int argc, char **argv) std::thread isam_update {process_isam}; // if you want to call less isam2 run (for saving redundant computations and no real-time visulization is required), uncommment this and comment all the above runisam2opt when node is added. std::thread viz_map {process_viz_map}; // visualization - map (low frequency because it is heavy) - std::thread viz_path {process_viz_path}; // visualization - path (high frequency) + //std::thread viz_path {process_viz_path}; // visualization - path (high frequency) ros::spin(); return 0; -} +} \ No newline at end of file diff --git a/SC-PGO/src/scanRegistration.cpp b/SC-PGO/src/scanRegistration.cpp index c63adf9..03b4170 100755 --- a/SC-PGO/src/scanRegistration.cpp +++ b/SC-PGO/src/scanRegistration.cpp @@ -41,7 +41,7 @@ #include "aloam_velodyne/common.h" #include "aloam_velodyne/tic_toc.h" #include -#include +#include #include #include #include