mapper: start writing..

CMakeLists.txt

@@ -58,6 +58,7 @@ target_link_libraries(main
   oh_my_loam
   extractor
   odometry
+  mapper
   solver
   ${CERES_LIBRARIES}
   helper

configs/config.yaml

@@ -17,7 +17,12 @@ extractor_config:
   neighbor_point_dist_thres: 0.05
   downsample_voxel_size: 0.3
 
+# configs for odometry
 odometry_config: 
   icp_iter_num : 2
   match_dist_sq_thresh: 1
-  vis: true
+  vis: true
+
+# configs for mapper
+odometry_config: 
+  vis: false

src/CMakeLists.txt

@@ -1,6 +1,7 @@
 add_subdirectory(extractor)
 add_subdirectory(visualizer)
 add_subdirectory(odometry)
+add_subdirectory(mapper)
 add_subdirectory(helper)
 add_subdirectory(solver)
 

src/mapper/CMakeLists.txt

@@ -0,0 +1,3 @@
+aux_source_directory(. SRC_FILES)
+
+add_library(mapper SHARED ${SRC_FILES})

src/mapper/mapper.cc

@@ -0,0 +1,16 @@
+#include "mapper.h"
+
+namespace oh_my_loam {
+
+bool Mapper::Init(const YAML::Node& config) {
+  config_ = config;
+  is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
+  AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
+  return true;
+}
+
+void Mapper::Process() {}
+
+void Mapper::Visualize() {}
+
+}  // namespace oh_my_loam

src/mapper/mapper.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "common.h"
+
+namespace oh_my_loam {
+
+class Mapper {
+ public:
+  Mapper() = default;
+
+  bool Init(const YAML::Node& config);
+
+  void Process();
+
+ private:
+  void Visualize();
+
+  TPointCloudPtr map_pts_;
+  Pose3D pose_curr2world_;
+
+  bool is_initialized = false;
+  bool is_vis_ = false;
+
+  YAML::Node config_;
+
+  DISALLOW_COPY_AND_ASSIGN(Mapper)
+};
+
+}  // namespace oh_my_loam

src/solver/cost_function.h

@@ -15,7 +15,7 @@ class PointLineCostFunction {
   bool operator()(const T* const q, const T* const p, T* residual) const;
 
   static ceres::CostFunction* Create(const PointLinePair& pair, double time) {
-    return new ceres::AutoDiffCostFunction<PointLineCostFunction, 1, 4, 3>(
+    return new ceres::AutoDiffCostFunction<PointLineCostFunction, 3, 4, 3>(
         new PointLineCostFunction(pair, time));
   }
 
@@ -60,9 +60,11 @@ bool PointLineCostFunction::operator()(const T* const q, const T* const p,
   Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
 
   // norm of cross product: triangle area x 2
-  T area = (pnt_n - pnt1).cross(pnt2 - pnt1).norm() * 0.5;
+  Eigen::Matrix<T, 3, 1> area = (pnt_n - pnt1).cross(pnt_n - pnt2) * 0.5;
   T base_length = (pnt2 - pnt1).norm();
-  residual[0] = area / base_length;
+  residual[0] = area[0] / base_length;
+  residual[1] = area[1] / base_length;
+  residual[2] = area[2] / base_length;
   return true;
 }