diff --git a/common/filter.h b/common/filter.h
index eff1a69..27334b7 100644
--- a/common/filter.h
+++ b/common/filter.h
@@ -41,4 +41,4 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
   });
 }
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/common/tic_toc.h b/common/tic_toc.h
index 89d9b6c..6e6401e 100644
--- a/common/tic_toc.h
+++ b/common/tic_toc.h
@@ -22,4 +22,4 @@ class TicToc {
   std::chrono::time_point<std::chrono::system_clock> start_, end_;
 };
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/common/types.h b/common/types.h
index 710e0df..8d7790e 100644
--- a/common/types.h
+++ b/common/types.h
@@ -4,7 +4,12 @@
 #include <pcl/point_types.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/point_cloud_handlers.h>
+
 #include <cmath>
+// This hpp file should be included if user-defined point type is added, see
+// "How are the point types exposed?" section in
+// https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
+#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
 
 namespace oh_my_loam {
 
@@ -47,11 +52,11 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
 #define PCLColorHandlerGenericField \
   pcl::visualization::PointCloudColorHandlerGenericField
 
-struct EIGEN_ALIGN16 PointXYZTCT {
+struct PointXYZTCT {
   PCL_ADD_POINT4D;
   float time = 0.0f;
   float curvature = std::nanf("");
-  // PointType label = PointType::NORNAL;
+  int8_t type = 0;  // -2, -1, 0, 1, or 2
 
   PointXYZTCT() {
     x = y = z = 0.0f;
@@ -59,8 +64,8 @@ struct EIGEN_ALIGN16 PointXYZTCT {
   }
 
   PointXYZTCT(float x, float y, float z, float time = 0.0f,
-              float curvature = NAN)
-      : x(x), y(y), z(z), time(time), curvature(curvature) {
+              float curvature = NAN, int8_t type = 0)
+      : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
     data[3] = 1.0f;
   }
 
@@ -77,19 +82,25 @@ struct EIGEN_ALIGN16 PointXYZTCT {
     z = p.z;
     time = p.time;
     curvature = p.curvature;
-    // type = p.type;
+    type = p.type;
     data[3] = 1.0f;
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
+  PointType Type() const { return static_cast<PointType>(type); }
 
-}  // oh_my_loam
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+}  // namespace oh_my_loam
 
 // clang-format off
 POINT_CLOUD_REGISTER_POINT_STRUCT(
   oh_my_loam::PointXYZTCT,
-  (float, x, x)(float, y, y)(float, z, z)
-    (float, time, time)(float, curvature, curvatur)
+  (float, x, x)
+  (float, y, y)
+  (float, z, z)
+  (float, time, time)
+  (float, curvature, curvatur)
+  (int8_t, type, type)
 )
 // clang-format on
\ No newline at end of file
diff --git a/common/utils.h b/common/utils.h
index 472cc15..31696e9 100644
--- a/common/utils.h
+++ b/common/utils.h
@@ -46,4 +46,4 @@ void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
 }
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/main.cc b/main.cc
index 3cd6537..0de525c 100644
--- a/main.cc
+++ b/main.cc
@@ -1,7 +1,9 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
+
 #include <functional>
+
 #include "oh_my_loam.h"
 
 void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
diff --git a/src/feature_extractor_VLP16.h b/src/feature_extractor_VLP16.h
index 5110a65..f9058c5 100644
--- a/src/feature_extractor_VLP16.h
+++ b/src/feature_extractor_VLP16.h
@@ -17,11 +17,11 @@ class FeatureExtractorVLP16 : public FeaturePointsExtractor {
     if (i++ < 10)
       std::cout << "OMEGA: "
                 << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
-                << " id = " << static_cast<int>(std::round(omega) + 0.01)
+                << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
                 << " z = " << pt.z << " "
                 << " d = " << Distance(pt) << std::endl;
-    return static_cast<int>(std::round(omega) + 0.01);
+    return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
   }
 };
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/src/feature_extractor_base.cc b/src/feature_extractor_base.cc
index b230cab..307d03c 100644
--- a/src/feature_extractor_base.cc
+++ b/src/feature_extractor_base.cc
@@ -1,19 +1,20 @@
 #include "feature_extractor_base.h"
 
 #include <cmath>
