From d0db9191f824d9b92fef08e01f4dee2a299ddab4 Mon Sep 17 00:00:00 2001 From: junjie <18979441997@163.com> Date: Tue, 20 Jun 2023 20:37:58 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9B=9E=E7=8E=AF=E4=BC=98=E5=8C=96=E5=B7=B2?= =?UTF-8?q?=E7=BB=8F=E5=AE=8C=E6=88=90,=E5=B0=8Fbug=E7=AD=89=E5=BE=85?= =?UTF-8?q?=E4=BB=A5=E5=90=8E=E8=A7=A3=E5=86=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +- rviz_cfg/loam_livox.rviz | 242 ++++++++++++++++++++------------------- src/LoopOptimization.hpp | 32 ++++-- src/laserMapping.cpp | 23 ++-- 4 files changed, 165 insertions(+), 135 deletions(-) diff --git a/README.md b/README.md index 06c3f27..5ff2bdd 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,8 @@ 1. 构建关键帧（完成） 2. 将关键帧添加到因子图中（完成） 3. 检测回环优化(完成) -4. 回环优化后对姿态和局部地图进行重新矫正（回环优化后，出现多线程数据冲突问题，参考LIO-SAM来解决问题） +4. 回环优化后对姿态和局部地图进行重新矫正（回环优化后，出现多线程数据冲突问题，参考LIO-SAM来解决问题）已解决 +5. 新发现一个bug，建图出现重影，有两个，但是暂时找不到问题所在。（2023年6月20号） diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index b1021cb..12c59c0 100644 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -12,6 +12,7 @@ Panels: - /Odometry1/Odometry1/Covariance1 - /Odometry1/Odometry1/Covariance1/Position1 - /Odometry1/Odometry1/Covariance1/Orientation1 + - /Path1 - /MarkerArray1/Namespaces1 Splitter Ratio: 0.6432291865348816 Tree Height: 651 @@ -99,8 +100,8 @@ Visualization Manager: - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 26.758302688598633 - Min Value: -0.040866173803806305 + Max Value: 3.2156617641448975 + Min Value: -0.10221096128225327 Value: true Axis: Z Channel Name: intensity @@ -135,7 +136,7 @@ Visualization Manager: Class: rviz/PointCloud2 Color: 255; 0; 0 Color Transformer: FlatColor - Decay Time: 0 + Decay Time: 300 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 @@ -146,6 +147,34 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 + Style: Points + Topic: /cloud_registered_body + 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: LocalMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false @@ -190,8 +219,92 @@ Visualization Manager: Topic: /Odometry 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 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 300 + Name: KeyFrameOdometry + 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 Enabled: true Name: Odometry + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 115; 210; 22 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: KeyFramePath + 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: /KeyFrame/KeyFramepath + Unreliable: false + Value: true + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: false + 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 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: false + Enabled: true + Name: Path - Class: rviz/Axes Enabled: true Length: 0.699999988079071 @@ -199,57 +312,6 @@ Visualization Manager: Radius: 0.10000000149011612 Reference Frame: Value: true - - Alpha: 0 - Buffer Length: 2 - Class: rviz/Path - Color: 25; 255; 255 - Enabled: false - 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 - Radius: 0.029999999329447746 - Shaft Diameter: 0.4000000059604645 - Shaft Length: 0.4000000059604645 - Topic: /path - Unreliable: false - 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 @@ -258,71 +320,15 @@ 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: - {} + loop_edges: true + loop_nodes: true Queue Size: 100 Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 115; 210; 22 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.10000000149011612 - Name: 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: /KeyFrame/KeyFramepath - Unreliable: false - Value: true Enabled: true Global Options: Background Color: 0; 0; 0 @@ -351,25 +357,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 42.25148010253906 + Distance: 39.18943786621094 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 22.733163833618164 - Y: -1.5876214504241943 - Z: -17.248741149902344 + X: 28.226346969604492 + Y: -0.0013725534081459045 + Z: -11.404071807861328 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8297972679138184 + Pitch: 0.4597962498664856 Target Frame: global Value: Orbit (rviz) - Yaw: 3.197211742401123 + Yaw: 3.167212963104248 Saved: ~ Window Geometry: Displays: diff --git a/src/LoopOptimization.hpp b/src/LoopOptimization.hpp index ec0feeb..17b360d 100644 --- a/src/LoopOptimization.hpp +++ b/src/LoopOptimization.hpp @@ -10,7 +10,8 @@ * 1. 构建关键帧（完成） * 2. 将关键帧添加到因子图中（完成） * 3. 检测回环优化(完成) - * 4. 回环优化后对姿态和局部地图进行重新矫正 + * 4. 回环优化后对姿态和局部地图进行重新矫正(完成) + * 修bug：里程计一直抖动的问题，暂时没有解决 */ #include // 用于发布关键帧可视化 #include @@ -71,6 +72,8 @@ class LoopOptimization int surroundingKeyframeSize_; float historyKeyframeSearchRadius_; float historyKeyframeSearchTimeDiff_; + float repeatKeyframeSearchTimeDiff_; + double lastLoopKeyframeTime_; int historyKeyframeSearchNum_; float historyKeyframeFitnessScore_; string odometryFrame_; @@ -146,11 +149,15 @@ LoopOptimization::LoopOptimization(const ros::Publisher & pubLoopConstraint) downSizeFilterICP.setLeafSize(0.4, 0.4, 0.4); - historyKeyframeFitnessScore_ = 0.3; + historyKeyframeFitnessScore_ = 0.17; pubLoopConstraintEdge_ = pubLoopConstraint; transformTobeMapped_update_flag_ = false; + + repeatKeyframeSearchTimeDiff_ = 5.0; + + lastLoopKeyframeTime_ = -1.0; std::cout << "LoopOptimization init succeed!" << std::endl; } @@ -304,6 +311,9 @@ void LoopOptimization::loopClosureThread() *copy_cloudKeyPoses3D_ = *cloudKeyPoses3D_; // 取出位置点 mtxLoopInfo.unlock(); + // rviz展示闭环边 + visualizeLoopClosure(); + // 当前关键帧索引，候选闭环匹配帧索引 int loopKeyCur; int loopKeyPre; @@ -336,9 +346,11 @@ void LoopOptimization::loopClosureThread() icp.align(*unused_result); // 未收敛，或者匹配不够好 - if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore_) + if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore_){ + std::cout << "\033[1;31m" << "fail ! Score is : " << "\033[0m" << icp.getFitnessScore() << std::endl; continue; - std::cout << "\033[1;31m" << "the loop is useful ! the Score is : " << "\033[0m" << icp.getFitnessScore() << std::endl; + } + std::cout << "\033[1;31m" << "the loop is success ! the Score is : " << "\033[0m" << icp.getFitnessScore() << std::endl; // 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云 @@ -370,9 +382,8 @@ void LoopOptimization::loopClosureThread() mtxLoopInfo.unlock(); loopIndexContainer_[loopKeyCur] = loopKeyPre; - - // rviz展示闭环边 - visualizeLoopClosure(); + lastLoopKeyframeTime_ = copy_cloudKeyPoses3D_->points[loopKeyCur].normal_x; // 回环检测成功，就重置时间 + } } @@ -461,6 +472,13 @@ bool LoopOptimization::detectLoopClosureDistance(int *latestID, int *closestID){ *latestID = loopKeyCur; *closestID = loopKeyPre; + // 不要在段时间内多次进行回环优化 + if(abs(copy_cloudKeyPoses3D_->points[loopKeyCur].normal_x - lastLoopKeyframeTime_) > repeatKeyframeSearchTimeDiff_){ + return true; + } else { + return false; + } + return true; } diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 5dfc1bf..f7c1704 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -581,15 +581,20 @@ void traverseKDTree_and_transform(KD_TREE::KD_TREE_NODE* node) { } -// 从状态量中获得关键帧的位置和姿态 +// 从状态量中获得关键帧的位置和姿态(忘记考虑外参了) void get_pose6D(state_ikfom state_input, float *transform_output){ - Eigen::Vector3d rpy = state_input.rot.unit_quaternion().toRotationMatrix().eulerAngles(0, 1, 2); + Eigen::Vector3d temp_pos_lid = state_input.pos + state_input.rot.matrix() * state_input.offset_T_L_I; // 雷达的真实位置 + MySO3 worldRotation = state_input.rot * state_input.offset_R_L_I.inverse(); // 雷达的真实姿态 + Eigen::Vector3d rpy = worldRotation.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]; + // transform_output[3] = state_input.pos[0]; + // transform_output[4] = state_input.pos[1]; + // transform_output[5] = state_input.pos[2]; + transform_output[3] = temp_pos_lid[0]; + transform_output[4] = temp_pos_lid[1]; + transform_output[5] = temp_pos_lid[2]; } int main(int argc, char** argv) @@ -728,7 +733,7 @@ int main(int argc, char** argv) continue; } - if(0) // If you need to see map point, change to "if(1)" + if(1) // If you need to see map point, change to "if(1)" { PointVector ().swap(ikdtree.PCL_Storage); ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); @@ -796,8 +801,8 @@ int main(int argc, char** argv) Eigen::Affine3f transformTobeMapped_Affine3f = loop_opt.trans2Affine3f(transformTobeMapped); Eigen::Affine3f transformTobeMapped_optimized_Affine3f = loop_opt.trans2Affine3f(transformTobeMapped_optimized); // 计算两个变换之间的变换（从旧到新的变换） - resultTransform = transformTobeMapped_optimized_Affine3f.inverse() * transformTobeMapped_Affine3f; - traverseKDTree_and_transform(ikdtree.Root_Node); + resultTransform = transformTobeMapped_Affine3f.inverse() * transformTobeMapped_optimized_Affine3f; + traverseKDTree_and_transform(ikdtree.Root_Node); // 这里修改有问题出现，因为这里的局部地图是一直累计的，和LIO-SAM不同 std::cout << "ikdtree.size is :" << ikdtree.size() << std::endl; // 然后对当前帧的所有点进行变换 @@ -820,7 +825,7 @@ int main(int argc, char** argv) 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); + publish_map(pubLaserCloudMap); double t11 = omp_get_wtime(); // std::cout << "feats_down_size: " << feats_down_size << " Whole mapping time(ms): " << (t11 - t00)*1000 << std::endl<< std::endl;