diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch
index 64a1deb..6f38467 100644
--- a/launch/mapping_avia.launch
+++ b/launch/mapping_avia.launch
@@ -20,18 +20,18 @@
 	<param name="edgeb" type="double" value="0.1"/>
 	<param name="smallp_intersect" type="double" value="172.5"/>
 	<param name="smallp_ratio" type="double" value="1.2"/>
-	<param name="point_filter_num" type="int" value="5"/>
+	<param name="point_filter_num" type="int" value="4"/>
   <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
 
   <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
   <param name="map_file_path" type="string" value=" " />
   <param name="max_iteration" type="int" value="2" />
   <param name="dense_map_enable" type="bool" value="true" />
-  <param name="fov_degree" type="double" value="80" />
+  <param name="fov_degree" type="double" value="75" />
   <param name="filter_size_corner" type="double" value="0.3" />
   <param name="filter_size_surf" type="double" value="0.2" />
   <param name="filter_size_map" type="double" value="0.2" />
-  <param name="cube_side_length" type="double" value="15" />
+  <param name="cube_side_length" type="double" value="10" />
   </node>
 
   <group if="$(arg rviz)">
diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz
index c377629..cd634ff 100644
--- a/rviz_cfg/loam_livox.rviz
+++ b/rviz_cfg/loam_livox.rviz
@@ -217,8 +217,8 @@ Visualization Manager:
         - Alpha: 0.20000000298023224
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: -1.7674484252929688
-            Min Value: -8.243647575378418
+            Max Value: 0.32742905616760254
+            Min Value: -2.179872751235962
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -664,18 +664,18 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 6.608828544616699
-        Y: 3.081573247909546
-        Z: -3.704350709915161
+        X: 6.49392557144165
+        Y: -0.4694555699825287
+        Z: -1.3836920261383057
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.17020323872566223
+      Pitch: 0.3497962951660156
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.0474979877471924
+      Yaw: 5.425689220428467
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp
index 5c7e0fa..63cc2c2 100644
--- a/src/IMU_Processing.hpp
+++ b/src/IMU_Processing.hpp
@@ -26,7 +26,7 @@
 
 /// *************Preconfiguration
 
-#define MAX_INI_COUNT (50)
+#define MAX_INI_COUNT (200)
 
 const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
 
@@ -216,7 +216,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
 
     #ifdef DEBUG_PRINT
     // fout<<head->header.stamp.toSec()<<" "<<angvel_avr.transpose()<<" "<<acc_avr.transpose()<<std::endl;
-    #endif
+    #endif  
     dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
     
     /* covariance propagation */
@@ -224,7 +224,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
     Eigen::Matrix3d Exp_f   = Exp(angvel_avr, dt);
     acc_avr_skew<<SKEW_SYM_MATRX(angvel_avr);
 
-    F_x.block<3,3>(0,0)  = - Exp_f;
+    F_x.block<3,3>(0,0)  = Exp(angvel_avr, - dt);
     F_x.block<3,3>(0,9)  = - Eye3d * dt;
     // F_x.block<3,3>(3,0)  = R_imu * off_vel_skew * dt;
     F_x.block<3,3>(3,6)  = Eye3d * dt;
@@ -290,7 +290,6 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout
     pos_imu<<VEC_FROM_ARRAY(head->pos);
     angvel_avr<<VEC_FROM_ARRAY(head->gyr);
 