+
 #include "filter.h"
 
 namespace oh_my_loam {
 
 const double kPointMinDist = 0.1;
 const int kScanSegNum = 6;
-const double kTwoPi = kTwoPi;
+const double kTwoPi = 2 * M_PI;
 const int kMinPtsNum = 100;
 
 void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
                                      FeaturePoints* const feature) const {
   PointCloudPtr cloud(new PointCloud);
-  std::cout << "BEFORE REMOVE, num = " << cloud->size() << std::endl;
+  std::cout << "BEFORE REMOVE, num = " << cloud_in.size() << std::endl;
   RemoveNaNPoint<Point>(cloud_in, cloud.get());
   RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
   std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl;
@@ -30,24 +31,24 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
   std::cout << std::endl;
   for (const auto& scan : scans) {
     feature->feature_pts += scan;
-    // for (const auto& pt : scan.points) {
-    //   switch (pt.type) {
-    //     case PointType::FLAT:
-    //       feature->flat_surf_pts->points.emplace_back(pt);
-    //       break;
-    //     case PointType::LESS_FLAT:
-    //       feature->less_flat_surf_pts->points.emplace_back(pt);
-    //       break;
-    //     case PointType::LESS_SHARP:
-    //       feature->less_sharp_corner_pts->points.emplace_back(pt);
-    //       break;
-    //     case PointType::SHARP:
-    //       feature->sharp_corner_pts->points.emplace_back(pt);
-    //       break;
-    //     default:
-    //       break;
-    //   }
-    // }
+    for (const auto& pt : scan.points) {
+      switch (pt.Type()) {
+        case PointType::FLAT:
+          feature->flat_surf_pts.points.emplace_back(pt);
+          break;
+        case PointType::LESS_FLAT:
+          feature->less_flat_surf_pts.points.emplace_back(pt);
+          break;
+        case PointType::LESS_SHARP:
+          feature->less_sharp_corner_pts.points.emplace_back(pt);
+          break;
+        case PointType::SHARP:
+          feature->sharp_corner_pts.points.emplace_back(pt);
+          break;
+        default:
+          break;
+      }
+    }
   }
 }
 
@@ -98,4 +99,4 @@ void FeaturePointsExtractor::SplitScan(
 //   }
 // }
 
-}  // oh_my_loam
+}  // namespace oh_my_loam
diff --git a/src/feature_extractor_base.h b/src/feature_extractor_base.h
index 9b54064..8083bd2 100644
--- a/src/feature_extractor_base.h
+++ b/src/feature_extractor_base.h
@@ -36,4 +36,4 @@ class FeaturePointsExtractor {
   int num_scans_ = 0;
 };
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/src/oh_my_loam.cc b/src/oh_my_loam.cc
index f3f4bdf..4beec4d 100644
--- a/src/oh_my_loam.cc
+++ b/src/oh_my_loam.cc
@@ -3,7 +3,7 @@
 namespace oh_my_loam {
 
 bool OhMyLoam::Init() {
-  is_vis_ = false;
+  is_vis_ = true;
   feature_extractor_.reset(new FeatureExtractorVLP16);
   if (is_vis_) {
     visualizer_.reset(new FeaturePointsVisualizer);
@@ -12,7 +12,6 @@ bool OhMyLoam::Init() {
 }
 
 void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
-  pcl::PointCloud<pcl::PointXYZ> cl;
   std::shared_ptr<FeaturePoints> feature_pts(new FeaturePoints);
   feature_extractor_->Extract(cloud, feature_pts.get());
   if (is_vis_) Visualize(cloud, feature_pts, timestamp);
@@ -21,11 +20,11 @@ void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
 void OhMyLoam::Visualize(
     const PointCloud& cloud,
     const std::shared_ptr<const FeaturePoints>& feature_pts, double timestamp) {
-  std::shared_ptr<FeaturePointsVisFrame> frame;
+  std::shared_ptr<FeaturePointsVisFrame> frame(new FeaturePointsVisFrame);
   frame->timestamp = timestamp;
   frame->cloud = cloud.makeShared();
   frame->feature_pts = feature_pts;
   visualizer_->Render(frame);
 }
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/src/oh_my_loam.h b/src/oh_my_loam.h
index fb65e62..eda8aa9 100644
--- a/src/oh_my_loam.h
+++ b/src/oh_my_loam.h
@@ -26,4 +26,4 @@ class OhMyLoam {
   std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr;
 };
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/src/visualizer_base.h b/src/visualizer_base.h
index 2573298..f731efe 100644
--- a/src/visualizer_base.h
+++ b/src/visualizer_base.h
@@ -2,11 +2,13 @@
 
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/point_cloud_handlers.h>
+
 #include <atomic>
 #include <deque>
 #include <mutex>
 #include <string>
 #include <thread>
+
 #include "types.h"
 
 namespace oh_my_loam {
@@ -160,4 +162,4 @@ class Visualizer {
   std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
 };
 
-}  // oh_my_loam
\ No newline at end of file
+}  // namespace oh_my_loam
\ No newline at end of file
diff --git a/src/visualizer_feature_points.h b/src/visualizer_feature_points.h
index 85ce447..ec8ab92 100644
--- a/src/visualizer_feature_points.h
+++ b/src/visualizer_feature_points.h
@@ -26,35 +26,34 @@ class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
     }
     {  // add all feature_pts
       std::string id = "feature_pts";
-      DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, GRAY, id,
+      DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, "time", id,
                              viewer_.get());
       rendered_cloud_ids_.push_back(id);
     }
