git update

src/ground_slam/include/camera.h

@@ -0,0 +1,59 @@
+#ifndef CAMERA_H_
+#define CAMERA_H_
+
+#include <iostream>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/SparseCore>
+
+class Camera{
+public:
+  Camera();
+  Camera(const std::string& camera_file);
+  Camera& operator=(const Camera& camera);
+  
+  void UndistortImage(cv::Mat& image, cv::Mat& undistort_image);
+  void GetNewCameraMatrix(cv::Mat& camera_matrix);
+  double GetImageHeight();
+  double GetImageWidth();
+  double GetHeight();
+  bool HeightIsAccurate();
+  void GetExtrinsics(Eigen::Matrix3d& extrinsics);
+  double GetLengthOfPixel();
+
+  Eigen::Vector3d ConvertPrincipalToCenter(const Eigen::Vector3d& image_plane_pose);
+  Eigen::Vector3d ConvertCenterToPrincipal(const Eigen::Vector3d& image_center_pose);
+
+  // Image plane: pixel plane, principal point on the image is the origin 
+  // Camera: normalized plane, 
+  // Robot: robot body coordinate system.
+  bool ConvertImagePlanePoseToCamera(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& camera_pose);
+  bool ConvertCameraPoseToImagePlane(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& camera_pose);
+  bool ConvertCameraPoseToRobot(Eigen::Vector3d& camera_pose, Eigen::Vector3d& robot_pose);
+  bool ConvertRobotPoseToCamera(Eigen::Vector3d& camera_pose, Eigen::Vector3d& robot_pose);
+  bool ConvertImagePlanePoseToRobot(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& robot_pose);
+  bool ConvertRobotPoseToImagePlane(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& robot_pose);
+
+private:
+  int _image_height;
+  int _image_width;
+  double _scale;
+  double _new_scale;
+  int _new_width;
+  int _new_height;
+  double _height;
+  bool _accurate_height;
+  cv::Mat _K;
+  cv::Mat _new_K;
+  cv::Mat _D;
+  cv::Mat _map1;
+  cv::Mat _map2;
+  Eigen::Matrix3d _extrinsics;
+};
+
+typedef std::shared_ptr<Camera> CameraPtr;
+
+#endif  // CAMERA_H_

src/ground_slam/include/circ_shift.h