-    int i = 0;
     for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
     {
       dt = it_pcl->curvature / double(1000) - head->offset_time;
diff --git a/src/feature_extract.cpp b/src/feature_extract.cpp
index e6b97d9..44822c7 100644
--- a/src/feature_extract.cpp
+++ b/src/feature_extract.cpp
@@ -3,6 +3,8 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <livox_ros_driver/CustomMsg.h>
 
+// Feature will be updated in next version
+
 typedef pcl::PointXYZINormal PointType;
 using namespace std;
 ros::Publisher pub_full, pub_surf, pub_corn;
@@ -15,12 +17,11 @@ enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
 
 struct orgtype
 {
-  double range; // sqrt(x*x+y*y)
-  double dista; // 该点与后一个点的距离平方
-  double angle[2]; // 前(后)一个点、该点、原点所成角度
-  double intersect; // 前后点与该点的夹角
-  E_jump edj[2]; // 每个点前后点的关系
-  // Surround nor_dir;
+  double range;
+  double dista; 
+  double angle[2];
+  double intersect;
+  E_jump edj[2];
   Feature ftype;
   orgtype()
   {
@@ -32,7 +33,6 @@ struct orgtype
   }
 };
 
-// const int hor_line = 6;
 const double rad2deg = 180*M_1_PI;
 
 int lidar_type;
@@ -63,25 +63,6 @@ int main(int argc, char **argv)
   ros::init(argc, argv, "feature_extract");
   ros::NodeHandle n;
 
-  // lidar_type = MID;
-  // blind = 0.5;
-  // inf_bound = 10;
-  // // N_SCANS = 1;
-  // group_size = 8;
-  // disA = 0.01; disB = 0.1;
-  // p2l_ratio = 400;
-  // limit_maxmid = 9;
-  // limit_midmin = 16;
-  // // limit_maxmin;
-  // jump_up_limit = cos(175.0/180*M_PI);
-  // jump_down_limit = cos(5.0/180*M_PI);
-  // cos160 = cos(160.0/180*M_PI);
-  // edgea = 3; // 2
-  // edgeb = 0.05; // 0.1
-  // smallp_intersect = cos(170.0/180*M_PI);
-  // smallp_ratio = 1.3;
-  // point_filter_num = 4;
-
   n.param<int>("lidar_type", lidar_type, 0);
   n.param<double>("blind", blind, 0.5);
   n.param<double>("inf_bound", inf_bound, 10);
@@ -107,75 +88,18 @@ int main(int argc, char **argv)
   cos160 = cos(cos160/180*M_PI);
   smallp_intersect = cos(smallp_intersect/180*M_PI);
 
-  // lidar_type = HORIZON;
-  // blind = 0.5;
-  // inf_bound = 10;
-  // N_SCANS = 6;
-  // group_size = 8;
-  // disA = 0.01; disB = 0.1;
-  // p2l_ratio = 225;
-  // limit_maxmid = 6.25;
-  // limit_midmin = 6.25;
-  // // limit_maxmin;
-  // jump_up_limit = cos(170.0/180*M_PI);
-  // jump_down_limit = cos(8.0/180*M_PI);
-  // cos160 = cos(160.0/180*M_PI);
-  // edgea = 2;
-  // edgeb = 0.1;
-  // smallp_intersect = cos(170.0/180*M_PI);
-  // smallp_ratio = 1.4;
-
-  // lidar_type = VELO16;
-  // blind = 0.5;
-  // inf_bound = 10;
-  // N_SCANS = 16;
-  // group_size = 9;
-  // disA = 0.015; disB = 0.3;
-  // p2l_ratio = 400;
-  // // limit_maxmid = 9;
-  // // limit_midmin = 16;
-  // limit_maxmin = 3.24;
-  // jump_up_limit = cos(170.0/180*M_PI);
-  // jump_down_limit = cos(8.0/180*M_PI);
-  // cos160 = cos(160.0/180*M_PI);
-  // edgea = 2; // 2
-  // edgeb = 0.1; // 0.1
-  // smallp_intersect = cos(170.0/180*M_PI);
-  // smallp_ratio = 1.4;
-
-  // lidar_type = OUST64; // lidar的种类
-  // blind = 0.5; // lidar的盲区（非平方）
-  // inf_bound = 10; // 无穷远点最近点的距离限制（非平方）
-  // N_SCANS = 64; // 6, 16, 64
-  // group_size = 9; // 7->8 9
-  // disA = 0.015; // 0.015 // Ax+B
-  // disB = 0.3;  // 0.3
-  // p2l_ratio = 196; // 225 196
-  // // limit_maxmid = 9; // 6.25
-  // // limit_midmin = 16; // 6.25
-  // limit_maxmin = 6.25; // 6.25
-  // jump_up_limit = cos(170.0/180*M_PI);
-  // jump_down_limit = cos(8.0/180*M_PI);
-  // cos160 = cos(160.0/180*M_PI);
-  // edgea = 2.5; // 2
-  // edgeb = 0.2; // 0.1
-  // smallp_intersect = cos(170.0/180*M_PI);
-  // smallp_ratio = 1.4;
-
-
   ros::Subscriber sub_points;
   
-  
   switch(lidar_type)
   {
   case MID:
-    printf("MID40-70\n");
+    printf("MID40\n");
     sub_points = n.subscribe("/livox/lidar", 1000, mid_handler);
     // sub_points = n.subscribe("/livox/lidar_1LVDG1S006J5GZ3", 1000, mid_handler);
     break;
 
   case HORIZON:
-    printf("HORIZON_MID70pro\n");
+    printf("HORIZON\n");
     sub_points = n.subscribe("/livox/lidar", 1000, horizon_handler);
     break;
 
@@ -195,9 +119,9 @@ int main(int argc, char **argv)
     break;
   }
 
-  pub_full = n.advertise<sensor_msgs::PointCloud2>("/livox_cloud", 20);
-  pub_surf = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 20);
-  pub_corn = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 20);
+  pub_full = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud", 100);
+  pub_surf = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
+  pub_corn = n.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
 
   ros::spin();
   return 0;
