git update

src/ground_slam/include/loop_closure.h

@@ -0,0 +1,43 @@
+#ifndef LOOP_CLOSURE_H_
+#define LOOP_CLOSURE_H_
+
+#include "read_configs.h"
+#include "correlation_flow.h"
+#include "map.h"
+
+
+struct LoopClosureResult{
+  bool found;
+  Eigen::Vector3d response;
+  FramePtr current_frame;
+  FramePtr loop_frame;
+  Eigen::Vector3d relative_pose;
+
+  LoopClosureResult(): found(false), response(-1.0, -1.0, -1.0) {}
+  LoopClosureResult(bool _found, Eigen::Vector3d _resopnse): found(_found), response(_resopnse) {}
+  LoopClosureResult& operator=(const LoopClosureResult& other){
+    found = other.found;
+    response = other.response;
+    current_frame = other.current_frame;
+    loop_frame = other.loop_frame;
+    relative_pose = other.relative_pose;
+    return *this;
+  }
+};
+
+class LoopClosure{
+public:
+  LoopClosure(LoopClosureConfig& loop_closure_config, CorrelationFlowPtr correlation_flow, MapPtr map);
+  LoopClosureResult FindLoopClosure(Eigen::ArrayXXf& image, FramePtr& current_frame);
+  LoopClosureResult FindLoopClosure(Eigen::ArrayXXf& image, FramePtr& current_frame, Eigen::Vector3d& prior_pose);
+  LoopClosureResult FindLoopClosure(Eigen::ArrayXXf& image, FramePtr& current_frame, std::vector<FramePtr>& frames);
+
+private:
+  LoopClosureConfig _loop_thr;
+  CorrelationFlowPtr _correlation_flow;
+  MapPtr _map;
+};
+
+typedef std::shared_ptr<LoopClosure> LoopClosurePtr;
+
+#endif  // LOOP_CLOSURE_H_