@@ -0,0 +1,252 @@
+// circ_shift.h
+// https://stackoverflow.com/questions/46077242/eigen-modifyable-custom-expression/46301503#46301503
+// this file implements circShift, fftshift, and ifftshift for Eigen vectors/matrices.
+//
+
+#pragma once
+#include <Eigen/Core>
+
+template <bool B> using bool_constant = std::integral_constant<bool, B>;
+
+namespace helper
+{
+namespace detail
+{
+template <typename T>
+constexpr std::true_type is_matrix(Eigen::MatrixBase<T>);
+std::false_type constexpr is_matrix(...);
+
+template <typename T>
+constexpr std::true_type is_array(Eigen::ArrayBase<T>);
+std::false_type constexpr is_array(...);
+}
+
+
+template <typename T>
+struct is_matrix : decltype(detail::is_matrix(std::declval<std::remove_cv_t<T>>()))
+{
+};
+
+template <typename T>
+struct is_array : decltype(detail::is_array(std::declval<std::remove_cv_t<T>>()))
+{
+};
+
+template <typename T>
+using is_matrix_or_array = bool_constant<is_array<T>::value || is_matrix<T>::value>;
+
+/*
+ * Index something if it's not an scalar
+ */
+template <typename T, typename std::enable_if<is_matrix_or_array<T>::value, int>::type = 0>
+auto index_if_necessary(T&& thing, Eigen::Index idx)
+{
+  return thing(idx);
+}
+
+/*
+ * Overload for scalar.
+ */
+template <typename T, typename std::enable_if<std::is_scalar<std::decay_t<T>>::value, int>::type = 0>
+  auto index_if_necessary(T&& thing, Eigen::Index)
+{
+  return thing;
+}
+}
+
+namespace Eigen
+{
+template <typename XprType, typename RowIndices, typename ColIndices>
+class CircShiftedView;
+
+namespace internal
+{
+template <typename XprType, typename RowIndices, typename ColIndices>
+struct traits<CircShiftedView<XprType, RowIndices, ColIndices>>
+  : traits<XprType>
+{
+  enum
+  {
+    RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
+    ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
+    MaxRowsAtCompileTime = (RowsAtCompileTime != Dynamic 
+                            ? int(RowsAtCompileTime) 
+                            : int(traits<XprType>::MaxRowsAtCompileTime)),
+    MaxColsAtCompileTime = (ColsAtCompileTime != Dynamic 
+                            ? int(ColsAtCompileTime)
+                            : int(traits<XprType>::MaxColsAtCompileTime)),
+    XprTypeIsRowMajor = (int(traits<XprType>::Flags) & RowMajorBit) != 0,
+    IsRowMajor = ((MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1
+                  : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
+                  : XprTypeIsRowMajor),
+    FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
+    FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
+    Flags = (traits<XprType>::Flags & HereditaryBits) | FlagsLvalueBit | FlagsRowMajorBit
+  };
+};
+}
+
+template <typename XprType, typename RowShift, typename ColShift, typename StorageKind>
+class CircShiftedViewImpl;
+
+
+template <typename XprType, typename RowShift, typename ColShift>
+class CircShiftedView : public CircShiftedViewImpl<XprType, RowShift, ColShift, 
+                                                   typename internal::traits<XprType>::StorageKind>
+{
+public:
+  typedef typename CircShiftedViewImpl<XprType, RowShift, ColShift,
+                                       typename internal::traits<XprType>::StorageKind>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(CircShiftedView)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CircShiftedView)
+
+  typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
+  typedef typename internal::remove_all<XprType>::type NestedExpression;
+
+  template <typename T0, typename T1>
+  CircShiftedView(XprType& xpr, const T0& rowShift, const T1& colShift)
+    : m_xpr(xpr), m_rowShift(rowShift), m_colShift(colShift)
+  {
+    for (auto c = 0; c < xpr.cols(); ++c)
+      assert(std::abs(helper::index_if_necessary(m_rowShift, c)) < m_xpr.rows()); // row shift must be within +- rows()-1
+    for (auto r = 0; r < xpr.rows(); ++r)
+      assert(std::abs(helper::index_if_necessary(m_colShift, r)) < m_xpr.cols()); // col shift must be within +- cols()-1
+  }
+
+  /** \returns number of rows */
+  Index rows() const { return m_xpr.rows(); }
+
+  /** \returns number of columns */
+  Index cols() const { return m_xpr.cols(); }
+
+  /** \returns the nested expression */
+  const typename internal::remove_all<XprType>::type&
+  nestedExpression() const { return m_xpr; }
+
+  /** \returns the nested expression */
+  typename internal::remove_reference<XprType>::type&
+  nestedExpression() { return m_xpr.const_cast_derived(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Index getRowIdx(Index row, Index col) const
+  {
+    Index R = m_xpr.rows();
+    assert(row >= 0 && row < R && col >= 0 && col < m_xpr.cols());
+    Index r = row - helper::index_if_necessary(m_rowShift, col);
+    if (r >= R)
+      return r - R;
+    if (r < 0)
+      return r + R;
+    return r;
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Index getColIdx(Index row, Index col) const
+  {
+    Index C = m_xpr.cols();
+    assert(row >= 0 && row < m_xpr.rows() && col >= 0 && col < C);
+    Index c = col - helper::index_if_necessary(m_colShift, row);
+    if (c >= C)
+      return c - C;
+    if (c < 0)
+      return c + C;
+    return c;
+  }
+
+protected:
+  MatrixTypeNested m_xpr;
+  RowShift m_rowShift;
+  ColShift m_colShift;
+};
+
+
+// Generic API dispatcher
+template <typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
+class CircShiftedViewImpl
+  : public internal::generic_xpr_base<CircShiftedView<XprType, RowIndices, ColIndices>>::type
+{
+ public:
+  typedef typename internal::generic_xpr_base<CircShiftedView<XprType, RowIndices, ColIndices>>::type Base;
+};
+
+namespace internal
+{
+template <typename ArgType, typename RowIndices, typename ColIndices>
+struct unary_evaluator<CircShiftedView<ArgType, RowIndices, ColIndices>, IndexBased>
+  : evaluator_base<CircShiftedView<ArgType, RowIndices, ColIndices>>
+{
+  typedef CircShiftedView<ArgType, RowIndices, ColIndices> XprType;
+
+  enum
+  {
+    CoeffReadCost = (evaluator<ArgType>::CoeffReadCost 
+                     + NumTraits<Index>::AddCost  /* for comparison */
+                     + NumTraits<Index>::AddCost) /* for addition */,
+    Flags = (evaluator<ArgType>::Flags & HereditaryBits),
+    Alignment = 0
+  };
+
+  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  typedef typename XprType::Scalar Scalar;
+  typedef typename XprType::CoeffReturnType CoeffReturnType;
+
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    CoeffReturnType coeff(Index row, Index col) const
+  {
+    return m_argImpl.coeff(m_xpr.getRowIdx(row, col), m_xpr.getColIdx(row, col));
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    CoeffReturnType coeff(Index idx) const
+  {
+    if (m_xpr.cols() == 1)
+      return m_argImpl.coeff(m_xpr.getRowIdx(idx, 1), 1);
+    if (m_xpr.rows() == 1)
+      return m_argImpl.coeff(1, m_xpr.getColIdx(1, idx));
+    assert(m_xpr.cols() == 1 || m_xpr.rows() == 1);
+    // default no-assert case - assume col vector
+    return m_argImpl.coeff(m_xpr.getRowIdx(idx, 1), 1);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Scalar& coeffRef(Index row, Index col)
+  {
+    assert(row >= 0 && row < m_xpr.rows() && col >= 0 && col < m_xpr.cols());
+
+    return m_argImpl.coeffRef(m_xpr.getRowIdx(row, col), m_xpr.getColIdx(row, col));
+  }
+
+ protected:
+
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_xpr;
+};
+} // end namespace internal
+} // end namespace Eigen
+
+template <typename XprType, typename RowShift, typename ColShift>
+auto circShift(Eigen::DenseBase<XprType>& x, RowShift r, ColShift c)
+{
+    return Eigen::CircShiftedView<XprType, RowShift, ColShift>(x.derived(), r, c);
+}
+
+template <typename XprType>
+auto fftshift(Eigen::DenseBase<XprType>& x)
+{
+  Eigen::Index rs = x.rows() / 2;
+  Eigen::Index cs = x.cols() / 2;
+  return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
+}
+
+template <typename XprType>
+auto ifftshift(Eigen::DenseBase<XprType>& x)
+{
+  Eigen::Index rs = (x.rows() + 1) / 2;
+  Eigen::Index cs = (x.cols() + 1) / 2;
+  return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
+}

src/ground_slam/include/correlation_flow.h

@@ -0,0 +1,36 @@
+#ifndef CORRELATION_FLOW_H
+#define CORRELATION_FLOW_H
+
+#include <fftw3.h>
+#include "utils.h"
+#include "read_configs.h"
+
+class CorrelationFlow{
+
+public:
+    CorrelationFlow(CFConfig& cf_config, double &image_height, double &image_width);
+    void ComputeIntermedium(const Eigen::ArrayXXf&, Eigen::ArrayXXcf&, Eigen::ArrayXXcf&);
+    Eigen::Vector3d ComputePose(const Eigen::ArrayXXcf&, const Eigen::ArrayXXf&, const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, Eigen::Vector3d&, bool);
+
+private:
+    CFConfig cfg;
+    Eigen::ArrayXXcf target_fft;
+    Eigen::ArrayXXcf target_rotation_fft;
+    Eigen::ArrayXXcf FFT(const Eigen::ArrayXXf&);
+    Eigen::ArrayXXf IFFT(const Eigen::ArrayXXcf&);
+    Eigen::ArrayXXcf GetTargetFFT(int, int);
+    inline Eigen::ArrayXXf RemoveZeroComponent(const Eigen::ArrayXXf&);
+    inline Eigen::ArrayXXcf gaussian_kernel(const Eigen::ArrayXXcf&, int, int);
+    inline Eigen::ArrayXXcf gaussian_kernel(const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, int, int);
+    inline Eigen::ArrayXXcf polynomial_kernel(const Eigen::ArrayXXcf&, int, int);
+    inline Eigen::ArrayXXcf polynomial_kernel(const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, int, int);
+    float EstimateTrans(const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, int, int, Eigen::Vector2d&);
+    inline Eigen::ArrayXXf polar(const Eigen::ArrayXXf&);
+    inline float GetInfo(const Eigen::ArrayXXf&, float);
+    
+    // void PrintArrayToFile (const Eigen::ArrayXXf target);
+};
+
+typedef std::shared_ptr<CorrelationFlow> CorrelationFlowPtr;
+
+#endif  // CORRELATION_FLOW_H

src/ground_slam/include/dataset.h

@@ -0,0 +1,30 @@
+#ifndef DATASET_H_
+#define DATASET_H_
+
+#include <vector>
+#include <Eigen/Dense>
+#include <Eigen/SparseCore>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "utils.h"
+
+
+class Dataset{
+public:
+  Dataset(const std::string& dataroot, const std::string& image_dir_name);
+  size_t GetDatasetLength();
+  bool GetImage(cv::Mat& image, size_t idx);
+  double GetTimestamp(size_t idx);
+
+private:
+  std::string _dataroot;
+  std::string _image_dir;
+  std::string _image_name_file_path;
+  std::vector<std::string> _image_names;
+  std::string _time_file_path;
+  std::vector<double> _timestamps;
+};
+
+#endif // DATASET_H_

src/ground_slam/include/edge.h

@@ -0,0 +1,31 @@
+#ifndef EDGE_H_
+#define EDGE_H_
+
+#include <string>
+#include <Eigen/Dense>
+#include <Eigen/SparseCore>
+
+#include "utils.h"
+
+struct Edge{
+  enum Type {
+    Odom = 0,
+    KCC = 1,
+    Loop = 2,
+    Others = 3,
+  };
+
+  int _edge_id;
+  Type _type;
+  int _from;
+  int _to;
+  Eigen::Vector3d _T;
+  Eigen::Matrix3d _information;
+
+  Edge();
+  Edge(int edge_id, Type type, int from, int to, Eigen::Vector3d& T, Eigen::Matrix3d& information);
+};
+
+typedef std::shared_ptr<Edge> EdgePtr;
+
+#endif  // EDGE_H_

src/ground_slam/include/frame.h

@@ -0,0 +1,45 @@
+#ifndef FRAME_H_
+#define FRAME_H_
+
+#include <string>
+#include <Eigen/Dense>
+#include <Eigen/SparseCore>
+
+#include "utils.h"
+
+
+class Frame{
+public:
+  Frame();
+  Frame(int frame_id);
+  Frame(int frame_id, double timestamp, Eigen::ArrayXXf&, Eigen::ArrayXXcf&, Eigen::ArrayXXcf&);
+  Frame& operator=(const Frame& other);
+
+  void SetFrameId(int frame_id);
+  int GetFrameId();
+  double GetTimestamp();
+  Eigen::ArrayXXf GetFrame();
+  void SetFFTResult(Eigen::ArrayXXcf& fft_result);
+  void SetFFTResult(Eigen::ArrayXXcf& fft_result, Eigen::ArrayXXcf& depth_fft_result);
+  void GetFFTResult(Eigen::ArrayXXcf& fft_result);
+  void GetFFTResult(Eigen::ArrayXXcf& fft_result, Eigen::ArrayXXcf& depth_fft_result);
+  void SetPose(Eigen::Vector3d& pose);
+  void GetPose(Eigen::Vector3d& pose);
+  void AddEdge(int edge_id);
+  void GetEdgeIds(std::vector<int>& edge_ids);
+  void SaveToDisk(const std::string root_dir);
+
+private:
+  int _frame_id;
+  double _timestamp;
+  Eigen::ArrayXXf _frame;
+  Eigen::ArrayXXcf _fft_result;
+  Eigen::ArrayXXcf _fft_polar;
+  Eigen::ArrayXXcf _depth_fft_result;
+  Eigen::Vector3d _pose;
+  std::vector<int> _edge_ids;
+};
+
+typedef std::shared_ptr<Frame> FramePtr;
+
+#endif  // FRAME_H_

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_

src/ground_slam/include/map.h

@@ -0,0 +1,81 @@
+#ifndef MAP_H_
+#define MAP_H_
+
+#include <vector>
+#include <map>
+#include <unordered_set>
+#include <unordered_map>
+
+#include "read_configs.h"
+#include "camera.h"
+#include "frame.h"
+#include "edge.h"
+#include "utils.h"
+
+
+struct GridLocation{
+  GridLocation() : x(0), y(0) {}
+  GridLocation(int x_, int y_) : x(x_), y(y_) {}
+  GridLocation& operator=(const GridLocation& other){
+    x = other.x;
+    y = other.y;
+    return *this;
+  }
+  friend std::ostream & operator<<(std::ostream &out, GridLocation &loc){
+    out << "(" << loc.x << ", " << loc.y << ")";
+    return out;
+  }
+
+  int x;
+  int y;
+};
+
+struct GridLocationHash{
+  std::size_t operator() (const GridLocation& loc) const{
+    return std::hash<int>()(loc.x) ^ std::hash<int>()(loc.y);
+  }
+};
+
+struct GridLocationEqual{
+  bool operator()(const GridLocation& l1, const GridLocation& l2) const{
+    return l1.x == l2.x && l1.y == l2.y;
+  }
+};
+
+typedef std::unordered_set<FramePtr> FrameSet;
+typedef std::unordered_map<GridLocation, FrameSet, GridLocationHash, GridLocationEqual> GridMap;
+
+class Map{
+public:
+  Map();
+  Map(MapConfig& map_config);
+
+  void AddFrame(FramePtr& frame);
+  void SetFrameDistance(FramePtr& frame, double distance);
+  void AddEdge(EdgePtr& edge);
+  
+  int GetAllFrames(std::vector<FramePtr>& frames);
+  double GetFrameDistance(FramePtr& frame);
+  int GetAllEdges(std::vector<EdgePtr>& edges);
+
+  void UpdatePoses(AlignedMap<int, Eigen::Vector3d> frame_poses);
+
+  GridLocation ComputeGridLocation(double x, double y);
+  GridLocation ComputeGridLocation(Eigen::Vector3d pose);
+  int GetFramesInGrids(std::vector<FramePtr>& frames, std::vector<GridLocation>& grid_locations);
+
+  FramePtr GetBaseframe();
+
+private:
+  CameraPtr _camera;
+  std::map<int, FramePtr> _frames;
+  std::map<FramePtr, double> _frame_distanses;
+  std::map<int, EdgePtr> _edges;
+  double _grid_scale;
+  GridMap _grid_map;
+  FramePtr _baseframe;
+};
+
+typedef std::shared_ptr<Map> MapPtr;
+
+#endif  // MAP_H_

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

src/ground_slam/include/map_stitcher.h

@@ -0,0 +1,42 @@
+#ifndef MAP_STITCHER_H_
+#define MAP_STITCHER_H_
+
+#include "optimization_2d/pose_graph_2d_error_term.h"
+#include "camera.h"
+#include "frame.h"
+#include "map.h"
+#include "read_configs.h"
+
+
+struct Cell{
+  int size;
+  Eigen::ArrayXXi data;
+  Eigen::ArrayXXi weight;
+
+  Cell(): size(0) {}
+  Cell(int _size): size(_size), data(Eigen::ArrayXXi::Zero(_size, _size)), weight(Eigen::ArrayXXi::Zero(_size, _size)) {}
+};
+
+typedef std::unordered_map<GridLocation, Cell, GridLocationHash, GridLocationEqual> OccupancyData;
+
+class MapStitcher{
+public:
+  MapStitcher(MapStitcherConfig config, CameraPtr camera);
+  void InsertFrame(FramePtr frame, cv::Mat& image);
+  // input is the posion in one axis, output is the cell and the position in cell
+  Eigen::Vector2i ComputeCellPosition(int x);
+  void AddImageToOccupancy(FramePtr frame);
+  void RecomputeOccupancy();
+  OccupancyData& GetOccupancyData();
+
+private:
+  int _cell_size;
+  bool _to_stitch;
+  CameraPtr _camera;
+  std::unordered_map<FramePtr, Eigen::MatrixXi> _raw_images;
+  OccupancyData _occupancy_data;
+};
+
+typedef std::shared_ptr<MapStitcher> MapStitcherPtr;
+
+#endif  // MAP_STITCHER_H_

src/ground_slam/include/optimization_2d/angle_local_parameterization.h

@@ -0,0 +1,66 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef OPTIMIZATION_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#define OPTIMIZATION_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+
+#include <ceres/local_parameterization.h>
+
+#include "normalize_angle.h"
+
+
+namespace ceres {
+namespace optimization_2d {
+
+// Defines a local parameterization for updating the angle to be constrained in
+// [-pi to pi).
+class AngleLocalParameterization {
+ public:
+  template <typename T>
+  bool operator()(const T* theta_radians,
+                  const T* delta_theta_radians,
+                  T* theta_radians_plus_delta) const {
+    *theta_radians_plus_delta =
+        NormalizeAngle(*theta_radians + *delta_theta_radians);
+
+    return true;
+  }
+
+  static ceres::LocalParameterization* Create() {
+    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+                                                     1,
+                                                     1>);
+  }
+};
+
+}  // namespace optimization_2d
+}  // namespace ceres
+
+#endif  // OPTIMIZATION_2D_ANGLE_LOCAL_PARAMETERIZATION_H_

src/ground_slam/include/optimization_2d/normalize_angle.h

@@ -0,0 +1,52 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef OPTIMIZATION_2D_NORMALIZE_ANGLE_H_
+#define OPTIMIZATION_2D_NORMALIZE_ANGLE_H_
+
+#include <cmath>
+#include <ceres/ceres.h>
+
+namespace ceres {
+namespace optimization_2d {
+
+// Normalizes the angle in radians between [-pi and pi).
+template <typename T>
+inline T NormalizeAngle(const T& angle_radians) {
+  // Use ceres::floor because it is specialized for double and Jet types.
+  T two_pi(2.0 * M_PI);
+  return angle_radians -
+         two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
+}
+
+}  // namespace optimization_2d
+}  // namespace ceres
+
+#endif  // OPTIMIZATION_2D_NORMALIZE_ANGLE_H_

src/ground_slam/include/optimization_2d/pose_graph_2d.h

@@ -0,0 +1,33 @@
+#ifndef OPTIMIZATION_2D_POSE_GRAPH_2D_H_
+#define OPTIMIZATION_2D_POSE_GRAPH_2D_H_
+
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <string>
+#include <vector>
+#include <ceres/ceres.h>
+
+#include "angle_local_parameterization.h"
+#include "pose_graph_2d_error_term.h"
+#include "types.h"
+
+namespace ceres {
+namespace optimization_2d {
+
+void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints,
+                              std::map<int, Pose2d>* poses,
+                              ceres::Problem* problem);
+
+void BuildOptimizationProblemWithScale(const std::vector<Constraint2d>& constraints,
+                              std::vector<ScaleData>& scale_data,
+                              const std::vector<int>& scale_data_idx,
+                              std::map<int, Pose2d>* poses,
+                              ceres::Problem* problem);
+
+bool SolveOptimizationProblem(ceres::Problem* problem);
+
+}  // namespace optimization_2d
+}  // namespace ceres
+
+#endif  // OPTIMIZATION_2D_POSE_GRAPH_2D_H_

src/ground_slam/include/optimization_2d/pose_graph_2d_error_term.h

@@ -0,0 +1,183 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Cost function for a 2D pose graph formulation.
+
+#ifndef OPTIMIZATION_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+#define OPTIMIZATION_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+
+#include <Eigen/Core>
+
+#include "optimization_2d/normalize_angle.h"
+
+namespace ceres {
+namespace optimization_2d {
+
+template <typename T>
+Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
+  const T cos_yaw = ceres::cos(yaw_radians);
+  const T sin_yaw = ceres::sin(yaw_radians);
+
+  Eigen::Matrix<T, 2, 2> rotation;
+  rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
+  return rotation;
+}
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement.
+//
+// residual =  information^{1/2} * [  r_a^T * (p_b - p_a) - \hat{p_ab}   ]
+//                                 [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
+//
+// where r_a is the rotation matrix that rotates a vector represented in frame A
+// into the global frame, and Normalize(*) ensures the angles are in the range
+// [-pi, pi).
+class PoseGraph2dErrorTerm {
+ public:
+  PoseGraph2dErrorTerm(double x_ab,
+                       double y_ab,
+                       double yaw_ab_radians,
+                       const Eigen::Matrix3d& sqrt_information)
+      : p_ab_(x_ab, y_ab),
+        yaw_ab_radians_(yaw_ab_radians),
+        sqrt_information_(sqrt_information) {}
+
+  template <typename T>
+  bool operator()(const T* const x_a,
+                  const T* const y_a,
+                  const T* const yaw_a,
+                  const T* const x_b,
+                  const T* const y_b,
+                  const T* const yaw_b,
+                  T* residuals_ptr) const {
+    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
+    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
+
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
+
+    residuals_map.template head<2>() =
+        RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
+    residuals_map(2) = NormalizeAngle(
+        (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+
+    // Scale the residuals by the square root information matrix to account for
+    // the measurement uncertainty.
+    residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+
+    return true;
+  }
+
+  static ceres::CostFunction* Create(double x_ab,
+                                     double y_ab,
+                                     double yaw_ab_radians,
+                                     const Eigen::Matrix3d& sqrt_information) {
+    return (new ceres::
+                AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
+                    new PoseGraph2dErrorTerm(
+                        x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  // The position of B relative to A in the A frame.
+  const Eigen::Vector2d p_ab_;
+  // The orientation of frame B relative to frame A.
+  const double yaw_ab_radians_;
+  // The inverse square root of the measurement covariance matrix.
+  const Eigen::Matrix3d sqrt_information_;
+};
+
+
+class PoseGraph2dErrorTermWithScale {
+ public:
+  PoseGraph2dErrorTermWithScale(double x_ab,
+                       double y_ab,
+                       double yaw_ab_radians,
+                       const Eigen::Matrix3d& sqrt_information)
+      : p_ab_(x_ab, y_ab),
+        yaw_ab_radians_(yaw_ab_radians),
+        sqrt_information_(sqrt_information) {}
+
+  template <typename T>
+  bool operator()(const T* const x_a,
+                  const T* const y_a,
+                  const T* const yaw_a,
+                  const T* const x_b,
+                  const T* const y_b,
+                  const T* const yaw_b,
+                  const T* const scale,
+                  T* residuals_ptr) const {
+    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
+    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
+
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
+
+    Eigen::Matrix<T, 2, 2> scale_matrix = Eigen::Matrix<T, 2, 2>::Identity();
+    scale_matrix *= (*scale);
+
+    residuals_map.template head<2>() =
+        RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - scale_matrix * p_ab_.cast<T>();
+    residuals_map(2) = NormalizeAngle(
+        (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+
+    // Scale the residuals by the square root information matrix to account for
+    // the measurement uncertainty.
+    residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+
+    return true;
+  }
+
+  static ceres::CostFunction* Create(double x_ab,
+                                     double y_ab,
+                                     double yaw_ab_radians,
+                                     const Eigen::Matrix3d& sqrt_information) {
+    return (new ceres::
+                AutoDiffCostFunction<PoseGraph2dErrorTermWithScale, 3, 1, 1, 1, 1, 1, 1, 1>(
+                    new PoseGraph2dErrorTermWithScale(
+                        x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  // The position of B relative to A in the A frame.
+  const Eigen::Vector2d p_ab_;
+  // The orientation of frame B relative to frame A.
+  const double yaw_ab_radians_;
+  // The inverse square root of the measurement covariance matrix.
+  const Eigen::Matrix3d sqrt_information_;
+};
+
+
+}  // namespace optimization_2d
+}  // namespace ceres
+
+#endif  // OPTIMIZATION_2D_POSE_GRAPH_2D_ERROR_TERM_H_

src/ground_slam/include/optimization_2d/types.h

@@ -0,0 +1,116 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
+// the graph has a unique integer ID with a position and orientation. There are
+// delta transformation constraints between two vertices.
+
+#ifndef OPTIMIZATION_2D_TYPES_H_
+#define OPTIMIZATION_2D_TYPES_H_
+
+#include <fstream>
+#include <Eigen/Core>
+
+#include "optimization_2d/normalize_angle.h"
+
+namespace ceres {
+namespace optimization_2d {
+
+struct ScaleData{
+  double scale;
+  bool fixed;
+
+  ScaleData() {}
+  ScaleData(double _scale, bool _fixed): scale(_scale), fixed(_fixed) {}
+
+};
+
+inline std::istream& operator>>(std::istream& input, ScaleData& scale_data) {
+  input >> scale_data.scale >> scale_data.fixed;
+  // Normalize the angle between -pi to pi.
+  return input;
+}
+
+// The state for each vertex in the pose graph.
+struct Pose2d {
+  double x;
+  double y;
+  double yaw_radians;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() { return "VERTEX_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
+  input >> pose.x >> pose.y >> pose.yaw_radians;
+  // Normalize the angle between -pi to pi.
+  pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+  return input;
+}
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint2d {
+  int id_begin;
+  int id_end;
+
+  double x;
+  double y;
+  double yaw_radians;
+
+  // The inverse of the covariance matrix for the measurement. The order of the
+  // entries are x, y, and yaw.
+  Eigen::Matrix3d information;
+
+  // The name of the data type in the g2o file format.
+  static std::string name() { return "EDGE_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+  input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
+      constraint.y >> constraint.yaw_radians >> constraint.information(0, 0) >>
+      constraint.information(0, 1) >> constraint.information(0, 2) >>
+      constraint.information(1, 1) >> constraint.information(1, 2) >>
+      constraint.information(2, 2);
+
+  // Set the lower triangular part of the information matrix.
+  constraint.information(1, 0) = constraint.information(0, 1);
+  constraint.information(2, 0) = constraint.information(0, 2);
+  constraint.information(2, 1) = constraint.information(1, 2);
+
+  // Normalize the angle between -pi to pi.
+  constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+  return input;
+}
+
+}  // namespace optimization_2d
+}  // namespace ceres
+
+#endif  // OPTIMIZATION_2D_TYPES_H_

src/ground_slam/include/read_configs.h

@@ -0,0 +1,137 @@
+#ifndef READ_CONFIGS_H_
+#define READ_CONFIGS_H_
+
+#include <iostream>
+#include <yaml-cpp/yaml.h>
+
+#include "utils.h"
+
+
+struct DatasetConfig{
+  std::string dataroot;
+  std::string image_dir_name;
+  std::string camera_file;
+};
+
+struct CFConfig{
+  int width;
+  int height;
+  float lambda;
+  int kernel;
+  float sigma;
+  float offset;
+  int power;
+  int rotation_divisor;
+  int rotation_channel;
+};
+
+struct KeyframeSelectionConfig{
+  double max_distance;
+  double max_angle;
+  double lower_response_thr;
+  double upper_response_thr;
+};
+
+struct MapConfig{
+  double grid_scale;
+};
+
+struct LoopClosureConfig{
+  bool to_find_loop;
+  double position_response_thr;
+  double angle_response_thr;
+  int frame_gap_thr;
+  double distance_thr;
+};
+
+struct MapStitcherConfig{
+  bool stitch_map;
+  int cell_size;
+};
+
+struct VisualizationConfig{
+  std::string frame_id;
+  std::string kcc_pose_topic;
+  std::string frame_pose_topic;
+  std::string map_topic;
+  std::string image_topic;
+};
+
+struct SavingConfig{
+  std::string saving_root;
+  bool save_pose;
+};
+
+struct Configs{
+  DatasetConfig dataset_config;
+  CFConfig cf_config;
+  KeyframeSelectionConfig keyframe_selection_config;
+  MapConfig map_config;
+  LoopClosureConfig loop_closure_config;
+  MapStitcherConfig map_stitcher_config;
+  VisualizationConfig visualization_config;
+  SavingConfig saving_config;
+
+  Configs(const std::string& config_file){
+    if(!FileExists(config_file)){
+      std::cout << "config file: " << config_file << " doesn't exist" << std::endl;
+      return;
+    }
+
+    YAML::Node file_node = YAML::LoadFile(config_file);
+    YAML::Node dataset_node = file_node["dataset"];
+    dataset_config.dataroot = dataset_node["dataroot"].as<std::string>();
+    dataset_config.image_dir_name = dataset_node["image_dir_name"].as<std::string>();
+    dataset_config.camera_file = dataset_node["camera_config"].as<std::string>();
+
+    YAML::Node cf_node = file_node["correlation_flow"];
+    cf_config.width = cf_node["width"].as<int>();
+    cf_config.height = cf_node["height"].as<int>();
+    cf_config.lambda = cf_node["lambda"].as<float>();
+    cf_config.rotation_divisor = cf_node["rotation_divisor"].as<int>();
+    cf_config.rotation_channel = cf_node["rotation_channel"].as<int>();
+    cf_config.kernel = cf_node["kernel"].as<int>();
+    cf_config.offset = cf_node["polynomial"]["offset"].as<float>();
+    cf_config.power = cf_node["polynomial"]["power"].as<int>();
+    cf_config.sigma = cf_node["gaussian"]["sigma"].as<float>();
+
+    YAML::Node kfs_node = file_node["keyframe_selection"];
+    keyframe_selection_config.max_distance = kfs_node["max_distance"].as<double>();
+    keyframe_selection_config.max_angle = kfs_node["max_angle"].as<double>();
+    keyframe_selection_config.lower_response_thr = kfs_node["lower_response_thr"].as<double>();
+    keyframe_selection_config.upper_response_thr = kfs_node["upper_response_thr"].as<double>();
+
+    YAML::Node map_node = file_node["map"];
+    map_config.grid_scale = map_node["grid_scale"].as<double>();
+
+    YAML::Node loop_closure_node = file_node["loop_closure"];
+    loop_closure_config.to_find_loop = 
+        loop_closure_node["to_find_loop"].as<bool>();
+    loop_closure_config.position_response_thr = 
+        loop_closure_node["position_response_thr"].as<double>();
+    loop_closure_config.angle_response_thr = 
+        loop_closure_node["angle_response_thr"].as<double>();
+    loop_closure_config.frame_gap_thr = 
+        loop_closure_node["frame_gap_thr"].as<int>();
+    loop_closure_config.distance_thr = 
+        loop_closure_node["distance_thr"].as<double>();
+
+    YAML::Node map_stitcher_node = file_node["map_sticther"];
+    map_stitcher_config.stitch_map = map_stitcher_node["stitch_map"].as<bool>();
+    map_stitcher_config.cell_size = map_stitcher_node["cell_size"].as<int>();
+
+    YAML::Node visualization_node = file_node["visualization"];
+    visualization_config.frame_id = visualization_node["frame_id"].as<std::string>();
+    visualization_config.kcc_pose_topic = visualization_node["topic"]["kcc_pose"].as<std::string>();
+    visualization_config.frame_pose_topic = visualization_node["topic"]["frame_pose"].as<std::string>();
+    visualization_config.map_topic = visualization_node["topic"]["map"].as<std::string>();
+    visualization_config.image_topic = visualization_node["topic"]["image"].as<std::string>();
+
+    YAML::Node saving_node = file_node["saving"];
+    saving_config.saving_root = saving_node["saving_root"].as<std::string>();
+    saving_config.save_pose = saving_node["save_pose"].as<bool>();
+  }
+};
+
+
+#endif  // READ_CONFIGS_H_

src/ground_slam/include/thread_publisher.h

@@ -0,0 +1,32 @@
+#ifndef THREAD_PUBLISHER_H_
+#define THREAD_PUBLISHER_H_
+#include <string>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <vector>
+#include <queue>
+#include <functional>
+
+template <typename T>
+class ThreadPublisher {
+public:
+  ThreadPublisher();
+  ~ThreadPublisher();
+
+  void Register(std::function<void(const std::shared_ptr<const T>&)> cb);
+  void Start();
+  void Publish(const std::shared_ptr<const T> msg);
+  void Process();
+  void ShutDown();
+
+private:
+  std::mutex msg_mutex_;
+  std::condition_variable msg_cond_;
+  std::queue<std::shared_ptr<const T>> msgs_;
+  std::thread publish_thread_;
+  std::vector<std::function<void(const std::shared_ptr<const T>&)>> callbacks_;
+  bool shutdown_requested_;
+};
+
+#endif  // THREAD_PUBLISHER_H_

src/ground_slam/include/timer.h

@@ -0,0 +1,27 @@
+#ifndef _TIMER_H_
+#define _TIMER_H_
+
+#include <sys/time.h>
+#include <stdio.h>
+
+
+typedef struct{
+	struct timeval start;
+	struct timeval stop;
+}Timer;
+
+void startTimer(Timer *pTimer);
+void stopTimer(Timer *pTimer);
+double getElapsedTime(Timer *pTimer);
+void writeTimeToFile(double arrTime[], int nCount, int nFrameNo, char *filename);
+
+#define INITIALIZE_TIMER Timer stTimer; double arrTime[100]
+#define START_TIMER startTimer(&stTimer)
+#define STOP_TIMER(text) stopTimer(&stTimer); printf(text); printf(": %.1f\n",getElapsedTime(&stTimer))
+#define END_TIMER(nIndex) stopTimer(&stTimer); arrTime[nIndex] = getElapsedTime(&stTimer)
+#define ACC_TIMER(nIndex) stopTimer(&stTimer); arrTime[nIndex] += getElapsedTime(&stTimer)
+#define WRITE_TIME_FILE(nCount, nFrameNo, filename) writeTimeToFile(arrTime, nCount, nFrameNo, (char *)filename)
+
+#endif//_TIMER_H_
+
+

src/ground_slam/include/utils.h

@@ -0,0 +1,82 @@
+#ifndef UTILS_H_
+#define UTILS_H_
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <vector>
+#include <sys/types.h>    
+#include <sys/stat.h>
+#include <functional>
+#include <map>
+#include <limits.h>
+#include <memory>
+#include <type_traits>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
+
+
+// Eigen type
+template <template <typename, typename> class Container, typename Type>
+using Aligned = Container<Type, Eigen::aligned_allocator<Type>>;
+
+template <typename KeyType, typename ValueType>
+using AlignedMap =
+    std::map<KeyType, ValueType, std::less<KeyType>,
+             Eigen::aligned_allocator<std::pair<const KeyType, ValueType>>>;
+
+template <typename KeyType, typename ValueType>
+using AlignedUnorderedMap = std::unordered_map<
+    KeyType, ValueType, std::hash<KeyType>, std::equal_to<KeyType>,
+    Eigen::aligned_allocator<std::pair<const KeyType, ValueType>>>;
+
+template <typename KeyType, typename ValueType>
+using AlignedUnorderedMultimap = std::unordered_multimap<
+    KeyType, ValueType, std::hash<KeyType>, std::equal_to<KeyType>,
+    Eigen::aligned_allocator<std::pair<const KeyType, ValueType>>>;
+
+template <typename Type>
+using AlignedUnorderedSet =
+    std::unordered_set<Type, std::hash<Type>, std::equal_to<Type>,
+                       Eigen::aligned_allocator<Type>>;
+
+// files
+bool FileExists(const std::string& file);
+bool PathExists(const std::string& path);
+void ConcatenateFolderAndFileName(
+    const std::string& folder, const std::string& file_name,
+    std::string* path);
+
+std::string ConcatenateFolderAndFileName(
+    const std::string& folder, const std::string& file_name);
+
+void MakeDir(const std::string& path);
+
+void ReadTxt(const std::string& file_path, 
+    std::vector<std::vector<std::string> >& lines, std::string seq);
+
+void WriteTxt(const std::string file_path, 
+    std::vector<std::vector<std::string> >& lines, std::string seq);
+
+// correlation flow
+void ConvertMatToNormalizedArray(cv::Mat& image, Eigen::ArrayXXf& array);
+Eigen::ArrayXXf ConvertMatToArray(const cv::Mat&);
+cv::Mat ConvertArrayToMat(const Eigen::ArrayXXf&);
+Eigen::ArrayXXf RotateArray(const Eigen::ArrayXXf&, float);
+Eigen::ArrayXXf WarpArray(const Eigen::ArrayXXf&, float tx, float ty, float degree);
+double NormalizeDegree(double angle_degree);
+
+// Eigen pose
+Eigen::Vector3d ComputeRelativePose(Eigen::Vector3d& pose1, Eigen::Vector3d& pose2);
+Eigen::Vector3d ComputeAbsolutePose(Eigen::Vector3d& pose1, Eigen::Vector3d& relative_pose);
+
+void ShowArray(const Eigen::ArrayXXf&, std::string window="debug", int waitKey=1);
+
+#endif  // UTILS_H_

src/ground_slam/include/visualization.h

@@ -0,0 +1,65 @@
+#ifndef VISUALIZATION_H_
+#define VISUALIZATION_H_
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <vector>
+#include <functional>
+#include <map>
+#include <memory>
+#include <Eigen/Core>
+
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+#include "read_configs.h"
+#include "map_stitcher.h"
+#include "map_builder.h"
+
+class Visualizer {
+public:
+  enum class TrajectoryType {
+    Frame = 0,
+    KCC = 1,
+    Odom = 2,
+  };
+
+  Visualizer(rclcpp::Node::SharedPtr node, VisualizationConfig& config);
+  void AddNewPoseToPath(
+    Eigen::Vector3d& pose, double time_double,
+    nav_msgs::msg::Path& path, std::string& frame_id);
+  void UpdateOdomPose(Eigen::Vector3d& pose, double time_double);
+  void UpdateKccPose(Eigen::Vector3d& pose, double time_double);
+  void UpdateFramePose(Aligned<std::vector, Eigen::Vector3d>& frame_poses,
+                       std::vector<double>& timestamps);
+  void ConvertMapToOccupancyMsgs(OccupancyData& map,
+                                 nav_msgs::msg::OccupancyGrid& msgs);
+  void UpdateMap(MapBuilder& map_builder);
+  void PublishImage(cv::Mat& image, double time_double);
+  void GetTrajectoryTxt(std::vector<std::vector<std::string>>& lines,
+                        TrajectoryType trajectory_type);
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  std::string frame_id_;
+
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr kcc_pose_pub_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr frame_pose_pub_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+
+  nav_msgs::msg::Path odom_pose_msgs_;
+  nav_msgs::msg::Path kcc_pose_msgs_;
+  nav_msgs::msg::Path frame_pose_msgs_;
+  nav_msgs::msg::OccupancyGrid occupancy_map_msgs_;
+};
+
+#endif  // VISUALIZATION_H_