@@ -218,7 +142,6 @@ void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
 
   for(uint i=0; i<plsize; i++)
   {
-    // types[i].range = sqrt(pl[i].x*pl[i].x + pl[i].y*pl[i].y);
     types[i].range = pl[i].x;
     vx = pl[i].x - pl[i+1].x;
     vy = pl[i].y - pl[i+1].y;
@@ -231,17 +154,9 @@ void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   give_feature(pl, types, pl_corn, pl_surf);
 
   ros::Time ct(ros::Time::now());
-  pub_func(pl, pub_full, ct);
-  pub_func(pl_surf, pub_surf, ct);
-  pub_func(pl_corn, pub_corn, ct);
-
-  // printf("%u %u %u\n", pl.size(), pl_surf.size(), pl_corn.size());
-  // int a; cin >> a;
-  // if(a == 0)
-  // {
-  //   exit(0);
-  // }
-
+  pub_func(pl, pub_full, msg->header.stamp);
+  pub_func(pl_surf, pub_surf, msg->header.stamp);
+  pub_func(pl_corn, pub_corn, msg->header.stamp);
 }
 
 void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
@@ -296,13 +211,110 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
 
   ros::Time ct;
   ct.fromNSec(msg->timebase);
-  pub_func(pl_full, pub_full, ct);
-  pub_func(pl_surf, pub_surf, ct);
-  pub_func(pl_corn, pub_corn, ct);
+  pub_func(pl_full, pub_full, msg->header.stamp);
+  pub_func(pl_surf, pub_surf, msg->header.stamp);
+  pub_func(pl_corn, pub_corn, msg->header.stamp);
   std::cout<<"[~~~~~~~~~~~~ Feature Extract ]: time: "<< omp_get_wtime() - t1<<" "<<msg->header.stamp.toSec()<<std::endl;
 }
 
+int orders[16] = {0, 2, 4, 6, 8, 10, 12, 14, 1, 3, 5, 7, 9, 11, 13, 15};
+
 void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+{
+  pcl::PointCloud<PointType> pl_orig;
+  pcl::fromROSMsg(*msg, pl_orig);
+  uint plsize = pl_orig.size();
+
+  vector<pcl::PointCloud<PointType>> pl_buff(N_SCANS);
+  vector<vector<orgtype>> typess(N_SCANS);
+  pcl::PointCloud<PointType> pl_corn, pl_surf, pl_full;
+
+  int scanID;
+  int last_stat = -1;
+  int idx = 0;
+
+  for(int i=0; i<N_SCANS; i++)
+  {
+    pl_buff[i].resize(plsize);
+    typess[i].resize(plsize);
+  }
+
+  for(uint i=0; i<plsize; i++)
+  {
+    PointType &ap = pl_orig[i];
+    double leng = sqrt(ap.x*ap.x + ap.y*ap.y);
+    if(leng < blind)
+    {
+      continue;
+    }
+
+    double ang = atan(ap.z / leng)*rad2deg;
+    scanID = int((ang + 15) / 2 + 0.5);
+
+    if(scanID>=N_SCANS || scanID<0)
+    {
+      continue;
+    }
+
+    if(orders[scanID] <= last_stat)
+    {
+      idx++;
+    }
+
+    pl_buff[scanID][idx].x = ap.x;
+    pl_buff[scanID][idx].y = ap.y;
+    pl_buff[scanID][idx].z = ap.z;
+    pl_buff[scanID][idx].intensity = ap.intensity;
+    typess[scanID][idx].range = leng;
+    last_stat = orders[scanID];
+  }
+
+  // for(int i=0; i<N_SCANS; i++)
+  // {
+  //   pl_full += pl_buff[i];
+  //   pub_func(pl_buff[i], pub_full, msg->header.stamp);
+  // }
+  
+
+  // for(int i=0; i<10; i++)
+  // {
+  //   printf("%f %f %f\n", pl_buff[0][i].x, pl_buff[0][i].y, pl_buff[0][i].z);
+  // }
+  // cout << endl;
+
+  idx++;
+
+  for(int j=0; j<N_SCANS; j++)
+  {
+    pcl::PointCloud<PointType> &pl = pl_buff[j];
+    vector<orgtype> &types = typess[j];
+    pl.erase(pl.begin()+idx, pl.end());
+    types.erase(types.begin()+idx, types.end());
+    plsize = idx - 1;
+    for(uint i=0; i<plsize; i++)
+    {
+      // types[i].range = sqrt(pl[i].x*pl[i].x + pl[i].y*pl[i].y);
+      vx = pl[i].x - pl[i+1].x;
+      vy = pl[i].y - pl[i+1].y;
+      vz = pl[i].z - pl[i+1].z;
+      types[i].dista = vx*vx + vy*vy + vz*vz;
+    }
+
+    types[plsize].range = sqrt(pl[plsize].x*pl[plsize].x + pl[plsize].y*pl[plsize].y);
+    
+    give_feature(pl, types, pl_corn, pl_surf);
+  }
+
+  // printf("%zu %zu\n", pl_surf.size(), pl_corn.size());
+
+  pub_func(pl_orig, pub_full, msg->header.stamp);
+  pub_func(pl_surf, pub_surf, msg->header.stamp);
+  pub_func(pl_corn, pub_corn, msg->header.stamp);
+
+}
+
+
+void velo16_handler1(const sensor_msgs::PointCloud2::ConstPtr &msg)
 {
   pcl::PointCloud<PointType> pl_orig;
   pcl::fromROSMsg(*msg, pl_orig);
@@ -354,6 +366,41 @@ void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
       stat = 0;
     }
   }
