git update

2026-06-08 16:35:07 +08:00
parent db6686fccb
commit 8b775db45f
59 changed files with 4901 additions and 1 deletions
--- a/src/ground_slam/include/map_builder.h
+++ b/src/ground_slam/include/map_builder.h
@@ -0,0 +1,83 @@
+#ifndef MAP_BUILDER_H_
+#define MAP_BUILDER_H_
+
+#include "read_configs.h"
+#include "camera.h"
+#include "frame.h"
+#include "correlation_flow.h"
+#include "map.h"
+#include "loop_closure.h"
+#include "map_stitcher.h"
+
+
+class MapBuilder{
+public:
+  MapBuilder(Configs& configs);
+
+  bool AddNewInput(cv::Mat& image, double timestamp = -1);
+  void ComputeFFTResult(cv::Mat& image);
+  void ConstructFrame(double timestamp);
+  void SetCurrentFramePose();
+  bool Initialize();
+  void UpdateIntermedium();
+  void UpdateCurrentPose();
+  bool Tracking(Eigen::Vector3d& response);
+  void AddCFEdge();
+  void AddCFEdgeToMap(Eigen::Vector3d& relative_pose, int from, int to, 
+      int edge_id, Edge::Type edge_type, Eigen::Matrix3d& info);
+  Eigen::Vector2d ComputeRelativeDA();
+  void SetFrameDistance();
+  bool FindLoopClosure();
+  void AddLoopEdges();
+  bool OptimizeMap();
+  void CheckAndOptimize();
+  void UpdateValueAfterLoop();
+
+  // for visualization
+  bool GetCFPose(Eigen::Vector3d& pose); 
+  bool GetFramePoses(Aligned<std::vector, Eigen::Vector3d>& poses, std::vector<double>& timestamps);
+  bool GetOccupancyMapOrigin(
+      Eigen::Vector3d& pixel_origin, Eigen::Matrix<double, 7, 1>& real_origin);  // [qw, qx, qy, qz, x, y, z]
+  double GetMapResolution();
+  OccupancyData& GetMapData();
+
+private:
+  bool _init;
+  int _frame_id;
+  int _edge_id;
+  bool _last_lost;
+  
+  // tmp
+  FramePtr _last_frame;
+  FramePtr _current_frame;
+  // image plane pose
+  Eigen::Vector3d _last_cf_pose;
+  Eigen::Vector3d _current_cf_pose;
+  // real scale pose
+  Eigen::Vector3d _last_cf_real_pose;
+  Eigen::Vector3d _current_cf_real_pose;
+  // robot pose
+  Eigen::Vector3d _last_pose;
+  Eigen::Vector3d _current_pose;
+  // distance
+  double _distance;
+
+  // intermedium rsults
+  Eigen::ArrayXXf _image_array;
+  Eigen::ArrayXXcf _last_fft_result;
+  Eigen::ArrayXXcf _last_fft_polar;
+  Eigen::ArrayXXcf _fft_result;
+  Eigen::ArrayXXcf _fft_polar;
+  std::vector<LoopClosureResult> _loop_matches;
+
+  Configs _configs;
+  CameraPtr _camera;
+  CorrelationFlowPtr _correlation_flow;
+  KeyframeSelectionConfig _kfs_config;
+  MapPtr _map;
+  LoopClosurePtr _loop_closure;
+  MapStitcherPtr _map_stitcher;
+};
+
+
+#endif  // MAP_BUILDER_H