-    // {  // add flat_surf_pts
-    //   std::string id = "flat_surf_pts";
-    //   DrawPointCloud(*frame.feature_ptsflat_surf_pts, CYAN, id,
-    //   viewer_.get());
-    //   rendered_cloud_ids_.push_back(id);
-    // }
-    // {  // add less_flat_surf_pts
-    //   std::string id = "less_flat_surf_pts";
-    //   DrawPointCloud(*frame.feature_ptsless_flat_surf_pts, GREEN, id,
-    //   viewer_.get());
-    //   rendered_cloud_ids_.push_back(id);
-    // }
-    // {  // add less_sharp_corner_pts
-    //   std::string id = "less_sharp_corner_pts";
-    //   DrawPointCloud(*frame.feature_ptsless_sharp_corner_pts, ORANGE, id,
-    //   viewer_.get());
-    //   rendered_cloud_ids_.push_back(id);
-    // }
-    // {  // add sharp_corner_pts
-    //   std::string id = "sharp_corner_pts";
-    //   DrawPointCloud(*frame.feature_pts.sharp_corner_pts, ORANGE, id,
-    //   viewer_.get());
-    //   rendered_cloud_ids_.push_back(id);
-    // }
+    {  // add flat_surf_pts
+      std::string id = "flat_surf_pts";
+      DrawPointCloud(frame.feature_pts->flat_surf_pts, CYAN, id, viewer_.get());
+      rendered_cloud_ids_.push_back(id);
+    }
+    {  // add less_flat_surf_pts
+      std::string id = "less_flat_surf_pts";
+      DrawPointCloud(frame.feature_pts->less_flat_surf_pts, GREEN, id,
+                     viewer_.get());
+      rendered_cloud_ids_.push_back(id);
+    }
+    {  // add less_sharp_corner_pts
+      std::string id = "less_sharp_corner_pts";
+      DrawPointCloud(frame.feature_pts->less_sharp_corner_pts, ORANGE, id,
+                     viewer_.get());
+      rendered_cloud_ids_.push_back(id);
+    }
+    {  // add sharp_corner_pts
+      std::string id = "sharp_corner_pts";
+      DrawPointCloud(frame.feature_pts->sharp_corner_pts, ORANGE, id,
+                     viewer_.get());
+      rendered_cloud_ids_.push_back(id);
+    }
   }
 };
 
-}  // oh_my_loam
+}  // namespace oh_my_loam