+
+  // idx = 0;
+  // int last_stat = -1;
+  // for(uint i=0; i<plsize; i++)
+  // {
+  //   PointType &ap = pl_orig[i];
+  //   // pl_full.push_back(ap);
+  //   double leng = sqrt(ap.x*ap.x + ap.y*ap.y);
+  //   if(leng < blind)
+  //   {
+  //     continue;
+  //   }
+
+  //   double ang = atan(ap.z / leng)*rad2deg;
+  //   scanID = int((ang + 15) / 2 + 0.5);
+
+  //   if(scanID>=N_SCANS || scanID<0)
+  //   {
+  //     continue;
+  //   }
+
+  //   if(orders[scanID] <= last_stat)
+  //   {
+  //     idx++;
+  //   }
+
+  //   pl_buff[scanID][idx].x = ap.x;
+  //   pl_buff[scanID][idx].y = ap.y;
+  //   pl_buff[scanID][idx].z = ap.z;
+  //   pl_buff[scanID][idx].intensity = ap.intensity;
+
+  //   last_stat = orders[scanID];
+  // }
+
+
   idx++;
 
 
@@ -374,13 +421,13 @@ void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
     }
     types[plsize].range = sqrt(pl[plsize].x*pl[plsize].x + pl[plsize].y*pl[plsize].y);
     
+    
     give_feature(pl, types, pl_corn, pl_surf);
   }
 
-  ros::Time ct(ros::Time::now());
-  pub_func(pl_full, pub_full, ct);
-  pub_func(pl_surf, pub_surf, ct);
-  pub_func(pl_corn, pub_corn, ct);
+  pub_func(pl_full, pub_full, msg->header.stamp);
+  pub_func(pl_surf, pub_surf, msg->header.stamp);
+  pub_func(pl_corn, pub_corn, msg->header.stamp);
 }
 
 void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
@@ -398,7 +445,6 @@ void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   for(int i=0; i<N_SCANS; i++)
   {
     pl_buff[i].reserve(plsize);
-    // typess[i].reserve(plsize);
   }
 
   for(uint i=0; i<plsize; i+=N_SCANS)
@@ -427,10 +473,9 @@ void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
     give_feature(pl, types, pl_corn, pl_surf);
   }
 
-  ros::Time ct(ros::Time::now());
-  pub_func(pl_orig, pub_full, ct);
-  pub_func(pl_surf, pub_surf, ct);
-  pub_func(pl_corn, pub_corn, ct);
+  pub_func(pl_orig, pub_full, msg->header.stamp);
+  pub_func(pl_surf, pub_surf, msg->header.stamp);
+  pub_func(pl_corn, pub_corn, msg->header.stamp);
 }
 
 
@@ -444,12 +489,13 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
     return;
   }
   uint head = 0;
+
   while(types[head].range < blind)
   {
     head++;
   }
 
-  // 平面点检测
+  // Surf
   plsize2 = plsize - group_size;
 
   Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
@@ -457,7 +503,6 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
 
   uint i_nex, i2;
   uint last_i = 0; uint last_i_nex = 0;
-  // 0:上次状态无用 1:上次为平面组
   int last_state = 0;
   int plane_type;
 
