diff --git a/config/vins_rviz_config.rviz b/config/vins_rviz_config.rviz index 6ed63a93d..0ffefd1b3 100644 --- a/config/vins_rviz_config.rviz +++ b/config/vins_rviz_config.rviz @@ -4,8 +4,8 @@ Panels: Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.465115994 - Tree Height: 221 + Splitter Ratio: 0.4651159942150116 + Tree Height: 168 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -14,14 +14,13 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: tracked image @@ -31,7 +30,11 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 359 + Tree Height: 365 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -41,7 +44,7 @@ Visualization Manager: Color: 130; 130; 130 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -53,21 +56,23 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 1 Name: Axes - Radius: 0.100000001 + Radius: 0.10000000149011612 Reference Frame: + Show Trail: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path @@ -77,9 +82,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true @@ -96,7 +102,7 @@ Visualization Manager: Unreliable: false Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /cam0/image_raw Max Value: 1 Median window: 5 @@ -106,7 +112,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Group Displays: - Alpha: 1 @@ -114,11 +120,11 @@ Visualization Manager: Class: rviz/Path Color: 0; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: Path Offset: X: 0 @@ -126,9 +132,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true @@ -137,7 +144,7 @@ Visualization Manager: Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 Value: true - Alpha: 1 @@ -155,15 +162,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: current_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false @@ -202,19 +207,31 @@ Visualization Manager: Value: false - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true + body: + Value: true + camera: + Value: true + world: + Value: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + world: + body: + camera: + {} Update Interval: 0 Value: true - Enabled: false + Enabled: true Name: VIO - Class: rviz/Group Displays: @@ -223,11 +240,11 @@ Visualization Manager: Class: rviz/Path Color: 0; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: pose_graph_path Offset: X: 0 @@ -235,9 +252,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true @@ -246,11 +264,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 170; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 @@ -258,9 +276,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: true @@ -269,7 +288,7 @@ Visualization Manager: Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -277,7 +296,7 @@ Visualization Manager: Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 Value: true - Class: rviz/Image @@ -297,7 +316,7 @@ Visualization Manager: Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: - {} + key_odometrys: true Queue Size: 100 Value: true - Alpha: 1 @@ -305,11 +324,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 @@ -317,9 +336,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: true @@ -328,11 +348,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 0; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 @@ -340,9 +360,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: true @@ -351,11 +372,11 @@ Visualization Manager: Class: rviz/Path Color: 0; 85; 255 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 @@ -363,9 +384,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: true @@ -374,11 +396,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 170; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 @@ -386,9 +408,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: true @@ -397,11 +420,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 170; 255 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 @@ -409,9 +432,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: true @@ -420,11 +444,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 @@ -432,9 +456,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: true @@ -443,6 +468,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 0; 0; 0 + Default Light: true Fixed Frame: world Frame Rate: 30 Name: root @@ -452,7 +478,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -462,33 +491,33 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 20.9684067 + Distance: 14.08361530303955 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: -2.7069304 - Y: -1.26974416 - Z: 2.1410624e-05 + X: -1.4666130542755127 + Y: -0.9299832582473755 + Z: 2.141062395821791e-05 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 1.0797962 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1397961378097534 Target Frame: - Value: XYOrbit (rviz) - Yaw: 3.08722663 + Yaw: 3.0522265434265137 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 912 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001df00000309fc0200000010fb0000000a0049006d00610067006501000000280000013d0000000000000000fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200720061007700200049006d0061006700650000000028000000300000000000000000fb00000012007200610077005f0069006d0061006700650000000028000000f90000001600fffffffb0000001a0074007200610063006b0065006400200069006d00610067006501000000280000012c0000001600fffffffb00000020006c006f006f0070005f006d0061007400630068005f0069006d006100670065010000015a000000b30000001600fffffffb000000100044006900730070006c00610079007301000002130000011e000000dd00fffffffc000000280000011e0000000000fffffffa000000000100000002fb0000001200720061007700200049006d0061006700650000000000ffffffff0000000000000000fb0000001a0074007200610063006b0065006400200069006d0061006700650100000000000002370000000000000000fb0000001000410052005f0069006d0061006700650100000373000000160000000000000000fb0000001200720061007700200069006d006100670065010000038f000000160000000000000000000000010000020800000399fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc00000028000003990000000000fffffffaffffffff0100000002fb000000100044006900730070006c0061007900730000000000ffffffff0000016a00fffffffb0000000a00560069006500770073000000023f0000016a0000010f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056e0000003bfc0100000002fb0000000800540069006d006501000000000000056e0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003890000030900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -497,9 +526,9 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1390 - X: 2127 - Y: 109 + Width: 1862 + X: 1138 + Y: 453 loop_match_image: collapsed: false raw_image: diff --git a/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp b/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp index f43ee37d5..cb56642ed 100644 --- a/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp +++ b/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp @@ -17,24 +17,24 @@ #include "pose_graph/details/keyframe.h" namespace pose_graph { -class PoseGraphEventObserver - : public std::enable_shared_from_this { +class PoseGraphEventObserver { public: PoseGraphEventObserver() = default; virtual ~PoseGraphEventObserver() = default; virtual void OnPoseGraphLoaded() = 0; virtual void OnPoseGraphSaved() = 0; - virtual void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) = 0; + virtual void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute, + int sequence_count) = 0; virtual void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, - int count) = 0; + int count, int sequence_count) = 0; virtual void OnKeyFrameConnectionFound( KeyFrame::Attributes current_kf_attribute, KeyFrame::Attributes old_kf_attribute, std::vector matched_2d_old_norm, std::vector matched_id, cv::Mat& thumb_image) = 0; virtual void OnPoseGraphOptimization( - std::vector kf_attributes) = 0; + std::vector kf_attributes, int sequence_count) = 0; virtual void OnNewSequentialEdge(Eigen::Vector3d p1, Eigen::Vector3d p2) = 0; virtual void OnNewLoopEdge(Eigen::Vector3d p1, Eigen::Vector3d p2) = 0; }; diff --git a/pose_graph/src/pose_graph.cpp b/pose_graph/src/pose_graph.cpp index 1b4b5f1e3..ae4b24135 100644 --- a/pose_graph/src/pose_graph.cpp +++ b/pose_graph/src/pose_graph.cpp @@ -241,7 +241,8 @@ bool PoseGraph::Load() { } } if (event_observer_) { - event_observer_->OnKeyFrameLoaded(current_kf_attribute, count); + event_observer_->OnKeyFrameLoaded(current_kf_attribute, count, + GetCurrentSequenceCount()); } count++; } @@ -489,7 +490,8 @@ void PoseGraph::AddKeyFrame(std::shared_ptr current_keyframe) { } // Generic callback - event_observer_->OnKeyFrameAdded(current_keyframe->getAttributes()); + event_observer_->OnKeyFrameAdded(current_keyframe->getAttributes(), + GetCurrentSequenceCount()); } void PoseGraph::AddKeyFrame(std::shared_ptr current_keyframe, @@ -1008,7 +1010,8 @@ void PoseGraph::StartOptimizationThread() { Optimize4DoF(); auto kf_attributes = GetKeyFrameAttributes(); if (event_observer_) { - event_observer_->OnPoseGraphOptimization(kf_attributes); + event_observer_->OnPoseGraphOptimization(kf_attributes, + GetCurrentSequenceCount()); } std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } diff --git a/pose_graph_ros/CMakeLists.txt b/pose_graph_ros/CMakeLists.txt index 31f5986a1..8e1c509c6 100644 --- a/pose_graph_ros/CMakeLists.txt +++ b/pose_graph_ros/CMakeLists.txt @@ -22,6 +22,7 @@ catkin_package() add_executable(pose_graph_node src/pose_graph_node.cpp + src/pose_graph_event_observer_impl.cpp src/utility/CameraPoseVisualization.cpp ) diff --git a/pose_graph_ros/docs/pose_graph_node_design.png b/pose_graph_ros/docs/pose_graph_node_design.png new file mode 100644 index 000000000..05a086e1f Binary files /dev/null and b/pose_graph_ros/docs/pose_graph_node_design.png differ diff --git a/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp b/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp index 8413c285f..e0a033b66 100644 --- a/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp +++ b/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include @@ -24,11 +23,11 @@ #include "camodocal/camera_models/CameraFactory.h" #include "pose_graph/pose_graph.hpp" -#include "pose_graph/details/pose_graph_event_observer.hpp" + #include "pose_graph_ros/utility/CameraPoseVisualization.h" namespace pose_graph { -class PoseGraphNode : public PoseGraphEventObserver { +class PoseGraphNode { public: ~PoseGraphNode(); @@ -40,24 +39,8 @@ class PoseGraphNode : public PoseGraphEventObserver { void StartCommandAndProcessingThreads(); void Process(); void Command(); - void Publish(); void NewSequence(); - // PoseGraphEventObserver callbacks - void OnPoseGraphLoaded() final; - void OnPoseGraphSaved() final; - void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) final; - void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, int count) final; - void OnKeyFrameConnectionFound(KeyFrame::Attributes current_kf_attribute, - KeyFrame::Attributes old_kf_attribute, - std::vector matched_2d_old_norm, - std::vector matched_id, - cv::Mat& thumb_image) final; - void OnPoseGraphOptimization( - std::vector kf_attributes) final; - void OnNewSequentialEdge(Vector3d p1, Vector3d p2) final; - void OnNewLoopEdge(Vector3d p1, Vector3d p2) final; - // ros callbacks void ImuForwardCallback(const nav_msgs::OdometryConstPtr& forward_msg); void VioCallback(const nav_msgs::OdometryConstPtr& pose_msg); @@ -69,35 +52,19 @@ class PoseGraphNode : public PoseGraphEventObserver { private: ros::NodeHandle nh_{"~"}; + + // pose graph PoseGraphConfig config_; PoseGraph pose_graph_ = PoseGraph(); - std::unique_ptr camera_pose_vis_; - bool loop_closure_; - camodocal::CameraPtr camera_; // Note: internally it uses a shared pointer. - bool visualise_imu_forward_; - std::queue image_buffer_; - std::queue point_buffer_; - std::queue pose_buffer_; - std::queue odometry_buffer_; - std::mutex buffer_mutex_; - std::mutex process_mutex_; - Eigen::Vector3d last_t_ = {-100, -100, -100}; - std::string vins_result_path_; - std::string image_topic_; - nav_msgs::Path path_[10]; - nav_msgs::Path base_path_; - nav_msgs::Path no_loop_path_; - std::unique_ptr posegraph_visualization_; + class PoseGraphEventObserverImpl; // forward declaration + std::shared_ptr pose_graph_event_observer_; // ros parameters std::string config_file_path_; - int visualization_shift_x_ = 0; - int visualization_shift_y_ = 0; int skip_cnt_threshold_ = 0; int skip_cnt_ = 0; double skip_distance_ = 0.0; static const int skip_first_cnt_threshold = 10; - const bool save_loop_path = true; int skip_first_cnt_ = 0; int frame_index_ = 0; int sequence_index_ = 1; @@ -106,6 +73,11 @@ class PoseGraphNode : public PoseGraphEventObserver { std::thread measurement_thread_; std::thread keyboard_command_thread_; + // ros publishers + ros::Publisher pub_camera_pose_visual_; + ros::Publisher pub_key_odometrys_; + ros::Publisher pub_vio_path_; + // ros subscribers ros::Subscriber sub_imu_forward_; ros::Subscriber sub_vio_; @@ -115,16 +87,20 @@ class PoseGraphNode : public PoseGraphEventObserver { ros::Subscriber sub_point_; ros::Subscriber sub_relo_relative_pose_; - // ros publishers - ros::Publisher pub_match_points_; - ros::Publisher pub_camera_pose_visual_; - ros::Publisher pub_key_odometrys_; - ros::Publisher pub_vio_path_; - ros::Publisher pub_pg_path_; - ros::Publisher pub_base_path_; - ros::Publisher pub_pose_graph_; - ros::Publisher pub_path_[10]; - ros::Publisher pub_match_img_; + // others + std::unique_ptr camera_pose_vis_; + bool loop_closure_; + camodocal::CameraPtr camera_; // Note: internally it uses a shared pointer. + bool visualise_imu_forward_; + std::queue image_buffer_; + std::queue point_buffer_; + std::queue pose_buffer_; + std::queue odometry_buffer_; + std::mutex buffer_mutex_; + std::mutex process_mutex_; + Eigen::Vector3d last_t_ = {-100, -100, -100}; + std::string image_topic_; + nav_msgs::Path no_loop_path_; }; } // namespace pose_graph diff --git a/pose_graph_ros/src/old/pose_graph_node.cpp b/pose_graph_ros/src/old/pose_graph_node.cpp deleted file mode 100644 index c794a5e77..000000000 --- a/pose_graph_ros/src/old/pose_graph_node.cpp +++ /dev/null @@ -1,768 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "pose_graph/details/keyframe.h" -#include "pose_graph/parameters.h" -#include "pose_graph/pose_graph.h" -#include "pose_graph/utility/tic_toc.h" -#include "pose_graph_ros/utility/CameraPoseVisualization.h" - -#define SKIP_FIRST_CNT 10 -#define SAVE_LOOP_PATH true -using namespace std; - -queue image_buf; -queue point_buf; -queue pose_buf; -queue odometry_buf; -std::mutex m_buf; -std::mutex m_process; -int frame_index = 0; -int sequence = 1; -PoseGraph posegraph; -int skip_first_cnt = 0; -int SKIP_CNT; -int skip_cnt = 0; -bool load_flag = 0; -bool start_flag = 0; -double SKIP_DIS = 0; -nav_msgs::Path path[10]; -nav_msgs::Path base_path; -CameraPoseVisualization* posegraph_visualization; - -int VISUALIZATION_SHIFT_X; -int VISUALIZATION_SHIFT_Y; -int ROW; -int COL; -int DEBUG_IMAGE; -int VISUALIZE_IMU_FORWARD; -int LOOP_CLOSURE; -int FAST_RELOCALIZATION; - -camodocal::CameraPtr m_camera; -Eigen::Vector3d tic; -Eigen::Matrix3d qic; -ros::Publisher pub_match_points; -ros::Publisher pub_camera_pose_visual; -ros::Publisher pub_key_odometrys; -ros::Publisher pub_vio_path; -ros::Publisher pub_pg_path; -ros::Publisher pub_base_path; -ros::Publisher pub_pose_graph; -ros::Publisher pub_path[10]; -ros::Publisher pub_match_img; -nav_msgs::Path no_loop_path; - -std::string BRIEF_PATTERN_FILE; -std::string POSE_GRAPH_SAVE_PATH; -std::string VINS_RESULT_PATH; -CameraPoseVisualization cameraposevisual(1, 0, 0, 1); -Eigen::Vector3d last_t(-100, -100, -100); -double last_image_time = -1; - -void publish() { - int sequence_cnt = posegraph.getCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - // if (sequence_loop[i] == true || i == base_sequence) - if (true) { - pub_pg_path.publish(path[i]); - pub_path[i].publish(path[i]); - posegraph_visualization->publish_by(pub_pose_graph, - path[sequence_cnt].header); - } - } - base_path.header.frame_id = "world"; - pub_base_path.publish(base_path); - // posegraph_visualization->publish_by(pub_pose_graph, - // path[sequence_cnt].header); -} - -void new_sequence() { - printf("new sequence\n"); - sequence++; - printf("sequence cnt %d \n", sequence); - if (sequence > 5) { - ROS_WARN( - "only support 5 sequences since it's boring to copy code for more " - "sequences."); - ROS_BREAK(); - } - posegraph_visualization->reset(); - publish(); - m_buf.lock(); - while (!image_buf.empty()) image_buf.pop(); - while (!point_buf.empty()) point_buf.pop(); - while (!pose_buf.empty()) pose_buf.pop(); - while (!odometry_buf.empty()) odometry_buf.pop(); - m_buf.unlock(); -} - -void on_keyframe_loaded_callback(KeyFrame* keyframe, - int pose_graph_load_count) { - Vector3d P; - Matrix3d R; - keyframe->getPose(P, R); - Quaterniond Q{R}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(keyframe->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; - pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; - pose_stamped.pose.position.z = P.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - base_path.poses.push_back(pose_stamped); - base_path.header = pose_stamped.header; - - if (pose_graph_load_count % 20 == 0) { - publish(); - } -} - -void on_optimization_step_completed_callback( - std::list keyframelist) { - list::iterator it; - int sequence_cnt = posegraph.getCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - path[i].poses.clear(); - } - base_path.poses.clear(); - posegraph_visualization->reset(); - - if (SAVE_LOOP_PATH) { - ofstream loop_path_file_tmp(VINS_RESULT_PATH, ios::out); - loop_path_file_tmp.close(); - } - - for (it = keyframelist.begin(); it != keyframelist.end(); it++) { - Vector3d P; - Matrix3d R; - (*it)->getPose(P, R); - Quaterniond Q; - Q = R; - // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); - - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time((*it)->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; - pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; - pose_stamped.pose.position.z = P.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - if ((*it)->sequence == 0) { - base_path.poses.push_back(pose_stamped); - base_path.header = pose_stamped.header; - } else { - path[(*it)->sequence].poses.push_back(pose_stamped); - path[(*it)->sequence].header = pose_stamped.header; - } - - if (SAVE_LOOP_PATH) { - ofstream loop_path_file(VINS_RESULT_PATH, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << (*it)->time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << P.x() << "," << P.y() << "," << P.z() << "," << Q.w() - << "," << Q.x() << "," << Q.y() << "," << Q.z() << "," - << endl; - loop_path_file.close(); - } - - if (SHOW_S_EDGE) { - list::reverse_iterator rit = keyframelist.rbegin(); - list::reverse_iterator lrit; - for (; rit != keyframelist.rend(); rit++) { - if ((*rit)->index == (*it)->index) { - lrit = rit; - lrit++; - for (int i = 0; i < 4; i++) { - if (lrit == keyframelist.rend()) break; - if ((*lrit)->sequence == (*it)->sequence) { - Vector3d conncected_P; - Matrix3d connected_R; - (*lrit)->getPose(conncected_P, connected_R); - posegraph_visualization->add_edge(P, conncected_P); - } - lrit++; - } - break; - } - } - } - if (SHOW_L_EDGE) { - if ((*it)->has_loop && (*it)->sequence == sequence_cnt) { - KeyFrame* connected_KF = posegraph.getKeyFrame((*it)->loop_index); - Vector3d connected_P; - Matrix3d connected_R; - connected_KF->getPose(connected_P, connected_R); - //(*it)->getVioPose(P, R); - (*it)->getPose(P, R); - if ((*it)->sequence > 0) { - posegraph_visualization->add_loopedge( - P, connected_P + - Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0)); - } - } - } - } - - publish(); -} - -void on_new_edge_callback(Vector3d p1, Vector3d p2) { - posegraph_visualization->add_edge(p1, p2); -} - -void on_new_loopedge_callback(Vector3d p1, Vector3d p2) { - posegraph_visualization->add_loopedge(p1, p2); -} - -void on_keyframe_connection_found_callback( - KeyFrame* cur_kf, KeyFrame* old_kf, - vector& matched_2d_old_norm, vector& matched_id) { - { - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), "bgr8", cur_kf->getThumbImage()) - .toImageMsg(); - msg->header.stamp = ros::Time(cur_kf->time_stamp); - pub_match_img.publish(msg); - } - if (FAST_RELOCALIZATION) { - sensor_msgs::PointCloud msg_match_points; - msg_match_points.header.stamp = ros::Time(cur_kf->time_stamp); - for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { - geometry_msgs::Point32 p; - p.x = matched_2d_old_norm[i].x; - p.y = matched_2d_old_norm[i].y; - p.z = matched_id[i]; - msg_match_points.points.push_back(p); - } - Eigen::Vector3d T = old_kf->T_w_i; - Eigen::Matrix3d R = old_kf->R_w_i; - Quaterniond Q(R); - sensor_msgs::ChannelFloat32 t_q_index; - t_q_index.values.push_back(T.x()); - t_q_index.values.push_back(T.y()); - t_q_index.values.push_back(T.z()); - t_q_index.values.push_back(Q.w()); - t_q_index.values.push_back(Q.x()); - t_q_index.values.push_back(Q.y()); - t_q_index.values.push_back(Q.z()); - t_q_index.values.push_back(cur_kf->index); - msg_match_points.channels.push_back(t_q_index); - pub_match_points.publish(msg_match_points); - } -} - -void image_callback(const sensor_msgs::ImageConstPtr& image_msg) { - // ROS_INFO("image_callback!"); - if (!LOOP_CLOSURE) return; - m_buf.lock(); - image_buf.push(image_msg); - m_buf.unlock(); - // printf(" image time %f \n", image_msg->header.stamp.toSec()); - - // detect unstable camera stream - if (last_image_time == -1) - last_image_time = image_msg->header.stamp.toSec(); - else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || - image_msg->header.stamp.toSec() < last_image_time) { - ROS_WARN("image discontinue! detect a new sequence!"); - new_sequence(); - } - last_image_time = image_msg->header.stamp.toSec(); -} - -void point_callback(const sensor_msgs::PointCloudConstPtr& point_msg) { - // ROS_INFO("point_callback!"); - if (!LOOP_CLOSURE) return; - m_buf.lock(); - point_buf.push(point_msg); - m_buf.unlock(); - /* - for (unsigned int i = 0; i < point_msg->points.size(); i++) - { - printf("%d, 3D point: %f, %f, %f 2D point %f, %f \n",i , - point_msg->points[i].x, point_msg->points[i].y, point_msg->points[i].z, - point_msg->channels[i].values[0], - point_msg->channels[i].values[1]); - } - */ -} - -void pose_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - // ROS_INFO("pose_callback!"); - if (!LOOP_CLOSURE) return; - m_buf.lock(); - pose_buf.push(pose_msg); - m_buf.unlock(); - /* - printf("pose t: %f, %f, %f q: %f, %f, %f %f \n", - pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z, - pose_msg->pose.pose.orientation.w, - pose_msg->pose.pose.orientation.x, - pose_msg->pose.pose.orientation.y, - pose_msg->pose.pose.orientation.z); - */ -} - -void imu_forward_callback(const nav_msgs::Odometry::ConstPtr& forward_msg) { - if (VISUALIZE_IMU_FORWARD) { - Vector3d vio_t(forward_msg->pose.pose.position.x, - forward_msg->pose.pose.position.y, - forward_msg->pose.pose.position.z); - Quaterniond vio_q; - vio_q.w() = forward_msg->pose.pose.orientation.w; - vio_q.x() = forward_msg->pose.pose.orientation.x; - vio_q.y() = forward_msg->pose.pose.orientation.y; - vio_q.z() = forward_msg->pose.pose.orientation.z; - - vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; - vio_q = posegraph.w_r_vio * vio_q; - - vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; - vio_q = posegraph.r_drift * vio_q; - - Vector3d vio_t_cam; - Quaterniond vio_q_cam; - vio_t_cam = vio_t + vio_q * tic; - vio_q_cam = vio_q * qic; - - cameraposevisual.reset(); - cameraposevisual.add_pose(vio_t_cam, vio_q_cam); - cameraposevisual.publish_by(pub_camera_pose_visual, forward_msg->header); - } -} -void relo_relative_pose_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - Vector3d relative_t = - Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - Quaterniond relative_q; - relative_q.w() = pose_msg->pose.pose.orientation.w; - relative_q.x() = pose_msg->pose.pose.orientation.x; - relative_q.y() = pose_msg->pose.pose.orientation.y; - relative_q.z() = pose_msg->pose.pose.orientation.z; - double relative_yaw = pose_msg->twist.twist.linear.x; - int index = pose_msg->twist.twist.linear.y; - // printf("receive index %d \n", index ); - Eigen::Matrix loop_info; - loop_info << relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(), - relative_q.x(), relative_q.y(), relative_q.z(), relative_yaw; - posegraph.updateKeyFrameLoop(index, loop_info); -} - -void vio_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - // ROS_INFO("vio_callback!"); - Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - Quaterniond vio_q; - vio_q.w() = pose_msg->pose.pose.orientation.w; - vio_q.x() = pose_msg->pose.pose.orientation.x; - vio_q.y() = pose_msg->pose.pose.orientation.y; - vio_q.z() = pose_msg->pose.pose.orientation.z; - - vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; - vio_q = posegraph.w_r_vio * vio_q; - - vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; - vio_q = posegraph.r_drift * vio_q; - - Vector3d vio_t_cam; - Quaterniond vio_q_cam; - vio_t_cam = vio_t + vio_q * tic; - vio_q_cam = vio_q * qic; - - if (!VISUALIZE_IMU_FORWARD) { - cameraposevisual.reset(); - cameraposevisual.add_pose(vio_t_cam, vio_q_cam); - cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header); - } - - odometry_buf.push(vio_t_cam); - if (odometry_buf.size() > 10) { - odometry_buf.pop(); - } - - visualization_msgs::Marker key_odometrys; - key_odometrys.header = pose_msg->header; - key_odometrys.header.frame_id = "world"; - key_odometrys.ns = "key_odometrys"; - key_odometrys.type = visualization_msgs::Marker::SPHERE_LIST; - key_odometrys.action = visualization_msgs::Marker::ADD; - key_odometrys.pose.orientation.w = 1.0; - key_odometrys.lifetime = ros::Duration(); - - // static int key_odometrys_id = 0; - key_odometrys.id = 0; // key_odometrys_id++; - key_odometrys.scale.x = 0.1; - key_odometrys.scale.y = 0.1; - key_odometrys.scale.z = 0.1; - key_odometrys.color.r = 1.0; - key_odometrys.color.a = 1.0; - - for (unsigned int i = 0; i < odometry_buf.size(); i++) { - geometry_msgs::Point pose_marker; - Vector3d vio_t; - vio_t = odometry_buf.front(); - odometry_buf.pop(); - pose_marker.x = vio_t.x(); - pose_marker.y = vio_t.y(); - pose_marker.z = vio_t.z(); - key_odometrys.points.push_back(pose_marker); - odometry_buf.push(vio_t); - } - pub_key_odometrys.publish(key_odometrys); - - if (!LOOP_CLOSURE) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header = pose_msg->header; - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = vio_t.x(); - pose_stamped.pose.position.y = vio_t.y(); - pose_stamped.pose.position.z = vio_t.z(); - no_loop_path.header = pose_msg->header; - no_loop_path.header.frame_id = "world"; - no_loop_path.poses.push_back(pose_stamped); - pub_vio_path.publish(no_loop_path); - } -} - -void extrinsic_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - m_process.lock(); - tic = Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - qic = Quaterniond(pose_msg->pose.pose.orientation.w, - pose_msg->pose.pose.orientation.x, - pose_msg->pose.pose.orientation.y, - pose_msg->pose.pose.orientation.z) - .toRotationMatrix(); - m_process.unlock(); -} - -void process() { - if (!LOOP_CLOSURE) return; - while (true) { - sensor_msgs::ImageConstPtr image_msg = NULL; - sensor_msgs::PointCloudConstPtr point_msg = NULL; - nav_msgs::Odometry::ConstPtr pose_msg = NULL; - - // find out the messages with same time stamp - m_buf.lock(); - if (!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) { - if (image_buf.front()->header.stamp.toSec() > - pose_buf.front()->header.stamp.toSec()) { - pose_buf.pop(); - printf("throw pose at beginning\n"); - } else if (image_buf.front()->header.stamp.toSec() > - point_buf.front()->header.stamp.toSec()) { - point_buf.pop(); - printf("throw point at beginning\n"); - } else if (image_buf.back()->header.stamp.toSec() >= - pose_buf.front()->header.stamp.toSec() && - point_buf.back()->header.stamp.toSec() >= - pose_buf.front()->header.stamp.toSec()) { - pose_msg = pose_buf.front(); - pose_buf.pop(); - while (!pose_buf.empty()) pose_buf.pop(); - while (image_buf.front()->header.stamp.toSec() < - pose_msg->header.stamp.toSec()) - image_buf.pop(); - image_msg = image_buf.front(); - image_buf.pop(); - - while (point_buf.front()->header.stamp.toSec() < - pose_msg->header.stamp.toSec()) - point_buf.pop(); - point_msg = point_buf.front(); - point_buf.pop(); - } - } - m_buf.unlock(); - - if (pose_msg != NULL) { - // printf(" pose time %f \n", pose_msg->header.stamp.toSec()); - // printf(" point time %f \n", point_msg->header.stamp.toSec()); - // printf(" image time %f \n", image_msg->header.stamp.toSec()); - // skip fisrt few - if (skip_first_cnt < SKIP_FIRST_CNT) { - skip_first_cnt++; - continue; - } - - if (skip_cnt < SKIP_CNT) { - skip_cnt++; - continue; - } else { - skip_cnt = 0; - } - - cv_bridge::CvImageConstPtr ptr; - if (image_msg->encoding == "8UC1") { - sensor_msgs::Image img; - img.header = image_msg->header; - img.height = image_msg->height; - img.width = image_msg->width; - img.is_bigendian = image_msg->is_bigendian; - img.step = image_msg->step; - img.data = image_msg->data; - img.encoding = "mono8"; - ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); - } else - ptr = - cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8); - - cv::Mat image = ptr->image; - // build keyframe - Vector3d T = Vector3d(pose_msg->pose.pose.position.x, - pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w, - pose_msg->pose.pose.orientation.x, - pose_msg->pose.pose.orientation.y, - pose_msg->pose.pose.orientation.z) - .toRotationMatrix(); - if ((T - last_t).norm() > SKIP_DIS) { - vector point_3d; - vector point_2d_uv; - vector point_2d_normal; - vector point_id; - - for (unsigned int i = 0; i < point_msg->points.size(); i++) { - cv::Point3f p_3d; - p_3d.x = point_msg->points[i].x; - p_3d.y = point_msg->points[i].y; - p_3d.z = point_msg->points[i].z; - point_3d.push_back(p_3d); - - cv::Point2f p_2d_uv, p_2d_normal; - double p_id; - p_2d_normal.x = point_msg->channels[i].values[0]; - p_2d_normal.y = point_msg->channels[i].values[1]; - p_2d_uv.x = point_msg->channels[i].values[2]; - p_2d_uv.y = point_msg->channels[i].values[3]; - p_id = point_msg->channels[i].values[4]; - point_2d_normal.push_back(p_2d_normal); - point_2d_uv.push_back(p_2d_uv); - point_id.push_back(p_id); - - // printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y); - } - - KeyFrame* keyframe = new KeyFrame( - pose_msg->header.stamp.toSec(), frame_index, T, R, image, point_3d, - point_2d_uv, point_2d_normal, point_id, sequence); - m_process.lock(); - start_flag = 1; - posegraph.addKeyFrame(keyframe, 1); - { - int sequence_cnt = posegraph.getCurrentSequenceCount(); - Vector3d P; - Matrix3d R; - keyframe->getPose(P, R); - Quaterniond Q{R}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(keyframe->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; - pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; - pose_stamped.pose.position.z = P.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - path[sequence_cnt].poses.push_back(pose_stamped); - path[sequence_cnt].header = pose_stamped.header; - - if (SAVE_LOOP_PATH) { - ofstream loop_path_file(VINS_RESULT_PATH, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << keyframe->time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << P.x() << "," << P.y() << "," << P.z() << "," - << Q.w() << "," << Q.x() << "," << Q.y() << "," - << Q.z() << "," << endl; - loop_path_file.close(); - } - } - publish(); - m_process.unlock(); - frame_index++; - last_t = T; - } - } - - std::chrono::milliseconds dura(5); - std::this_thread::sleep_for(dura); - } -} - -void command() { - if (!LOOP_CLOSURE) return; - while (1) { - char c = getchar(); - if (c == 's') { - m_process.lock(); - posegraph.savePoseGraph(); - m_process.unlock(); - printf( - "save pose graph finish\nyou can set 'load_previous_pose_graph' to 1 " - "in the config file to reuse it next time\n"); - // printf("program shutting down...\n"); - // ros::shutdown(); - } - if (c == 'n') new_sequence(); - - std::chrono::milliseconds dura(5); - std::this_thread::sleep_for(dura); - } -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "pose_graph_ros"); - ros::NodeHandle n("~"); - // posegraph.registerPub(n); - - // OnPoseGraphLoaded - posegraph.setOnKeyFrameLoadedCallback(on_keyframe_loaded_callback); - - // OnPoseGraphOptimization - posegraph.setOnOptimizationStepCompletedCallback( - on_optimization_step_completed_callback); - - // OnKeyFrameAdded - posegraph.setOnNewEdgeCallback(on_new_edge_callback); - posegraph.setOnNewLoopEdgeCallback(on_new_loopedge_callback); - posegraph.setOnKeyFrameConnectionFoundCallback( - on_keyframe_connection_found_callback); - - posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0); - posegraph_visualization->setScale(0.1); - posegraph_visualization->setLineWidth(0.01); - - // read param - n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X); - n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y); - n.getParam("skip_cnt", SKIP_CNT); - n.getParam("skip_dis", SKIP_DIS); - std::string config_file; - n.getParam("config_file", config_file); - cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); - if (!fsSettings.isOpened()) { - std::cerr << "ERROR: Wrong path to settings" << std::endl; - } - - double camera_visual_size = fsSettings["visualize_camera_size"]; - cameraposevisual.setScale(camera_visual_size); - cameraposevisual.setLineWidth(camera_visual_size / 10.0); - - LOOP_CLOSURE = fsSettings["loop_closure"]; - std::string IMAGE_TOPIC; - int LOAD_PREVIOUS_POSE_GRAPH; - if (LOOP_CLOSURE) { - ROW = fsSettings["image_height"]; - COL = fsSettings["image_width"]; - std::string pkg_path = ros::package::getPath("pose_graph_ros"); - string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin"; - cout << "vocabulary_file" << vocabulary_file << endl; - posegraph.loadVocabulary(vocabulary_file); - - BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml"; - cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl; - m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile( - config_file.c_str()); - - fsSettings["image_topic"] >> IMAGE_TOPIC; - fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH; - fsSettings["output_path"] >> VINS_RESULT_PATH; - fsSettings["save_image"] >> DEBUG_IMAGE; - - // create folder if not exists - FileSystemHelper::createDirectoryIfNotExists(POSE_GRAPH_SAVE_PATH.c_str()); - FileSystemHelper::createDirectoryIfNotExists(VINS_RESULT_PATH.c_str()); - - VISUALIZE_IMU_FORWARD = fsSettings["visualize_imu_forward"]; - LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"]; - FAST_RELOCALIZATION = fsSettings["fast_relocalization"]; - VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv"; - std::ofstream fout(VINS_RESULT_PATH, std::ios::out); - fout.close(); - fsSettings.release(); - - if (LOAD_PREVIOUS_POSE_GRAPH) { - printf("load pose graph\n"); - m_process.lock(); - posegraph.loadPoseGraph(); - m_process.unlock(); - printf("load pose graph finish\n"); - load_flag = 1; - } else { - printf("no previous pose graph\n"); - load_flag = 1; - } - } - - fsSettings.release(); - - ros::Subscriber sub_imu_forward = - n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback); - ros::Subscriber sub_vio = - n.subscribe("/vins_estimator/odometry", 2000, vio_callback); - ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback); - ros::Subscriber sub_pose = - n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback); - ros::Subscriber sub_extrinsic = - n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback); - ros::Subscriber sub_point = - n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback); - ros::Subscriber sub_relo_relative_pose = n.subscribe( - "/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback); - - pub_match_img = n.advertise("match_image", 1000); - pub_camera_pose_visual = - n.advertise("camera_pose_visual", 1000); - pub_key_odometrys = - n.advertise("key_odometrys", 1000); - pub_vio_path = n.advertise("no_loop_path", 1000); - pub_match_points = n.advertise("match_points", 100); - pub_pg_path = n.advertise("pose_graph_path", 1000); - pub_base_path = n.advertise("base_path", 1000); - pub_pose_graph = - n.advertise("pose_graph", 1000); - for (int i = 1; i < 10; i++) - pub_path[i] = n.advertise("path_" + to_string(i), 1000); - - std::thread measurement_process; - std::thread keyboard_command_process; - - measurement_process = std::thread(process); - keyboard_command_process = std::thread(command); - - ros::spin(); - - return 0; -} diff --git a/pose_graph_ros/src/pose_graph_event_observer_impl.cpp b/pose_graph_ros/src/pose_graph_event_observer_impl.cpp new file mode 100644 index 000000000..2b17cd1d3 --- /dev/null +++ b/pose_graph_ros/src/pose_graph_event_observer_impl.cpp @@ -0,0 +1,303 @@ +/** + * @file pose_graph_event_observer_impl.cpp + * @brief + * @date 09-03-2025 + * + * @copyright Copyright (c) 2025 Cheo Kee Jin. + */ + +#include "pose_graph_event_observer_impl.hpp" + +#include + +#include +#include +#include + +#define SHOW_S_EDGE false +#define SHOW_L_EDGE true + +namespace pose_graph { +PoseGraphNode::PoseGraphEventObserverImpl::PoseGraphEventObserverImpl( + ros::NodeHandle& nh, const std::string config_file_path) + : nh_(nh) { + posegraph_visualization_ = + std::make_unique(1.0, 0.0, 1.0, 1.0); + posegraph_visualization_->setScale(0.1); + posegraph_visualization_->setLineWidth(0.01); + + // Load config file + cv::FileStorage fs(config_file_path, cv::FileStorage::READ); + if (!fs.isOpened()) { + ROS_ERROR("PoseGraphEventObserver::Failed to open config file: %s", + config_file_path.c_str()); + ros::shutdown(); + } + + fast_relocalization_ = (int)fs["fast_relocalization"]; + + vins_result_path_ = std::string(fs["output_path"]); + FileSystemHelper::createDirectoryIfNotExists(vins_result_path_.c_str()); + vins_result_path_ = vins_result_path_ + "/vins_result_loop.csv"; + + fs.release(); + + if (!ReadParameters()) { + ROS_ERROR("PoseGraphEventObserver::Failed to read parameters"); + ros::shutdown(); + } + + StartPublishers(); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::Publish(int sequence_count) { + for (int i = 1; i <= sequence_count; i++) { + pub_pg_path_.publish(path_[i]); + pub_path_[i].publish(path_[i]); + posegraph_visualization_->publish_by(pub_pose_graph_, + path_[sequence_count].header); + } + base_path_.header.frame_id = "world"; + pub_base_path_.publish(base_path_); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::ResetPoseGraphVisualisation() { + posegraph_visualization_->reset(); +} + +bool PoseGraphNode::PoseGraphEventObserverImpl::ReadParameters() { + nh_.getParam("visualization_shift_x", visualization_shift_x_); + nh_.getParam("visualization_shift_y", visualization_shift_y_); + + ROS_INFO( + "Loaded parameters: visualization_shift_x: %d, " + "visualization_shift_y: %d", + visualization_shift_x_, visualization_shift_y_); + + return true; +} + +void PoseGraphNode::PoseGraphEventObserverImpl::StartPublishers() { + pub_match_img_ = nh_.advertise("match_image", 2000); + pub_match_points_ = + nh_.advertise("match_points", 100); + pub_pg_path_ = nh_.advertise("pose_graph_path", 1000); + pub_base_path_ = nh_.advertise("base_path", 1000); + pub_pose_graph_ = + nh_.advertise("pose_graph", 1000); + for (int i = 1; i < 10; i++) + pub_path_[i] = nh_.advertise("path_" + to_string(i), 1000); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnPoseGraphLoaded() { + ROS_DEBUG("Pose graph loaded"); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnPoseGraphSaved() { + ROS_DEBUG("Pose graph saved"); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnKeyFrameAdded( + KeyFrame::Attributes kf_attribute, int sequence_count) { + ROS_DEBUG("On Keyframe added"); + + Eigen::Quaterniond quarternion{kf_attribute.rotation}; + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(kf_attribute.time_stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose.position.x = + kf_attribute.position.x() + visualization_shift_x_; + pose_stamped.pose.position.y = + kf_attribute.position.y() + visualization_shift_y_; + pose_stamped.pose.position.z = kf_attribute.position.z(); + pose_stamped.pose.orientation.x = quarternion.x(); + pose_stamped.pose.orientation.y = quarternion.y(); + pose_stamped.pose.orientation.z = quarternion.z(); + pose_stamped.pose.orientation.w = quarternion.w(); + path_[sequence_count].poses.push_back(pose_stamped); + path_[sequence_count].header = pose_stamped.header; + + if (save_loop_path) { + std::ofstream loop_path_file(vins_result_path_, ios::app); + loop_path_file.setf(ios::fixed, ios::floatfield); + loop_path_file.precision(0); + loop_path_file << kf_attribute.time_stamp * 1e9 << ","; + loop_path_file.precision(5); + loop_path_file << kf_attribute.position.x() << "," + << kf_attribute.position.y() << "," + << kf_attribute.position.z() << "," << quarternion.w() << "," + << quarternion.x() << "," << quarternion.y() << "," + << quarternion.z() << "," << endl; + loop_path_file.close(); + } + + Publish(sequence_count); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnKeyFrameLoaded( + KeyFrame::Attributes kf_attribute, int count, int sequence_count) { + ROS_INFO("On Keyframe loaded"); + Eigen::Quaterniond Q{kf_attribute.rotation}; + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(kf_attribute.time_stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose.position.x = + kf_attribute.position.x() + visualization_shift_x_; + pose_stamped.pose.position.y = + kf_attribute.position.y() + visualization_shift_y_; + pose_stamped.pose.position.z = kf_attribute.position.z(); + pose_stamped.pose.orientation.x = Q.x(); + pose_stamped.pose.orientation.y = Q.y(); + pose_stamped.pose.orientation.z = Q.z(); + pose_stamped.pose.orientation.w = Q.w(); + base_path_.poses.push_back(pose_stamped); + base_path_.header = pose_stamped.header; + + if (count % 20 == 0) { + Publish(sequence_count); + } +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnKeyFrameConnectionFound( + KeyFrame::Attributes current_kf_attribute, + KeyFrame::Attributes old_kf_attribute, + std::vector matched_2d_old_norm, + std::vector matched_id, cv::Mat& thumb_image) { + ROS_DEBUG("On Keyframe connection found"); + { + sensor_msgs::ImagePtr msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumb_image) + .toImageMsg(); + msg->header.stamp = ros::Time(current_kf_attribute.time_stamp); + pub_match_img_.publish(msg); + } + if (fast_relocalization_) { + sensor_msgs::PointCloud msg_match_points; + msg_match_points.header.stamp = ros::Time(current_kf_attribute.time_stamp); + for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { + geometry_msgs::Point32 p; + p.x = matched_2d_old_norm[i].x; + p.y = matched_2d_old_norm[i].y; + p.z = matched_id[i]; + msg_match_points.points.push_back(p); + } + Eigen::Vector3d T = old_kf_attribute.position; + Eigen::Matrix3d R = old_kf_attribute.rotation; + Quaterniond Q(R); + sensor_msgs::ChannelFloat32 t_q_index; + t_q_index.values.push_back(T.x()); + t_q_index.values.push_back(T.y()); + t_q_index.values.push_back(T.z()); + t_q_index.values.push_back(Q.w()); + t_q_index.values.push_back(Q.x()); + t_q_index.values.push_back(Q.y()); + t_q_index.values.push_back(Q.z()); + t_q_index.values.push_back(current_kf_attribute.index); + msg_match_points.channels.push_back(t_q_index); + pub_match_points_.publish(msg_match_points); + } +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnPoseGraphOptimization( + std::vector kf_attributes, int sequence_count) { + // ROS_INFO("On Pose graph optimization"); + std::vector::iterator it; + for (int i = 1; i <= sequence_count; i++) { + path_[i].poses.clear(); + } + base_path_.poses.clear(); + posegraph_visualization_->reset(); + + if (save_loop_path) { + std::ofstream loop_path_file_tmp(vins_result_path_, ios::out); + loop_path_file_tmp.close(); + } + + for (it = kf_attributes.begin(); it != kf_attributes.end(); it++) { + Eigen::Quaterniond Q; + Q = it->rotation; + // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); + + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(it->time_stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose.position.x = it->position.x() + visualization_shift_x_; + pose_stamped.pose.position.y = it->position.y() + visualization_shift_y_; + pose_stamped.pose.position.z = it->position.z(); + pose_stamped.pose.orientation.x = Q.x(); + pose_stamped.pose.orientation.y = Q.y(); + pose_stamped.pose.orientation.z = Q.z(); + pose_stamped.pose.orientation.w = Q.w(); + if (it->sequence == 0) { + base_path_.poses.push_back(pose_stamped); + base_path_.header = pose_stamped.header; + } else { + path_[it->sequence].poses.push_back(pose_stamped); + path_[it->sequence].header = pose_stamped.header; + } + + if (save_loop_path && !vins_result_path_.empty()) { + std::ofstream loop_path_file(vins_result_path_, ios::app); + loop_path_file.setf(ios::fixed, ios::floatfield); + loop_path_file.precision(0); + loop_path_file << it->time_stamp * 1e9 << ","; + loop_path_file.precision(5); + loop_path_file << it->position.x() << "," << it->position.y() << "," + << it->position.z() << "," << Q.w() << "," << Q.x() << "," + << Q.y() << "," << Q.z() << "," << endl; + loop_path_file.close(); + } + + if (SHOW_S_EDGE) { + std::vector::reverse_iterator rit = + kf_attributes.rbegin(); + std::vector::reverse_iterator lrit; + for (; rit != kf_attributes.rend(); rit++) { + if (rit->index == it->index) { + lrit = rit; + lrit++; + for (int i = 0; i < 4; i++) { + if (lrit == kf_attributes.rend()) break; + if (lrit->sequence == it->sequence) { + posegraph_visualization_->add_edge(it->position, lrit->position); + } + lrit++; + } + break; + } + } + } + if (SHOW_L_EDGE) { + if (it->has_loop && it->sequence == sequence_count) { + std::find_if( + kf_attributes.begin(), kf_attributes.end(), + [&](KeyFrame::Attributes& attr) { + if (attr.index == it->loop_index && it->sequence > 0) { + posegraph_visualization_->add_loopedge( + it->position, + attr.position + Vector3d(visualization_shift_x_, + visualization_shift_y_, 0)); + return true; + } + return false; + }); + } + } + } + + Publish(sequence_count); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnNewSequentialEdge( + Vector3d p1, Vector3d p2) { + if (!SHOW_S_EDGE) return; + posegraph_visualization_->add_edge(p1, p2); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnNewLoopEdge(Vector3d p1, + Vector3d p2) { + if (!SHOW_L_EDGE) return; + p2 += Vector3d(visualization_shift_x_, visualization_shift_y_, 0); + posegraph_visualization_->add_loopedge(p1, p2); +} +} // namespace pose_graph \ No newline at end of file diff --git a/pose_graph_ros/src/pose_graph_event_observer_impl.hpp b/pose_graph_ros/src/pose_graph_event_observer_impl.hpp new file mode 100644 index 000000000..d5c4bb671 --- /dev/null +++ b/pose_graph_ros/src/pose_graph_event_observer_impl.hpp @@ -0,0 +1,76 @@ +/** + * @file pose_graph_event_observer_impl.cpp + * @brief + * @date 09-03-2025 + * + * @copyright Copyright (c) 2025 Cheo Kee Jin. + */ + +#ifndef SRC_POSE_GRAPH_EVENT_OBSERVER_IMPL_HPP +#define SRC_POSE_GRAPH_EVENT_OBSERVER_IMPL_HPP + +#include + +#include + +#include +#include + +#include "pose_graph/details/pose_graph_event_observer.hpp" + +#include "pose_graph_ros/pose_graph_node.hpp" +#include "pose_graph_ros/utility/CameraPoseVisualization.h" + +namespace pose_graph { +class PoseGraphNode::PoseGraphEventObserverImpl + : public PoseGraphEventObserver { + public: + PoseGraphEventObserverImpl(ros::NodeHandle& nh, std::string config_file_path); + ~PoseGraphEventObserverImpl() = default; + + void Publish(int sequence_count); + void ResetPoseGraphVisualisation(); + + private: + bool ReadParameters(); + void StartPublishers(); + + // PoseGraphEventObserver callbacks + void OnPoseGraphLoaded() final; + void OnPoseGraphSaved() final; + void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute, + int sequence_count) final; + void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, int count, + int sequence_count) final; + void OnKeyFrameConnectionFound(KeyFrame::Attributes current_kf_attribute, + KeyFrame::Attributes old_kf_attribute, + std::vector matched_2d_old_norm, + std::vector matched_id, + cv::Mat& thumb_image) final; + void OnPoseGraphOptimization(std::vector kf_attributes, + int sequence_count) final; + void OnNewSequentialEdge(Vector3d p1, Vector3d p2) final; + void OnNewLoopEdge(Vector3d p1, Vector3d p2) final; + + private: + ros::NodeHandle& nh_; + nav_msgs::Path path_[10]; + nav_msgs::Path base_path_; + std::string vins_result_path_; + bool fast_relocalization_{false}; + const bool save_loop_path = true; + int visualization_shift_x_ = 0; + int visualization_shift_y_ = 0; + + // ros publishers + ros::Publisher pub_match_points_; + ros::Publisher pub_pg_path_; + ros::Publisher pub_base_path_; + ros::Publisher pub_pose_graph_; + ros::Publisher pub_path_[10]; + ros::Publisher pub_match_img_; + std::unique_ptr posegraph_visualization_; +}; +} // namespace pose_graph + +#endif /* SRC_POSE_GRAPH_EVENT_OBSERVER_IMPL_HPP */ diff --git a/pose_graph_ros/src/pose_graph_node.cpp b/pose_graph_ros/src/pose_graph_node.cpp index 1cd529df1..5a0b09bfc 100644 --- a/pose_graph_ros/src/pose_graph_node.cpp +++ b/pose_graph_ros/src/pose_graph_node.cpp @@ -14,13 +14,11 @@ #include #include -#define SHOW_S_EDGE false -#define SHOW_L_EDGE true +#include "pose_graph_event_observer_impl.hpp" namespace pose_graph { PoseGraphNode::~PoseGraphNode() { - ros::shutdown(); if (measurement_thread_.joinable()) { measurement_thread_.join(); } @@ -54,11 +52,6 @@ bool PoseGraphNode::Start() { camera_pose_vis_->setScale(camera_visual_size); camera_pose_vis_->setLineWidth(camera_visual_size / 10.0); - posegraph_visualization_ = - std::make_unique(1.0, 0.0, 1.0, 1.0); - posegraph_visualization_->setScale(0.1); - posegraph_visualization_->setLineWidth(0.01); - loop_closure_ = (int)fs["loop_closure"]; bool load_previous_pose_graph = false; @@ -75,19 +68,13 @@ bool PoseGraphNode::Start() { config_file_path_.c_str()); image_topic_ = std::string(fs["image_topic"]); config_.saved_pose_graph_dir = std::string(fs["pose_graph_save_path"]); - config_.save_debug_image = (int)fs["save_image"]; - - vins_result_path_ = std::string(fs["output_path"]); FileSystemHelper::createDirectoryIfNotExists( config_.saved_pose_graph_dir.c_str()); - FileSystemHelper::createDirectoryIfNotExists(vins_result_path_.c_str()); - vins_result_path_ = vins_result_path_ + "/vins_result_loop.csv"; + config_.save_debug_image = (int)fs["save_image"]; + config_.fast_relocalization = (int)fs["fast_relocalization"]; visualise_imu_forward_ = (int)fs["visualise_imu_forward"]; load_previous_pose_graph = (int)fs["load_previous_pose_graph"]; - config_.fast_relocalization = (int)fs["fast_relocalization"]; - - std::ofstream fout(vins_result_path_, std::ios::out); if (load_previous_pose_graph) { ROS_INFO("Loading previous pose graph"); @@ -102,8 +89,10 @@ bool PoseGraphNode::Start() { StartPublishersAndSubscribers(); + pose_graph_event_observer_ = + std::make_shared(nh_, config_file_path_); pose_graph_.Initialize(config_, camera_); - pose_graph_.RegisterEventObserver(PoseGraphEventObserver::shared_from_this()); + pose_graph_.RegisterEventObserver(pose_graph_event_observer_); StartCommandAndProcessingThreads(); return true; @@ -115,21 +104,23 @@ bool PoseGraphNode::ReadParameters() { return false; } - nh_.getParam("visualization_shift_x", visualization_shift_x_); - nh_.getParam("visualization_shift_y", visualization_shift_y_); nh_.getParam("skip_cnt", skip_cnt_threshold_); nh_.getParam("skip_dis", skip_distance_); ROS_INFO( - "Loaded parameters: config_file: %s, visualization_shift_x: %d, " - "visualization_shift_y: %d, skip_cnt: %d, skip_dis: %f", - config_file_path_.c_str(), visualization_shift_x_, visualization_shift_y_, - skip_cnt_threshold_, skip_distance_); + "Loaded parameters for PoseGraphNode: config_file: %s, skip_cnt: %d, " + "skip_dis: %f", + config_file_path_.c_str(), skip_cnt_threshold_, skip_distance_); return true; } void PoseGraphNode::StartPublishersAndSubscribers() { + pub_camera_pose_visual_ = nh_.advertise( + "camera_pose_visual", 1000); + pub_key_odometrys_ = + nh_.advertise("key_odometrys", 1000); + pub_vio_path_ = nh_.advertise("no_loop_path", 1000); sub_imu_forward_ = nh_.subscribe( "/vins_estimator/imu_propagate", 2000, &PoseGraphNode::ImuForwardCallback, this); @@ -149,21 +140,6 @@ void PoseGraphNode::StartPublishersAndSubscribers() { sub_relo_relative_pose_ = nh_.subscribe( "/vins_estimator/relo_relative_pose", 2000, &PoseGraphNode::ReloRelativePoseCallback, this); - - pub_match_img_ = nh_.advertise("match_image", 2000); - pub_camera_pose_visual_ = nh_.advertise( - "camera_pose_visual", 1000); - pub_key_odometrys_ = - nh_.advertise("key_odometrys", 1000); - pub_vio_path_ = nh_.advertise("no_loop_path", 1000); - pub_match_points_ = - nh_.advertise("match_points", 100); - pub_pg_path_ = nh_.advertise("pose_graph_path", 1000); - pub_base_path_ = nh_.advertise("base_path", 1000); - pub_pose_graph_ = - nh_.advertise("pose_graph", 1000); - for (int i = 1; i < 10; i++) - pub_path_[i] = nh_.advertise("path_" + to_string(i), 1000); } void PoseGraphNode::StartCommandAndProcessingThreads() { @@ -294,41 +270,6 @@ void PoseGraphNode::Process() { { std::lock_guard lock(process_mutex_); pose_graph_.AddKeyFrame(keyframe); - { - int sequence_cnt = pose_graph_.GetCurrentSequenceCount(); - auto kf_attribute = keyframe->getAttributes(); - Eigen::Quaterniond quarternion{kf_attribute.rotation}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(keyframe->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = - kf_attribute.position.x() + visualization_shift_x_; - pose_stamped.pose.position.y = - kf_attribute.position.y() + visualization_shift_y_; - pose_stamped.pose.position.z = kf_attribute.position.z(); - pose_stamped.pose.orientation.x = quarternion.x(); - pose_stamped.pose.orientation.y = quarternion.y(); - pose_stamped.pose.orientation.z = quarternion.z(); - pose_stamped.pose.orientation.w = quarternion.w(); - path_[sequence_cnt].poses.push_back(pose_stamped); - path_[sequence_cnt].header = pose_stamped.header; - - if (save_loop_path) { - std::ofstream loop_path_file(vins_result_path_, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << kf_attribute.time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << kf_attribute.position.x() << "," - << kf_attribute.position.y() << "," - << kf_attribute.position.z() << "," - << quarternion.w() << "," << quarternion.x() << "," - << quarternion.y() << "," << quarternion.z() << "," - << endl; - loop_path_file.close(); - } - } - Publish(); } frame_index_++; last_t_ = T; @@ -342,6 +283,8 @@ void PoseGraphNode::Process() { void PoseGraphNode::Command() { if (!loop_closure_) return; while (ros::ok()) { + // Note: Since getchar() is a blocking call, a SIGINT signal will not be + // able to interrupt this thread. This is a known issue. char c = getchar(); if (c == 's') { { @@ -363,18 +306,6 @@ void PoseGraphNode::Command() { } } -void PoseGraphNode::Publish() { - int sequence_cnt = pose_graph_.GetCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - pub_pg_path_.publish(path_[i]); - pub_path_[i].publish(path_[i]); - posegraph_visualization_->publish_by(pub_pose_graph_, - path_[sequence_cnt].header); - } - base_path_.header.frame_id = "world"; - pub_base_path_.publish(base_path_); -} - void PoseGraphNode::NewSequence() { ROS_INFO("new sequence"); sequence_index_++; @@ -385,8 +316,8 @@ void PoseGraphNode::NewSequence() { "sequences."); ROS_BREAK(); } - posegraph_visualization_->reset(); - Publish(); + pose_graph_event_observer_->ResetPoseGraphVisualisation(); + pose_graph_event_observer_->Publish(pose_graph_.GetCurrentSequenceCount()); { std::lock_guard lock(buffer_mutex_); while (!image_buffer_.empty()) image_buffer_.pop(); @@ -396,180 +327,6 @@ void PoseGraphNode::NewSequence() { } } -void PoseGraphNode::OnPoseGraphLoaded() { ROS_DEBUG("Pose graph loaded"); } - -void PoseGraphNode::OnPoseGraphSaved() { ROS_DEBUG("Pose graph saved"); } - -void PoseGraphNode::OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) { - ROS_DEBUG("On Keyframe added"); -} - -void PoseGraphNode::OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, - int count) { - ROS_INFO("On Keyframe loaded"); - Eigen::Quaterniond Q{kf_attribute.rotation}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(kf_attribute.time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = - kf_attribute.position.x() + visualization_shift_x_; - pose_stamped.pose.position.y = - kf_attribute.position.y() + visualization_shift_y_; - pose_stamped.pose.position.z = kf_attribute.position.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - base_path_.poses.push_back(pose_stamped); - base_path_.header = pose_stamped.header; - - if (count % 20 == 0) { - Publish(); - } -} - -void PoseGraphNode::OnKeyFrameConnectionFound( - KeyFrame::Attributes current_kf_attribute, - KeyFrame::Attributes old_kf_attribute, - std::vector matched_2d_old_norm, - std::vector matched_id, cv::Mat& thumb_image) { - ROS_DEBUG("On Keyframe connection found"); - { - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumb_image) - .toImageMsg(); - msg->header.stamp = ros::Time(current_kf_attribute.time_stamp); - pub_match_img_.publish(msg); - } - if (config_.fast_relocalization) { - sensor_msgs::PointCloud msg_match_points; - msg_match_points.header.stamp = ros::Time(current_kf_attribute.time_stamp); - for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { - geometry_msgs::Point32 p; - p.x = matched_2d_old_norm[i].x; - p.y = matched_2d_old_norm[i].y; - p.z = matched_id[i]; - msg_match_points.points.push_back(p); - } - Eigen::Vector3d T = old_kf_attribute.position; - Eigen::Matrix3d R = old_kf_attribute.rotation; - Quaterniond Q(R); - sensor_msgs::ChannelFloat32 t_q_index; - t_q_index.values.push_back(T.x()); - t_q_index.values.push_back(T.y()); - t_q_index.values.push_back(T.z()); - t_q_index.values.push_back(Q.w()); - t_q_index.values.push_back(Q.x()); - t_q_index.values.push_back(Q.y()); - t_q_index.values.push_back(Q.z()); - t_q_index.values.push_back(current_kf_attribute.index); - msg_match_points.channels.push_back(t_q_index); - pub_match_points_.publish(msg_match_points); - } -} - -void PoseGraphNode::OnPoseGraphOptimization( - std::vector kf_attributes) { - // ROS_INFO("On Pose graph optimization"); - std::vector::iterator it; - int sequence_cnt = pose_graph_.GetCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - path_[i].poses.clear(); - } - base_path_.poses.clear(); - posegraph_visualization_->reset(); - - if (save_loop_path) { - std::ofstream loop_path_file_tmp(vins_result_path_, ios::out); - loop_path_file_tmp.close(); - } - - for (it = kf_attributes.begin(); it != kf_attributes.end(); it++) { - Eigen::Quaterniond Q; - Q = it->rotation; - // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); - - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(it->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = it->position.x() + visualization_shift_x_; - pose_stamped.pose.position.y = it->position.y() + visualization_shift_y_; - pose_stamped.pose.position.z = it->position.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - if (it->sequence == 0) { - base_path_.poses.push_back(pose_stamped); - base_path_.header = pose_stamped.header; - } else { - path_[it->sequence].poses.push_back(pose_stamped); - path_[it->sequence].header = pose_stamped.header; - } - - if (save_loop_path && !vins_result_path_.empty()) { - std::ofstream loop_path_file(vins_result_path_, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << it->time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << it->position.x() << "," << it->position.y() << "," - << it->position.z() << "," << Q.w() << "," << Q.x() << "," - << Q.y() << "," << Q.z() << "," << endl; - loop_path_file.close(); - } - - if (SHOW_S_EDGE) { - std::vector::reverse_iterator rit = - kf_attributes.rbegin(); - std::vector::reverse_iterator lrit; - for (; rit != kf_attributes.rend(); rit++) { - if (rit->index == it->index) { - lrit = rit; - lrit++; - for (int i = 0; i < 4; i++) { - if (lrit == kf_attributes.rend()) break; - if (lrit->sequence == it->sequence) { - posegraph_visualization_->add_edge(it->position, lrit->position); - } - lrit++; - } - break; - } - } - } - if (SHOW_L_EDGE) { - if (it->has_loop && it->sequence == sequence_cnt) { - std::find_if( - kf_attributes.begin(), kf_attributes.end(), - [&](KeyFrame::Attributes& attr) { - if (attr.index == it->loop_index && it->sequence > 0) { - posegraph_visualization_->add_loopedge( - it->position, - attr.position + Vector3d(visualization_shift_x_, - visualization_shift_y_, 0)); - return true; - } - return false; - }); - } - } - } - - Publish(); -} - -void PoseGraphNode::OnNewSequentialEdge(Vector3d p1, Vector3d p2) { - if (!SHOW_S_EDGE) return; - posegraph_visualization_->add_edge(p1, p2); -} - -void PoseGraphNode::OnNewLoopEdge(Vector3d p1, Vector3d p2) { - if (!SHOW_L_EDGE) return; - p2 += Vector3d(visualization_shift_x_, visualization_shift_y_, 0); - posegraph_visualization_->add_loopedge(p1, p2); -} - void PoseGraphNode::ImuForwardCallback( const nav_msgs::OdometryConstPtr& forward_msg) { if (!visualise_imu_forward_) return; @@ -777,9 +534,8 @@ void PoseGraphNode::ReloRelativePoseCallback( int main(int argc, char** argv) { ros::init(argc, argv, "pose_graph_node"); - std::shared_ptr pose_graph_node = - std::make_shared(); - if (!pose_graph_node->Start()) { + pose_graph::PoseGraphNode pose_graph_node; + if (!pose_graph_node.Start()) { ROS_ERROR("Failed to start PoseGraphNode"); ros::shutdown(); }