diff --git a/CMakeLists.txt b/CMakeLists.txt
index a547f7d..ef92d01 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,6 +58,7 @@ target_link_libraries(main
   oh_my_loam
   extractor
   odometry
+  mapper
   solver
   ${CERES_LIBRARIES}
   helper
diff --git a/configs/config.yaml b/configs/config.yaml
index 5163c34..942a5cf 100644
--- a/configs/config.yaml
+++ b/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
\ No newline at end of file
+  vis: true
+
+# configs for mapper
+odometry_config: 
+  vis: false
\ No newline at end of file
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 02ab287..972fb72 100644
--- a/src/CMakeLists.txt
+++ b/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)
 
diff --git a/src/mapper/CMakeLists.txt b/src/mapper/CMakeLists.txt
new file mode 100644
index 0000000..61d72d1
--- /dev/null
+++ b/src/mapper/CMakeLists.txt
@@ -0,0 +1,3 @@
+aux_source_directory(. SRC_FILES)
+
+add_library(mapper SHARED ${SRC_FILES})
diff --git a/src/mapper/mapper.cc b/src/mapper/mapper.cc
new file mode 100644
index 0000000..5042ba8
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/mapper/mapper.h b/src/mapper/mapper.h
new file mode 100644
index 0000000..d3395b3
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/solver/cost_function.h b/src/solver/cost_function.h
index 2cc55ec..fe1c02e 100644
--- a/src/solver/cost_function.h
+++ b/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;
 }