@@ -511,12 +556,12 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
     {
       if(last_state == 1)
       {
-        uint i_nex_tem; // 临时变量
+        uint i_nex_tem;
         uint j;
         for(j=last_i+1; j<=last_i_nex; j++)
         {
           uint i_nex_tem2 = i_nex_tem;
-          Eigen::Vector3d curr_direct2; // curr_direct临时变量
+          Eigen::Vector3d curr_direct2;
 
           uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
 
@@ -561,100 +606,100 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
   }
 
   plsize2 = plsize - 3;
-  // for(uint i=head+3; i<plsize2; i++)
-  // {
-  //   if(types[i].range<blind || types[i].ftype>=Real_Plane)
-  //   {
-  //     continue;
-  //   }
+  for(uint i=head+3; i<plsize2; i++)
+  {
+    if(types[i].range<blind || types[i].ftype>=Real_Plane)
+    {
+      continue;
+    }
 
-  //   if(types[i-1].dista<1e-16 || types[i].dista<1e-16)
-  //   {
-  //     continue;
-  //   }
+    if(types[i-1].dista<1e-16 || types[i].dista<1e-16)
+    {
+      continue;
+    }
 
-  //   Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
-  //   Eigen::Vector3d vecs[2];
+    Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
+    Eigen::Vector3d vecs[2];
 
-  //   for(int j=0; j<2; j++)
-  //   {
-  //     int m = -1;
-  //     if(j == 1)
-  //     {
-  //       m = 1;
-  //     }
+    for(int j=0; j<2; j++)
+    {
+      int m = -1;
+      if(j == 1)
+      {
+        m = 1;
+      }
 
-  //     if(types[i+m].range < blind)
-  //     {
-  //       if(types[i].range > inf_bound)
-  //       {
-  //         types[i].edj[j] = Nr_inf;
-  //       }
-  //       else
-  //       {
-  //         types[i].edj[j] = Nr_blind;
-  //       }
-  //       continue;
-  //     }
+      if(types[i+m].range < blind)
+      {
+        if(types[i].range > inf_bound)
+        {
+          types[i].edj[j] = Nr_inf;
+        }
+        else
+        {
+          types[i].edj[j] = Nr_blind;
+        }
+        continue;
+      }
 
-  //     vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z);
-  //     vecs[j] = vecs[j] - vec_a;
+      vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z);
+      vecs[j] = vecs[j] - vec_a;
       
-  //     types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
-  //     if(types[i].angle[j] < jump_up_limit)
-  //     {
-  //       types[i].edj[j] = Nr_180;
-  //     }
-  //     else if(types[i].angle[j] > jump_down_limit)
-  //     {
-  //       types[i].edj[j] = Nr_zero;
-  //     }
-  //   }
+      types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
+      if(types[i].angle[j] < jump_up_limit)
+      {
+        types[i].edj[j] = Nr_180;
+      }
+      else if(types[i].angle[j] > jump_down_limit)
+      {
+        types[i].edj[j] = Nr_zero;
+      }
+    }
 
-  //   types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
-  //   if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista)
-  //   {
-  //     if(types[i].intersect > cos160)
-  //     {
-  //       if(edge_jump_judge(pl, types, i, Prev))
-  //       {
-  //         types[i].ftype = Edge_Jump;
-  //       }
-  //     }
-  //   }
-  //   else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista)
-  //   {
-  //     if(types[i].intersect > cos160)
-  //     {
-  //       if(edge_jump_judge(pl, types, i, Next))
-  //       {
-  //         types[i].ftype = Edge_Jump;
-  //       }
-  //     }
-  //   }
-  //   else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf)
-  //   {
-  //     if(edge_jump_judge(pl, types, i, Prev))
-  //     {
-  //       types[i].ftype = Edge_Jump;
-  //     }
-  //   }
-  //   else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor)
-  //   {
-  //     if(edge_jump_judge(pl, types, i, Next))
-  //     {
-  //       types[i].ftype = Edge_Jump;
-  //     }
+    types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
+    if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista)
+    {
+      if(types[i].intersect > cos160)
+      {
+        if(edge_jump_judge(pl, types, i, Prev))
+        {
+          types[i].ftype = Edge_Jump;
+        }
+      }
+    }
+    else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista)
+    {
+      if(types[i].intersect > cos160)
+      {
+        if(edge_jump_judge(pl, types, i, Next))
+        {
+          types[i].ftype = Edge_Jump;
+        }
+      }
+    }
+    else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf)
+    {
+      if(edge_jump_judge(pl, types, i, Prev))
+      {
+        types[i].ftype = Edge_Jump;
+      }
+    }
+    else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor)
+    {
+      if(edge_jump_judge(pl, types, i, Next))
+      {
+        types[i].ftype = Edge_Jump;
+      }
      
-  //   }
-  //   else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor)
-  //   {
-  //     if(types[i].ftype == Nor)
-  //     {
-  //       types[i].ftype = Wire;
-  //     }
-  //   }
-  // }
+    }
+    else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor)
+    {
+      if(types[i].ftype == Nor)
+      {
+        types[i].ftype = Wire;
+      }
+    }
+  }
 
   plsize2 = plsize-1;
   double ratio;
@@ -696,81 +741,84 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
     }
   }
 
-  int last_surface = 0;
-  for(uint i=0; i<plsize; i++)
-  {
-    if(types[i].ftype==Poss_Plane || types[i].ftype==Real_Plane)
-    {
-      if(last_surface == 0)
-      {
-        pl_surf.push_back(pl[i]);
-        last_surface = 1;
-      }
-      else
-      {
-        last_surface = 0;
-      }
-      
-    }
-    else if(types[i].ftype==Edge_Jump || types[i].ftype==Edge_Plane)
-    {
-      pl_corn.push_back(pl[i]);
-    }
-    
-  }  
 
-  // int last_surface = -1;
-  
-  // for(uint j=head; j<plsize; j++)
+  // int last_surface = 0;
+  // for(uint i=0; i<plsize; i++)
   // {
-  //   if(types[j].ftype==Poss_Plane || types[j].ftype==Real_Plane)
+  //   if(types[i].ftype==Poss_Plane || types[i].ftype==Real_Plane)
   //   {
-  //     if(last_surface == -1)
+  //     if(last_surface == 0)
   //     {
-  //       last_surface = j;
+  //       pl_surf.push_back(pl[i]);
+  //       last_surface = 1;
   //     }
-  //     else if(j == (last_surface+point_filter_num-1))
+  //     else
   //     {
