diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5f98e2f --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +# 暂时不考虑log +log/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d72a1b..2ed1d88 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,8 @@ find_package(catkin REQUIRED COMPONENTS eigen_conversions ) +find_package( GTSAMCMakeTools ) +find_package(GTSAM REQUIRED QUIET) find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) find_package( Sophus REQUIRED ) @@ -65,6 +67,7 @@ include_directories( ${PCL_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} ${Sophus_INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIR} include) add_message_files( @@ -79,10 +82,10 @@ generate_messages( catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime - DEPENDS EIGEN3 PCL + DEPENDS EIGEN3 PCL GTSAM INCLUDE_DIRS ) add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) -target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${Sophus_LIBRARIES} fmt) +target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${Sophus_LIBRARIES} fmt gtsam) target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) \ No newline at end of file diff --git a/README.md b/README.md index 6274e19..03c2a59 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,13 @@ # S-FAST_LIO + +## 添加回环优化部分 +1. 构建关键帧（完成） +2. 将关键帧添加到因子图中（完成） +3. 检测回环优化(完成) +4. 回环优化后对姿态和局部地图进行重新矫正 + + + ## Simplified Implementation of FAST_LIO S-FAST_LIO is a simplified implementation of FAST_LIO (Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry."), which is modified from [FAST_LIO](https://github.com/hku-mars/FAST_LIO). This code is clean and accessible. It is a reference material for SLAM beginners.The main modifications are as follows: diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index 6261935..a2b8deb 100644 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -6,17 +6,15 @@ Panels: Expanded: - /Global Options1 - /mapping1 - - /mapping1/surround1 - - /mapping1/currPoints1 - /mapping1/currPoints1/Autocompute Value Bounds1 - - /Odometry1/Odometry1 + - /Odometry1 - /Odometry1/Odometry1/Shape1 - /Odometry1/Odometry1/Covariance1 - /Odometry1/Odometry1/Covariance1/Position1 - /Odometry1/Odometry1/Covariance1/Orientation1 - /MarkerArray1/Namespaces1 Splitter Ratio: 0.6432291865348816 - Tree Height: 1105 + Tree Height: 651 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -61,14 +59,12 @@ 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 Radius: 0.05999999865889549 Reference Frame: - Show Trail: false Value: false - Class: rviz/Group Displays: @@ -103,15 +99,15 @@ Visualization Manager: - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 21.307769775390625 - Min Value: -55.33193588256836 + Max Value: 6.4026899337768555 + Min Value: 0.03668518736958504 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor - Decay Time: 1000 + Decay Time: 60 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 @@ -181,7 +177,6 @@ Visualization Manager: Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -197,14 +192,12 @@ Visualization Manager: 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: - Show Trail: false Value: true - Alpha: 0 Buffer Length: 2 @@ -223,7 +216,6 @@ Visualization Manager: Z: 0 Pose Color: 25; 255; 255 Pose Style: None - Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 @@ -266,6 +258,48 @@ Visualization Manager: {} Queue Size: 100 Value: false + - 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: false + Keep: 300 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /KeyFrame/odometry + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /KeyFrame/loop_closure_constraints + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 0; 0; 0 @@ -294,33 +328,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 126.42096710205078 + Distance: 38.23896026611328 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: 37.57487106323242 - Y: 53.90274429321289 - Z: -1.1499662399291992 + X: 27.581117630004883 + Y: 2.437171459197998 + Z: -14.271961212158203 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5647965669631958 + Pitch: 0.5397967100143433 Target Frame: global - Yaw: 3.46720814704895 + Value: Orbit (rviz) + Yaw: 3.322209119796753 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1369 + Height: 890 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c800000498fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004700000498000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009a800000052fc0100000002fb0000000800540069006d00650100000000000009a80000037100fffffffb0000000800540069006d00650100000000000004500000000000000000000007d90000049800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c8000002c8fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000057300000052fc0100000002fb0000000800540069006d0065010000000000000573000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a5000002c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -329,6 +363,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2472 - X: 1920 - Y: 34 + Width: 1395 + X: 1165 + Y: 53 diff --git a/src/LoopOptimization.hpp b/src/LoopOptimization.hpp new file mode 100644 index 0000000..f028c05 --- /dev/null +++ b/src/LoopOptimization.hpp @@ -0,0 +1,594 @@ +/** + * @file LoopOptimization.hpp + * @author your name (you@domain.com) + * @brief 用于进行回环优化的部分 + * @version 0.1 + * @date 2023-06-17 + * + * @copyright Copyright (c) 2023 + * @todo + * 1. 构建关键帧（完成） + * 2. 将关键帧添加到因子图中（完成） + * 3. 检测回环优化(完成) + * 4. 回环优化后对姿态和局部地图进行重新矫正 + */ +#include // 用于发布关键帧可视化 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; + +class LoopOptimization +{ +public: + // 定义关键帧的数据 + pcl::PointCloud::Ptr cloudKeyPoses3D_; // 关键帧的位置 + pcl::PointCloud::Ptr copy_cloudKeyPoses3D_; // 关键帧的位置 + std::vector::Ptr> pointCloud_keyFrame_; // 关键帧的点云（lidar系下） + std::vector transformTobeMapped_vector_; // 关键帧的姿态六维 + bool transformTobeMapped_update_flag_; + int KeyFrameNumber; + double timeLaserInfoCur_; + pcl::VoxelGrid downSizeFilterICP; + + // 回环信息数据 + std::map loopIndexContainer_; // from new to old + std::vector> loopIndexQueue_; + std::vector loopPoseQueue_; + std::vector loopNoiseQueue_; + + pcl::KdTreeFLANN::Ptr kdtreeHistoryKeyPoses_; + + // 关键帧参数 + float surroundingkeyframeAddingDistThreshold_; + float surroundingkeyframeAddingAngleThreshold_; + + // Loop closure + bool aLoopIsClosed_; + bool loopClosureEnableFlag_; + float loopClosureFrequency_; + int surroundingKeyframeSize_; + float historyKeyframeSearchRadius_; + float historyKeyframeSearchTimeDiff_; + int historyKeyframeSearchNum_; + float historyKeyframeFitnessScore_; + string odometryFrame_; + + ros::Publisher pubLoopConstraintEdge_; + + std::mutex mtxLoopInfo; + + // voxel filter paprams + float pointCloudLeafSize_; + + // gtsam + gtsam::NonlinearFactorGraph gtSAMgraph_; + gtsam::Values initialEstimate_; + gtsam::Values optimizedEstimate_; + gtsam::ISAM2 *isam_; + gtsam::Values isamCurrentEstimate_; + + +public: + LoopOptimization(const ros::Publisher & pubLoopConstraint); + ~LoopOptimization(); + bool saveKeyFrame(pcl::PointCloud::Ptr cloud_input, float pose[6]); + void addOdomFactor(pcl::PointCloud::Ptr cloud_input, float pose[6], double lidar_time); + void addLoopFactor(); + void optimization(); + void gtsamClear(); + + void publish_KeyFrame(const ros::Publisher & pubOdomAftMapped, double lidar_time, string odometryFrame); + + void loopClosureThread(); + void visualizeLoopClosure(); + bool detectLoopClosureDistance(int *latestID, int *closestID); + void loopFindNearKeyframes(pcl::PointCloud::Ptr& nearKeyframes, const int& key, const int& searchNum); + pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, float * transformIn); + + void correctPoses(); + void getCurrpose(float * transformOut); + // 位姿态变换函数 + gtsam::Pose3 trans2gtsamPose(float transformIn[]); + Eigen::Affine3f trans2Affine3f(float transformIn[]); +}; + +LoopOptimization::LoopOptimization(const ros::Publisher & pubLoopConstraint) +{ + // 对数据进行初始化 + cloudKeyPoses3D_.reset(new pcl::PointCloud()); + copy_cloudKeyPoses3D_.reset(new pcl::PointCloud()); + + // 参数初始化，后期使用yaml文件调试参数 + KeyFrameNumber = 0; + surroundingkeyframeAddingDistThreshold_ = 1.0; + surroundingkeyframeAddingAngleThreshold_ = 0.2; + aLoopIsClosed_ = false; + + // ISM2参数 + gtsam::ISAM2Params parameters; + parameters.relinearizeThreshold = 0.1; + parameters.relinearizeSkip = 1; + isam_ = new gtsam::ISAM2(parameters); + + // 发布激光里程计，rviz中表现为坐标轴 + + loopClosureEnableFlag_ = true; + loopClosureFrequency_ = 1.0; + historyKeyframeSearchNum_ = 20; + + historyKeyframeSearchRadius_ = 5.0; + + kdtreeHistoryKeyPoses_.reset(new pcl::KdTreeFLANN()); + timeLaserInfoCur_ = 1.0; + historyKeyframeSearchTimeDiff_ = 40.0; + + downSizeFilterICP.setLeafSize(0.4, 0.4, 0.4); + + historyKeyframeFitnessScore_ = 0.3; + + pubLoopConstraintEdge_ = pubLoopConstraint; + + transformTobeMapped_update_flag_ = false; + std::cout << "LoopOptimization init succeed!" << std::endl; +} + +LoopOptimization::~LoopOptimization() +{ +} + +// 输入本帧的点云和姿态，构建关键帧 PointCloudXYZI::Ptr +bool LoopOptimization::saveKeyFrame(pcl::PointCloud::Ptr cloud_input, float pose[6]){ + // 为第一帧创建关键帧 + if (cloudKeyPoses3D_->points.empty()) + return true; + // 前一帧位姿 + Eigen::Affine3f transStart = trans2Affine3f(transformTobeMapped_vector_.back()); + // 当前帧位姿 + Eigen::Affine3f transFinal = trans2Affine3f(pose); + // 位姿变换增量 + Eigen::Affine3f transBetween = transStart.inverse() * transFinal; + float x, y, z, roll, pitch, yaw; + pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); + // 旋转和平移量都较小，当前帧不设为关键帧 + if (abs(roll) < surroundingkeyframeAddingAngleThreshold_ && + abs(pitch) < surroundingkeyframeAddingAngleThreshold_ && + abs(yaw) < surroundingkeyframeAddingAngleThreshold_ && + sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold_) + return false; + + return true; +} + + +// 添加新关键帧，添加到gtsam优化中去 +void LoopOptimization::addOdomFactor(pcl::PointCloud::Ptr cloud_input, float pose[6], double lidar_time){ + // 添加到因子图中 + if (cloudKeyPoses3D_->points.empty()){ + // 第一帧初始化先验因子 + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter + gtSAMgraph_.add(gtsam::PriorFactor(0, trans2gtsamPose(pose), priorNoise)); + // 变量节点设置初始值 + initialEstimate_.insert(0, trans2gtsamPose(pose)); + } 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 = trans2gtsamPose(transformTobeMapped_vector_.back()); + gtsam::Pose3 poseTo = trans2gtsamPose(pose); + // 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差 + gtSAMgraph_.add(gtsam::BetweenFactor(cloudKeyPoses3D_->size()-1, cloudKeyPoses3D_->size(), poseFrom.between(poseTo), odometryNoise)); + // 变量节点设置初始值 + initialEstimate_.insert(cloudKeyPoses3D_->size(), poseTo); + } + // 添加关键帧信息 + pcl::PointCloud::Ptr thisKeyFrame_PointCloud(new pcl::PointCloud()); + pcl::copyPointCloud(*cloud_input, *thisKeyFrame_PointCloud); + pointCloud_keyFrame_.push_back(thisKeyFrame_PointCloud); + // (下面这些，应该在优化结束后添加) + float* array = new float[6]; // 使用 new 运算符在堆区分配内存(注意内存的管理) + for(int i = 0; i < 6; i++) + array[i] = pose[i]; + transformTobeMapped_vector_.push_back(array); // 这样的数据保存有问题 + PointType pose3D; + pose3D.x = pose[3]; + pose3D.y = pose[4]; + pose3D.z = pose[5]; + pose3D.intensity = float(KeyFrameNumber); // 记录当前点对应的帧的id + pose3D.normal_x = float(lidar_time); // 用normal_x记录时间 + cloudKeyPoses3D_->points.push_back(pose3D); + timeLaserInfoCur_ = lidar_time; + KeyFrameNumber++; +} + +// 添加闭环因子(这里需要外部的数据) +void LoopOptimization::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]; + gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue_[i]; + gtSAMgraph_.add(gtsam::BetweenFactor(indexFrom, indexTo, poseBetween, noiseBetween)); + } + std::cout << "----------插入回环优化了-----------" << std::endl; + + loopIndexQueue_.clear(); + loopPoseQueue_.clear(); + loopNoiseQueue_.clear(); + aLoopIsClosed_ = true; +} + +void LoopOptimization::optimization(){ + // gtSAMgraph_.print("--------------------------\n"); + // 执行优化 + isam_->update(gtSAMgraph_, initialEstimate_); + isam_->update(); +} + +void LoopOptimization::gtsamClear(){ + // update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了 + gtSAMgraph_.resize(0); + initialEstimate_.clear(); +} + +// 发布一个坐标轴，作为关键帧的位姿 +void LoopOptimization::publish_KeyFrame(const ros::Publisher & pubOdomAftMapped, double lidar_time, string odometryFrame){ + // 发布激光里程计，odom等价map + float *transform = transformTobeMapped_vector_.back(); // 指向同一个堆区 + odometryFrame_ = odometryFrame; + nav_msgs::Odometry laserKeyFrameROS; + laserKeyFrameROS.header.stamp = ros::Time().fromSec(lidar_time); + laserKeyFrameROS.header.frame_id = odometryFrame; + laserKeyFrameROS.child_frame_id = "KeyFrame"; + laserKeyFrameROS.pose.pose.position.x = transform[3]; + laserKeyFrameROS.pose.pose.position.y = transform[4]; + laserKeyFrameROS.pose.pose.position.z = transform[5]; + laserKeyFrameROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transform[0], transform[1], transform[2]); + pubOdomAftMapped.publish(laserKeyFrameROS); +} + + + +// 进行欧氏距离的回环检测函数 +/** + * 闭环线程 + * 1、闭环scan-to-map，icp优化位姿 + * 1) 在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧 + * 2) 提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样 + * 3) 执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿 + * 2、rviz展示闭环边 +*/ +void LoopOptimization::loopClosureThread() +{ + if (loopClosureEnableFlag_ == false) + return; + + ros::Rate rate(loopClosureFrequency_); + while (ros::ok()) + { + rate.sleep(); + // 闭环scan-to-map，icp优化位姿 + // 1、在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧 + // 2、提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样 + // 3、执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿 + // 注：闭环的时候没有立即更新当前帧的位姿，而是添加闭环因子，让图优化去更新位姿 + if (cloudKeyPoses3D_->points.empty() == true) + continue; // 没有直接跳过 + mtxLoopInfo.lock(); + *copy_cloudKeyPoses3D_ = *cloudKeyPoses3D_; // 取出位置点 + mtxLoopInfo.unlock(); + + // 当前关键帧索引，候选闭环匹配帧索引 + int loopKeyCur; + int loopKeyPre; + + if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false) + continue; + std::cout << "\033[1;31m" << "find loop succed!" << "\033[0m" << std::endl; + // 提取 + pcl::PointCloud::Ptr cureKeyframeCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr prevKeyframeCloud(new pcl::PointCloud()); + // 提取当前关键帧特征点集合，降采样 + loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0); + loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum_); + + // 如果特征点较少，结束 + if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000) + continue; + // ICP参数设置 + static pcl::IterativeClosestPoint icp; + icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius_*2); + icp.setMaximumIterations(100); + icp.setTransformationEpsilon(1e-6); + icp.setEuclideanFitnessEpsilon(1e-6); + icp.setRANSACIterations(0); + + // scan-to-map，调用icp匹配 + icp.setInputSource(cureKeyframeCloud); + icp.setInputTarget(prevKeyframeCloud); + pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); + icp.align(*unused_result); + + // 未收敛，或者匹配不够好 + if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore_) + continue; + std::cout << "\033[1;31m" << "the loop is useful ! the Score is : " << "\033[0m" << icp.getFitnessScore() << std::endl; + + // 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云 + + + // 闭环优化得到的当前关键帧与闭环关键帧之间的位姿变换 + float x, y, z, roll, pitch, yaw; + Eigen::Affine3f correctionLidarFrame; + correctionLidarFrame = icp.getFinalTransformation(); + + // 闭环优化前当前帧位姿 + Eigen::Affine3f tWrong = trans2Affine3f(transformTobeMapped_vector_[copy_cloudKeyPoses3D_->points[loopKeyCur].intensity]); + // 闭环优化后当前帧位姿 + Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong; + pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw); + gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::RzRyRx(roll, pitch, yaw), gtsam::Point3(x, y, z)); + + // 闭环匹配帧的位姿 + gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped_vector_[copy_cloudKeyPoses3D_->points[loopKeyPre].intensity]); + gtsam::Vector Vector6(6); + float noiseScore = icp.getFitnessScore(); + Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore; + gtsam::noiseModel::Diagonal::shared_ptr constraintNoise = gtsam::noiseModel::Diagonal::Variances(Vector6); + + // 添加闭环因子需要的数据 + mtxLoopInfo.lock(); + loopIndexQueue_.push_back(make_pair(loopKeyCur, loopKeyPre)); + loopPoseQueue_.push_back(poseFrom.between(poseTo)); + loopNoiseQueue_.push_back(constraintNoise); + mtxLoopInfo.unlock(); + + loopIndexContainer_[loopKeyCur] = loopKeyPre; + + // rviz展示闭环边 + visualizeLoopClosure(); + } +} + +// rviz展示闭环边 +void LoopOptimization::visualizeLoopClosure(){ + if (loopIndexContainer_.empty()) + return; + visualization_msgs::MarkerArray markerArray; + // 闭环顶点 + visualization_msgs::Marker markerNode; + markerNode.header.frame_id = odometryFrame_; + markerNode.header.stamp = ros::Time().fromSec(cloudKeyPoses3D_->points.back().normal_x); + 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 = ros::Time().fromSec(cloudKeyPoses3D_->points.back().normal_x); + 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 = cloudKeyPoses3D_->points[key_cur].x; + p.y = cloudKeyPoses3D_->points[key_cur].y; + p.z = cloudKeyPoses3D_->points[key_cur].z; + markerNode.points.push_back(p); + markerEdge.points.push_back(p); + p.x = cloudKeyPoses3D_->points[key_pre].x; + p.y = cloudKeyPoses3D_->points[key_pre].y; + p.z = cloudKeyPoses3D_->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 LoopOptimization::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 pointSearchIndLoop; + std::vector pointSearchSqDisLoop; + kdtreeHistoryKeyPoses_->setInputCloud(copy_cloudKeyPoses3D_); + kdtreeHistoryKeyPoses_->radiusSearch(copy_cloudKeyPoses3D_->back(), historyKeyframeSearchRadius_, pointSearchIndLoop, pointSearchSqDisLoop, 0); + + // 在候选关键帧集合中，找到与当前帧时间相隔较远的帧，设为候选匹配帧 + for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i) + { + int id = pointSearchIndLoop[i]; + if (abs(copy_cloudKeyPoses3D_->points[id].normal_x - timeLaserInfoCur_) > historyKeyframeSearchTimeDiff_) + { + loopKeyPre = id; + break; + } + } + if (loopKeyPre == -1 || loopKeyCur == loopKeyPre) + return false; + + *latestID = loopKeyCur; + *closestID = loopKeyPre; + + return true; +} + + +/** + * 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合，降采样 +*/ +void LoopOptimization::loopFindNearKeyframes(pcl::PointCloud::Ptr& nearKeyframes, const int& key, const int& searchNum) +{ + // 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合 + nearKeyframes->clear(); + int cloudSize = copy_cloudKeyPoses3D_->size(); + for (int i = -searchNum; i <= searchNum; ++i) + { + int keyNear = key + i; + if (keyNear < 0 || keyNear >= cloudSize ) + continue; + *nearKeyframes += *transformPointCloud(pointCloud_keyFrame_[keyNear], transformTobeMapped_vector_[copy_cloudKeyPoses3D_->points[keyNear].intensity]); + } + + if (nearKeyframes->empty()) + return; + + // 降采样 + pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); + downSizeFilterICP.setInputCloud(nearKeyframes); + downSizeFilterICP.filter(*cloud_temp); + *nearKeyframes = *cloud_temp; +} + +/** + * 对点云cloudIn进行变换transformIn，返回结果点云 +*/ +pcl::PointCloud::Ptr LoopOptimization::transformPointCloud(pcl::PointCloud::Ptr cloudIn, float * transformIn) +{ + pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); + + int cloudSize = cloudIn->size(); + cloudOut->resize(cloudSize); + + Eigen::Affine3f transCur = pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]); + + // 多线程加速(暂时不开启) + // #pragma omp parallel for num_threads(numberOfCores) + for (int i = 0; i < cloudSize; ++i) + { + const auto &pointFrom = cloudIn->points[i]; + cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3); + cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3); + cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3); + cloudOut->points[i].intensity = pointFrom.intensity; + } + return cloudOut; +} + +// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹(对正常输出进行一帧的输出，检测到回环重新全部矫正) +// 还有对局部地图进行变换（在检测到回环后进行） +void LoopOptimization::correctPoses(){ + if(!aLoopIsClosed_){ // 没有回环只得到最近一帧的位置和姿态 + gtsam::Pose3 latestEstimate; + isamCurrentEstimate_ = isam_->calculateEstimate(); + latestEstimate = isamCurrentEstimate_.at(isamCurrentEstimate_.size()-1); // 最新一帧 + // 添加新的优化后的位置 + float* array = new float[6]; // 使用 new 运算符在堆区分配内存(注意内存的管理) + array[0] = latestEstimate.rotation().roll(); + array[1] = latestEstimate.rotation().pitch(); + array[2] = latestEstimate.rotation().yaw(); + array[3] = latestEstimate.translation().x(); + array[4] = latestEstimate.translation().y(); + array[5] = latestEstimate.translation().z(); + transformTobeMapped_vector_.push_back(array); // 这样的数据保存有问题 + PointType pose3D; + pose3D.x = array[3]; + pose3D.y = array[4]; + pose3D.z = array[5]; + pose3D.intensity = float(KeyFrameNumber); // 记录当前点对应的帧的id + pose3D.normal_x = float(timeLaserInfoCur_); // 用normal_x记录时间 + cloudKeyPoses3D_->points.push_back(pose3D); + // 这里暂时就不对协方差进行更新了 + } else { // 对所有位置都进行修改 + aLoopIsClosed_ = false; + if (cloudKeyPoses3D_->points.empty()) + return; + // 清空局部map + + // 清空里程计轨迹 + + // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿 + int numPoses = isamCurrentEstimate_.size(); + for (int i = 0; i < numPoses; ++i) { + cloudKeyPoses3D_->points[i].x = isamCurrentEstimate_.at(i).translation().x(); + cloudKeyPoses3D_->points[i].y = isamCurrentEstimate_.at(i).translation().y(); + cloudKeyPoses3D_->points[i].z = isamCurrentEstimate_.at(i).translation().z(); + + transformTobeMapped_vector_[i][0] = isamCurrentEstimate_.at(i).rotation().roll(); + transformTobeMapped_vector_[i][1] = isamCurrentEstimate_.at(i).rotation().pitch(); + transformTobeMapped_vector_[i][2] = isamCurrentEstimate_.at(i).rotation().yaw(); + transformTobeMapped_vector_[i][3] = cloudKeyPoses3D_->points[i].x; + transformTobeMapped_vector_[i][4] = cloudKeyPoses3D_->points[i].y; + transformTobeMapped_vector_[i][5] = cloudKeyPoses3D_->points[i].z; + + // 更新里程计轨迹(尽量不要在里面进行显示，发送到主函数让他显示) + // updatePath(transformTobeMapped_vector_[i]); + } + transformTobeMapped_update_flag_ = true; // 告诉外部程序，这里已经进行了更新了，需要重新发布一次整个的状态 + } +} + +void LoopOptimization::getCurrpose(float * transformOut){ + for(int i = 0; i < 6; i++) + transformOut[i] = transformTobeMapped_vector_.back()[i]; +} + +/** + * 位姿格式变换 +*/ +gtsam::Pose3 LoopOptimization::trans2gtsamPose(float transformIn[]) +{ + return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]), + gtsam::Point3(transformIn[3], transformIn[4], transformIn[5])); +} + +/** + * Eigen格式的位姿变换 +*/ +Eigen::Affine3f LoopOptimization::trans2Affine3f(float transformIn[]) +{ + return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], + transformIn[0], transformIn[1], transformIn[2]); +} + diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 542b2e6..29387f3 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -24,6 +24,7 @@ #include #include "IMU_Processing.hpp" +#include "LoopOptimization.hpp" #define INIT_TIME (0.1) #define LASER_POINT_COV (0.001) @@ -55,6 +56,10 @@ int feats_down_size = 0, NUM_MAX_ITERATIONS = 0, pcd_save_interval = -1, pcd_ bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; +// 六维分别对应 roll pitch yaw x y z +float transformTobeMapped[6]; // 当前帧的姿态，用于构建关键帧 +float transformTobeMapped_optimized[6]; // 优化后的姿态 + vector cub_needrm; vector Nearest_Points; vector extrinT(3, 0.0); @@ -528,6 +533,16 @@ void publish_path(const ros::Publisher pubPath) } } +// 从状态量中获得关键帧的位置和姿态 +void get_pose6D(state_ikfom state_input, float *transform_output){ + Eigen::Vector3d rpy = state_input.rot.unit_quaternion().toRotationMatrix().eulerAngles(0, 1, 2); + transform_output[0] = rpy[0]; // roll + transform_output[1] = rpy[1]; // pitch + transform_output[2] = rpy[2]; // yaw + transform_output[3] = state_input.pos[0]; + transform_output[4] = state_input.pos[1]; + transform_output[5] = state_input.pos[2]; +} int main(int argc, char** argv) { @@ -583,6 +598,8 @@ int main(int argc, char** argv) ros::Publisher pubLaserCloudMap = nh.advertise("/Laser_map", 100000); ros::Publisher pubOdomAftMapped = nh.advertise ("/Odometry", 100000); ros::Publisher pubPath = nh.advertise ("/path", 100000); + ros::Publisher pubLaserKeyFrameGlobal = nh.advertise ("/KeyFrame/odometry", 1); // 发布可视化 + ros::Publisher pubLoopConstraintEdge = nh.advertise("/KeyFrame/loop_closure_constraints", 1); // 回环边可视化 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); @@ -597,6 +614,10 @@ int main(int argc, char** argv) signal(SIGINT, SigHandle); ros::Rate rate(5000); + LoopOptimization loop_opt(pubLoopConstraintEdge); + + std::thread loopthread(&LoopOptimization::loopClosureThread, &loop_opt); + while (ros::ok()) { if (flg_exit) break; @@ -670,6 +691,41 @@ int main(int argc, char** argv) state_point = kf.get_x(); pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I; + static int KeyFrameNumber = 0; + get_pose6D(state_point, transformTobeMapped); + if(loop_opt.saveKeyFrame(feats_down_body, transformTobeMapped) == true){ + KeyFrameNumber++; + std::cout << "Add new key frame! " << KeyFrameNumber << std::endl; + loop_opt.addOdomFactor(feats_down_body, transformTobeMapped, Measures.lidar_end_time); + loop_opt.addLoopFactor(); // 添加回环优化因子 + loop_opt.optimization(); + loop_opt.gtsamClear(); + // 回环优化后对姿态和局部地图进行重新矫正 + // loop_opt.correctPoses(); + + // 然后重新发布一下整体的姿态，并且调整x状态量，以便进行之后的滤波 + + loop_opt.getCurrpose(transformTobeMapped_optimized); // 得到优化后的姿态 + + // 发布关键帧进行可视化 + loop_opt.publish_KeyFrame(pubLaserKeyFrameGlobal, Measures.lidar_end_time, "camera_init"); + } + // 先修改当前状态 + // Eigen::AngleAxisd roll_rotation(transformTobeMapped_optimized[0], Eigen::Vector3d::UnitX()); + // Eigen::AngleAxisd pitch_rotation(transformTobeMapped_optimized[1], Eigen::Vector3d::UnitY()); + // Eigen::AngleAxisd yaw_rotation(transformTobeMapped_optimized[2], Eigen::Vector3d::UnitZ()); + // Eigen::Quaterniond temp_q = yaw_rotation * pitch_rotation * roll_rotation; + // state_point.rot.setQuaternion(temp_q); + // state_point.pos.x() = transformTobeMapped_optimized[3]; + // state_point.pos.y() = transformTobeMapped_optimized[4]; + // state_point.pos.z() = transformTobeMapped_optimized[5]; + // kf.change_x(state_point); // 应该是基本没有变化（线进行可视化看，是不是进行了回环优化） + + + if(loop_opt.transformTobeMapped_update_flag_){ // 进行了回环更新了，需要重新发布一下状态进行可视化 + + } + /******* Publish odometry *******/ publish_odometry(pubOdomAftMapped); @@ -678,7 +734,7 @@ int main(int argc, char** argv) map_incremental(); /******* Publish points *******/ - if (path_en) publish_path(pubPath); + if (path_en) publish_path(pubPath); // 这个发布的就是路径，需要更新这个 if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body); // publish_map(pubLaserCloudMap); @@ -690,6 +746,7 @@ int main(int argc, char** argv) rate.sleep(); } + loopthread.join(); /**************** save map ****************/ /* 1. make sure you have enough memories /* 2. pcd save will largely influence the real-time performences **/