-  //       PointType ap;
-  //       for(uint k=last_surface; k<=j; k++)
-  //       {
-  //         ap.x += pl[k].x;
-  //         ap.y += pl[k].y;
-  //         ap.z += pl[k].z;
-  //       }
-  //       ap.x /= point_filter_num;
-  //       ap.y /= point_filter_num;
-  //       ap.z /= point_filter_num;
-  //       pl_surf.push_back(ap);
-  //       // printf("%d-%d: %lf %lf %lf\n", last_surface, j, ap.x, ap.y, ap.z);
-  //       last_surface = -1;
+  //       last_surface = 0;
   //     }
+      
   //   }
-  //   else
+  //   else if(types[i].ftype==Edge_Jump || types[i].ftype==Edge_Plane)
   //   {
-  //     if(types[j].ftype==Edge_Jump || types[j].ftype==Edge_Plane)
-  //     {
-  //       pl_corn.push_back(pl[j]);
-  //     }
-  //     if(last_surface != -1)
-  //     {
-  //       PointType ap;
-  //       for(uint k=last_surface; k<j; k++)
-  //       {
-  //         ap.x += pl[k].x;
-  //         ap.y += pl[k].y;
-  //         ap.z += pl[k].z;
-  //       }
-  //       ap.x /= (j-last_surface);
-  //       ap.y /= (j-last_surface);
-  //       ap.z /= (j-last_surface);
-  //       pl_surf.push_back(ap);
-  //       // printf("%d-%d: %lf %lf %lf\n", last_surface, j-1, ap.x, ap.y, ap.z);
-  //     }
-  //     last_surface = -1;
+  //     pl_corn.push_back(pl[i]);
   //   }
+
   // }
 
+  int last_surface = -1;
+  for(uint j=head; j<plsize; j++)
+  {
+    if(types[j].ftype==Poss_Plane || types[j].ftype==Real_Plane)
+    {
+      if(last_surface == -1)
+      {
+        last_surface = j;
+      }
+      
+      if(j == uint(last_surface+point_filter_num-1))
+      {
+        PointType ap;
+        for(uint k=last_surface; k<=j; k++)
+        {
+          ap.x += pl[k].x;
+          ap.y += pl[k].y;
+          ap.z += pl[k].z;
+          ap.curvature += pl[k].curvature;
+        }
+        ap.x /= point_filter_num;
+        ap.y /= point_filter_num;
+        ap.z /= point_filter_num;
+        ap.curvature /= point_filter_num;
+        pl_surf.push_back(ap);
+        // printf("%d-%d: %lf %lf %lf\n", last_surface, j, ap.x, ap.y, ap.z);
+        last_surface = -1;
+      }
+    }
+    else
+    {
+      if(types[j].ftype==Edge_Jump || types[j].ftype==Edge_Plane)
+      {
+        pl_corn.push_back(pl[j]);
+      }
+      if(last_surface != -1)
+      {
+        PointType ap;
+        for(uint k=last_surface; k<j; k++)
+        {
+          ap.x += pl[k].x;
+          ap.y += pl[k].y;
+          ap.z += pl[k].z;
+          ap.curvature += pl[k].curvature;
+        }
+        ap.x /= (j-last_surface);
+        ap.y /= (j-last_surface);
+        ap.z /= (j-last_surface);
+        ap.curvature /= (j-last_surface);
+        pl_surf.push_back(ap);
+      }
+      last_surface = -1;
+    }
+  }
 
 }
 
@@ -936,153 +984,4 @@ bool edge_jump_judge(const pcl::PointCloud<PointType> &pl, vector<orgtype> &type
 }
 
 
-int plane_judge1(const pcl::PointCloud<PointType> &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct)
-{
-  double group_dis = 0.01*pl[i].x + 0.1;
-  group_dis = group_dis*group_dis;
-
-  double two_dis;
-  vector<double> disarr;
-  disarr.reserve(128);
-
-  for(i_nex=i; i_nex<i+group_size; i_nex++)
-  {
-    if(pl[i_nex].x < blind)
-    {
-      curr_direct = Eigen::Vector3d::Zero();
-      return 2;
-    }
-    disarr.push_back(types[i_nex].dista);
-  }
-  for(;;)
-  {
-    if(pl[i_nex].x < blind)
-    {
-      curr_direct = Eigen::Vector3d::Zero();
-      return 2;
-    }
-    
-    vx = pl[i_nex].x - pl[i].x;
-    vy = pl[i_nex].y - pl[i].y;
-    vz = pl[i_nex].z - pl[i].z;
-    two_dis = vx*vx + vy*vy + vz*vz;
-    if(two_dis >= group_dis)
-    {
-      break;
-    }
-    disarr.push_back(types[i_nex].dista);
-    i_nex++;
-  }
-  
-  
-  double leng_wid = 0;
-  double v1[3], v2[3];
-  for(uint j=i+1; j<i_nex; j++)
-  {
-    v1[0] = pl[j].x - pl[i].x;
-    v1[1] = pl[j].y - pl[i].y;
-    v1[2] = pl[j].z - pl[i].z;
-
-    v2[0] = v1[1]*vz - vy*v1[2];
-    v2[1] = v1[2]*vx - v1[0]*vz;
-    v2[2] = v1[0]*vy - vx*v1[1];
-
-    double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2];
-    if(lw > leng_wid)
-    {
-      leng_wid = lw;
-    }
-  }
-
-  // if((two_dis*two_dis/leng_wid) <= 225)
-  if((two_dis*two_dis/leng_wid) <= 400)
-  {
-    curr_direct = Eigen::Vector3d::Zero();
-    return 0;
-  }
-
-  // 寻找中位数(不是准确的中位数，有点偏差)
-  for(uint32_t j=0; j<disarr.size()-1; j++)
-  {
-    for(uint32_t k=j+1; k<disarr.size(); k++)
-    {
-      if(disarr[j] < disarr[k])
-      {
-        double a = disarr[j];
-        disarr[j] = disarr[k];
-        disarr[k] = a;
-      }
-    }
-  }
-
-  if(disarr[disarr.size()-2] < 1e-16)
-  {
-    // printf("!!!!two points are the same(at least two groups)\n");
-    // exit(0);
-    return 0;
-  }
-  
-  double dismax_mid = disarr[0]/disarr[disarr.size()/2];
-  double dismid_min = disarr[disarr.size()/2]/disarr[disarr.size()-2];
-
-  if(dismax_mid>=9 || dismid_min>=16)
-  {
-    curr_direct = Eigen::Vector3d::Zero();
-    return 0;
-  }
-
-  curr_direct << vx, vy, vz;
-  curr_direct.normalize();
-  return 1;
-}
-
-bool edge_jump_judge1(const pcl::PointCloud<PointType> &pl, vector<orgtype> &types, uint i, Surround nor_dir)
-{
-  // if(nor_dir == 0)
-  // {
-  //   if(pl[i-1].x<blind)
-  //   {
-  //     return true;
-  //   }
-  //   else if(pl[i-2].x<blind)
-  //   {
-  //     return false;
-  //   }
-  // }
-  // else if(nor_dir == 1)
-  // {
-  //   if(pl[i+1].x<blind)
-  //   {
-  //     return true;
-  //   }
-  //   else if(pl[i+2].x<blind)
-  //   {
-  //     return false;
-  //   }
-  // }
-  double d1 = types[i+nor_dir-1].dista;
-  double d2 = types[i+3*nor_dir-2].dista;
-  double d;
-
-  if(d1<d2)
-  {
-    d = d1;
-    d1 = d2;
-    d2 = d;
-  }
-
-  d1 = sqrt(d1);
-  d2 = sqrt(d2);
-
-  if(d1>0.05)
-  {
-    if(d1>3*d2 || (d1-d2)>0.05)
-    {
-      return false;
-    }
-  }
-  return true;
-}
-
-
 
diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp
index 474039e..59b4f9a 100644
--- a/src/laserMapping.cpp
+++ b/src/laserMapping.cpp
@@ -82,6 +82,7 @@ int laserCloudValidInd[250];
 int laserCloudSurroundInd[250];
 int laserCloudValidNum    = 0;
 int laserCloudSurroundNum = 0;
+int laserCloudSelNum      = 0;
 
 const int laserCloudWidth  = 42;
 const int laserCloudHeight = 22;
@@ -425,7 +426,6 @@ void lasermap_fov_segment()
                     
                     if (isInLaserFOV)
                     {
-                        
                         laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j
                                 + laserCloudWidth * laserCloudHeight * k;
                         laserCloudValidNum ++;
@@ -533,14 +533,12 @@ int main(int argc, char** argv)
 
     ros::Subscriber sub_pcl = nh.subscribe("/laser_cloud_flat", 20000, feat_points_cbk);
     ros::Subscriber sub_imu = nh.subscribe("/livox/imu", 20000, imu_cbk);
-    // ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>
-    //         ("/livox_undistort", 100, featsLastHandler);
     ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
             ("/cloud_registered", 100);
+    ros::Publisher pubLaserCloudEffect  = nh.advertise<sensor_msgs::PointCloud2>
+            ("/cloud_effected", 100);
     ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
             ("/Laser_map", 100);
-    // ros::Publisher pubSolvedPose6D = nh.advertise<fast_lio::States>
-    //         ("/States_updated", 100);
     ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> 
             ("/aft_mapped_to_init", 10);
 
@@ -711,7 +709,7 @@ int main(int argc, char** argv)
                         {
                             /** Find the closest surface/line in the map **/
                             kdtreeSurfFromMap->nearestKSearch(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf);
-                            if (pointSearchSqDis_surf[NUM_MATCH_POINTS - 1] < 3.0)
+                            if (pointSearchSqDis_surf[NUM_MATCH_POINTS - 1] < 3)
                             {
                                 point_selected_surf[i] = true;
                             }
@@ -773,13 +771,13 @@ int main(int argc, char** argv)
                             //if(fabs(pd2) > 0.1) continue;
                             float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel_tmpt.x * pointSel_tmpt.x + pointSel_tmpt.y * pointSel_tmpt.y + pointSel_tmpt.z * pointSel_tmpt.z));
 
-                            if (s > 0.1)
+                            if (s > 0.92)
                             {
                                 point_selected_surf[i] = true;
-                                coeffSel_tmpt->points[i].x = s * pa;
-                                coeffSel_tmpt->points[i].y = s * pb;
-                                coeffSel_tmpt->points[i].z = s * pc;
-                                coeffSel_tmpt->points[i].intensity = s * pd2;
+                                coeffSel_tmpt->points[i].x = pa;
+                                coeffSel_tmpt->points[i].y = pb;
+                                coeffSel_tmpt->points[i].z = pc;
+                                coeffSel_tmpt->points[i].intensity = pd2;
                             }
                             else
                             {
@@ -794,7 +792,7 @@ int main(int argc, char** argv)
                     for (int i = 0; i < coeffSel_tmpt->points.size(); i++)
                     {
                         float error_abs = std::abs(coeffSel_tmpt->points[i].intensity);
-                        if (point_selected_surf[i]  && (error_abs < 0.5))
+                        if (point_selected_surf[i])
                         {
                             laserCloudOri->push_back(feats_down->points[i]);
                             coeffSel->push_back(coeffSel_tmpt->points[i]);
@@ -802,7 +800,7 @@ int main(int argc, char** argv)
                         }
                     }
 
-                    int laserCloudSelNum = laserCloudOri->points.size();
+                    laserCloudSelNum = laserCloudOri->points.size();
                     double ave_residual = total_residual / laserCloudSelNum;
                     // ave_res_last 
 
@@ -886,14 +884,13 @@ int main(int argc, char** argv)
                     euler_cur = RotMtoEuler(state.rot_end);
 
                     #ifdef DEBUG_PRINT
-                    // std::cout<<"solution: "<<solution.transpose()<<std::endl;
                     std::cout<<"update: R"<<euler_cur.transpose()*57.3<<" p "<<state.pos_end.transpose()<<" v "<<state.vel_end.transpose()<<" bg"<<state.bias_g.transpose()<<" ba"<<state.bias_a.transpose()<<std::endl;
                     std::cout<<"dR & dT: "<<deltaR<<" "<<deltaT<<" res norm:"<<ave_residual<<std::endl;
                     #endif
 
                     /*** Rematch Judgement ***/
                     rematch_en = false;
-                    if ((deltaR < 0.07 && deltaT < 0.015))
+                    if ((deltaR < 0.01 && deltaT < 0.015) || ((rematch_num == 0) && (iterCount == (NUM_MAX_ITERATIONS - 2))))
                     {
                         rematch_en = true;
                         rematch_num ++;
@@ -907,7 +904,6 @@ int main(int argc, char** argv)
                             /*** Covariance Update ***/
                             G.block<DIM_OF_STATES,6>(0,0) = K * Hsub;
                             state.cov = (I_STATE - G) * state.cov;
-                            // std::cout<<"propagated cov: "<<state.cov.diagonal().transpose()<<std::endl;
                         }
                         solve_time += omp_get_wtime() - solve_start;
                         break;
@@ -951,15 +947,15 @@ int main(int argc, char** argv)
             }
             t3 = omp_get_wtime();
 
-            /******* Publish current frame points in world coordinates:  *******/            
+            /******* Publish current frame points in world coordinates:  *******/
             laserCloudFullRes2->clear();
             *laserCloudFullRes2 = dense_map_en ? (*feats_undistort) : (* feats_down);
-            // *laserCloudFullRes2 = dense_map_en ? (*laserCloudFullRes) : (* feats_down);
 
             int laserCloudFullResNum = laserCloudFullRes2->points.size();
-
+    
             pcl::PointXYZRGB temp_point;
             laserCloudFullResColor->clear();
+            {
             for (int i = 0; i < laserCloudFullResNum; i++)
             {
                 RGBpointBodyToWorld(&laserCloudFullRes2->points[i], &temp_point);
@@ -971,6 +967,23 @@ int main(int argc, char** argv)
             laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar);
             laserCloudFullRes3.header.frame_id = "/camera_init";
             pubLaserCloudFullRes.publish(laserCloudFullRes3);
+            }
+
+            /******* Publish Effective points *******/
+            // {
+            // laserCloudFullResColor->clear();
+            // pcl::PointXYZRGB temp_point;
+            // for (int i = 0; i < laserCloudSelNum; i++)
+            // {
+            //     RGBpointBodyToWorld(&laserCloudOri->points[i], &temp_point);
+            //     laserCloudFullResColor->push_back(temp_point);
+            // }
+            // sensor_msgs::PointCloud2 laserCloudFullRes3;
+            // pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3);
+            // laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar);
+            // laserCloudFullRes3.header.frame_id = "/camera_init";
+            // pubLaserCloudEffect.publish(laserCloudFullRes3);
+            // }
 
             /******* Publish Maps:  *******/
             // sensor_msgs::PointCloud2 laserCloudMap;