FAST-LIO 1 updated

include/common_lib.h

@@ -1,7 +1,7 @@
 #ifndef COMMON_LIB_H
 #define COMMON_LIB_H
 
-#include <Exp_mat.h>
+#include <so3_math.h>
 #include <Eigen/Eigen>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
@@ -14,6 +14,7 @@
 
 
 // #define DEBUG_PRINT
+// #define USE_ikdtree
 
 #define PI_M (3.14159265358)
 #define G_m_s2 (9.8099)         // Gravaty const in GuangDong/China
@@ -21,7 +22,7 @@
 #define DIM_OF_PROC_N (12)      // Dimension of process noise (Let Dim(SO(3)) = 3)
 #define CUBE_LEN  (6.0)
 #define LIDAR_SP_LEN    (2)
-#define INIT_COV  (0.0001)
+#define INIT_COV   (0.0001)
 
 #define VEC_FROM_ARRAY(v)        v[0],v[1],v[2]
 #define MAT_FROM_ARRAY(v)        v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
@@ -29,6 +30,7 @@
 #define ARRAY_FROM_EIGEN(mat)    mat.data(), mat.data() + mat.rows() * mat.cols()
 #define STD_VEC_FROM_EIGEN(mat)  std::vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
 
+
 #define DEBUG_FILE_DIR(name)  (std::string(std::string(ROOT_DIR) + "Log/"+ name))
 
 typedef fast_lio::Pose6D Pose6D;
@@ -65,9 +67,45 @@ struct StatesGroup
         this->cov     = Eigen::Matrix<double,DIM_OF_STATES,DIM_OF_STATES>::Identity() * INIT_COV;
 	};
 
+    StatesGroup(const StatesGroup& b) {
+		this->rot_end = b.rot_end;
+		this->pos_end = b.pos_end;
+        this->vel_end = b.vel_end;
+        this->bias_g  = b.bias_g;
+        this->bias_a  = b.bias_a;
+        this->gravity = b.gravity;
+        this->cov     = b.cov;
+	};
+
+    StatesGroup& operator=(const StatesGroup& b)
+	{
+        this->rot_end = b.rot_end;
+		this->pos_end = b.pos_end;
+        this->vel_end = b.vel_end;
+        this->bias_g  = b.bias_g;
+        this->bias_a  = b.bias_a;
+        this->gravity = b.gravity;
+        this->cov     = b.cov;
+        return *this;
+	};
+
+
+    StatesGroup operator+(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
+	{
+        StatesGroup a;
+		a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
+		a.pos_end = this->pos_end + state_add.block<3,1>(3,0);
+        a.vel_end = this->vel_end + state_add.block<3,1>(6,0);
+        a.bias_g  = this->bias_g  + state_add.block<3,1>(9,0);
+        a.bias_a  = this->bias_a  + state_add.block<3,1>(12,0);
+        a.gravity = this->gravity + state_add.block<3,1>(15,0);
+        a.cov     = this->cov;
+		return a;
+	};
+
     StatesGroup& operator+=(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
 	{
-		this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
+        this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
 		this->pos_end += state_add.block<3,1>(3,0);
         this->vel_end += state_add.block<3,1>(6,0);
         this->bias_g  += state_add.block<3,1>(9,0);
@@ -76,6 +114,19 @@ struct StatesGroup
 		return *this;
 	};
 
+    Eigen::Matrix<double, DIM_OF_STATES, 1> operator-(const StatesGroup& b)
+	{
+        Eigen::Matrix<double, DIM_OF_STATES, 1> a;
+        Eigen::Matrix3d rotd(b.rot_end.transpose() * this->rot_end);
+        a.block<3,1>(0,0)  = Log(rotd);
+        a.block<3,1>(3,0)  = this->pos_end - b.pos_end;
+        a.block<3,1>(6,0)  = this->vel_end - b.vel_end;
+        a.block<3,1>(9,0)  = this->bias_g  - b.bias_g;
+        a.block<3,1>(12,0) = this->bias_a  - b.bias_a;
+        a.block<3,1>(15,0) = this->gravity - b.gravity;
+		return a;
+	};
+
 	Eigen::Matrix3d rot_end;      // the estimated attitude (rotation matrix) at the end lidar point
     Eigen::Vector3d pos_end;      // the estimated position at the end lidar point (world frame)
     Eigen::Vector3d vel_end;      // the estimated velocity at the end lidar point (world frame)
@@ -115,26 +166,6 @@ auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &a, const Eigen::Ma
     return std::move(rot_kp);
 }
 
-template<typename T>
+
-Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
-{
-    T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
-    bool singular = sy < 1e-6;
-    T x, y, z;
-    if(!singular)
-    {
-        x = atan2(rot(2, 1), rot(2, 2));
-        y = atan2(-rot(2, 0), sy);   
-        z = atan2(rot(1, 0), rot(0, 0));  
-    }
-    else
-    {    
-        x = atan2(-rot(1, 2), rot(1, 1));    
-        y = atan2(-rot(2, 0), sy);    
-        z = 0;
-    }
-    Eigen::Matrix<T, 3, 1> ang(x, y, z);
-    return ang;
-}
 
 #endif

include/so3_math.h

@@ -0,0 +1,105 @@
+#ifndef SO3_MATH_H
+#define SO3_MATH_H
+
+#include <math.h>
+#include <Eigen/Core>
+#include <opencv/cv.h>
+// #include <common_lib.h>
+
+#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
+
+template<typename T>
+Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
+{
+    T ang_norm = ang.norm();
+    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
+    if (ang_norm > 0.0000001)
+    {
+        Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
+        Eigen::Matrix<T, 3, 3> K;
+        K << SKEW_SYM_MATRX(r_axis);
+        /// Roderigous Tranformation
+        return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
+    }
+    else
+    {
+        return Eye3;
+    }
+}
+
+template<typename T, typename Ts>
+Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
+{
+    T ang_vel_norm = ang_vel.norm();
+    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
+
+    if (ang_vel_norm > 0.0000001)
+    {
+        Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
+        Eigen::Matrix<T, 3, 3> K;
+
+        K << SKEW_SYM_MATRX(r_axis);
+
+        T r_ang = ang_vel_norm * dt;
+
+        /// Roderigous Tranformation
+        return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
+    }
+    else
+    {
+        return Eye3;
+    }
+}
+
+template<typename T>
+Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
+{
+    T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
+    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
+    if (norm > 0.00001)
+    {
+        T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
+        Eigen::Matrix<T, 3, 3> K;
+        K << SKEW_SYM_MATRX(r_ang);
+
+        /// Roderigous Tranformation
+        return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
+    }
+    else
+    {
+        return Eye3;
+    }
+}
+
+/* Logrithm of a Rotation Matrix */
+template<typename T>
+Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
+{
+    T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
+    Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
+    return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
+}
+
+template<typename T>
+Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
+{
+    T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
+    bool singular = sy < 1e-6;
+    T x, y, z;
+    if(!singular)
+    {
+        x = atan2(rot(2, 1), rot(2, 2));
+        y = atan2(-rot(2, 0), sy);   
+        z = atan2(rot(1, 0), rot(0, 0));  
+    }
+    else
+    {    
+        x = atan2(-rot(1, 2), rot(1, 1));    
+        y = atan2(-rot(2, 0), sy);    
+        z = 0;
+    }
+    Eigen::Matrix<T, 3, 1> ang(x, y, z);
+    return ang;
+}
+
+#endif

launch/mapping_avia.launch

@@ -20,12 +20,12 @@
 	<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="4"/>
+	<param name="point_filter_num" type="int" value="1"/>
   <node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
 
-  <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
+  <node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen" required="true">
   <param name="map_file_path" type="string" value=" " />
-  <param name="max_iteration" type="int" value="2" />
+  <param name="max_iteration" type="int" value="10" />
   <param name="dense_map_enable" type="bool" value="true" />
   <param name="fov_degree" type="double" value="75" />
   <param name="filter_size_corner" type="double" value="0.3" />

launch/mapping_avia_outdoor.launch

@@ -20,22 +20,22 @@
 	<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="3"/>
+	<param name="point_filter_num" type="int" value="1"/>
   <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="10" />
   <param name="dense_map_enable" type="bool" value="true" />
-  <param name="fov_degree" type="double" value="80" />
+  <param name="fov_degree" type="double" value="70" />
   <param name="filter_size_corner" type="double" value="0.5" />
   <param name="filter_size_surf" type="double" value="0.4" />
-  <param name="filter_size_map" type="double" value="0.5" />
+  <param name="filter_size_map" type="double" value="0.4" />
   <param name="cube_side_length" type="double" value="50" />
   </node>
 
-  <group if="$(arg rviz)">
+  <!-- <group if="$(arg rviz)">
     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
-  </group>
+  </group> -->
 
 </launch>

rviz_cfg/loam_livox.rviz

@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -11,6 +11,7 @@ Panels:
         - /mapping1/surround1
         - /mapping1/currPoints1
         - /mapping1/PointCloud21
+        - /mapping1/PointCloud22
         - /Debug1/PointCloud21
         - /Debug1/PointCloud22
         - /Debug1/PointCloud23
@@ -20,17 +21,14 @@ Panels:
         - /Odometry1/Odometry2/Shape1
         - /Odometry1/Odometry3
         - /Odometry1/Odometry3/Shape1
-        - /Features1
         - /Features1/PointCloud21
         - /Features1/PointCloud22
         - /Features1/PointCloud23
         - /Axes2
-        - /Odometry2
         - /Odometry2/Shape1
-        - /Marker1
+        - /Path1
-        - /PointStamped1
+      Splitter Ratio: 0.520588219165802
-      Splitter Ratio: 0.5
+    Tree Height: 839
-    Tree Height: 1066
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -57,7 +55,7 @@ Toolbars:
 Visualization Manager:
   Class: ""
   Displays:
-    - Alpha: 0.5
+    - Alpha: 1
       Cell Size: 1
       Class: rviz/Grid
       Color: 160; 160; 164
@@ -77,9 +75,9 @@ Visualization Manager:
       Value: false
     - Class: rviz/Axes
       Enabled: true
-      Length: 0.5
+      Length: 0.699999988079071
       Name: Axes
-      Radius: 0.10000000149011612
+      Radius: 0.05999999865889549
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz/Group
@@ -161,29 +159,6 @@ Visualization Manager:
       Name: odometry
     - Class: rviz/Group
       Displays:
-        - Alpha: 1
-          Buffer Length: 10000
-          Class: rviz/Path
-          Color: 255; 85; 0
-          Enabled: false
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: mappingPath
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic: /laser_odom_path
-          Unreliable: false
-          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -206,7 +181,7 @@ Visualization Manager:
           Position Transformer: XYZ
           Queue Size: 1
           Selectable: false
-          Size (Pixels): 4
+          Size (Pixels): 2
           Size (m): 0.05000000074505806
           Style: Points
           Topic: /cloud_registered
@@ -214,11 +189,11 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
-        - Alpha: 0.20000000298023224
+        - Alpha: 0.10000000149011612
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 0.32742905616760254
+            Max Value: 7.485495090484619
-            Min Value: -2.179872751235962
+            Min Value: -0.6337304711341858
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -245,6 +220,36 @@ Visualization Manager:
           Use rainbow: true
           Value: true
         - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 5.01795768737793
+            Min Value: -0.3809538185596466
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 239; 41; 41
+          Max Intensity: 0
+          Min Color: 239; 41; 41
+          Min Intensity: 0
+          Name: PointCloud2
+          Position Transformer: XYZ
+          Queue Size: 1
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.05999999865889549
+          Style: Flat Squares
+          Topic: /cloud_effected
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: false
+          Value: false
+        - Alpha: 0.800000011920929
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
             Max Value: 10
@@ -253,26 +258,26 @@ Visualization Manager:
           Axis: Z
           Channel Name: intensity
           Class: rviz/PointCloud2
-          Color: 255; 255; 255
+          Color: 239; 41; 41
-          Color Transformer: Intensity
+          Color Transformer: FlatColor
           Decay Time: 0
           Enabled: false
           Invert Rainbow: false
-          Max Color: 255; 0; 0
+          Max Color: 255; 255; 255
-          Max Intensity: 152
+          Max Intensity: 4096
-          Min Color: 255; 0; 0
+          Min Color: 0; 0; 0
           Min Intensity: 0
           Name: PointCloud2
           Position Transformer: XYZ
-          Queue Size: 10
+          Queue Size: 1
           Selectable: true
           Size (Pixels): 3
-          Size (m): 0.05000000074505806
+          Size (m): 0.20000000298023224
           Style: Flat Squares
           Topic: /Laser_map
           Unreliable: false
           Use Fixed Frame: true
-          Use rainbow: false
+          Use rainbow: true
           Value: false
       Enabled: true
       Name: mapping
@@ -500,7 +505,7 @@ Visualization Manager:
           Position Transformer: XYZ
           Queue Size: 1
           Selectable: true
-          Size (Pixels): 5
+          Size (Pixels): 7
           Size (m): 0.20000000298023224
           Style: Points
           Topic: /laser_cloud_flat
@@ -523,7 +528,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 78; 154; 6
-          Max Intensity: 155
+          Max Intensity: 0
           Min Color: 78; 154; 6
           Min Intensity: 0
           Name: PointCloud2
@@ -531,7 +536,7 @@ Visualization Manager:
           Queue Size: 10
           Selectable: true
           Size (Pixels): 3
-          Size (m): 0.05999999865889549
+          Size (m): 0.07999999821186066
           Style: Flat Squares
           Topic: /livox_undistort
           Unreliable: false
@@ -553,7 +558,7 @@ Visualization Manager:
           Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 160
+          Max Intensity: 155
           Min Color: 255; 255; 255
           Min Intensity: 0
           Name: PointCloud2
@@ -568,13 +573,13 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: false
           Value: false
-      Enabled: true
+      Enabled: false
       Name: Features
     - Class: rviz/Axes
       Enabled: true
-      Length: 1
+      Length: 0.699999988079071
       Name: Axes
-      Radius: 0.10000000149011612
+      Radius: 0.05999999865889549
       Reference Frame: aft_mapped
       Value: true
     - Angle Tolerance: 0.10000000149011612
@@ -594,7 +599,7 @@ Visualization Manager:
           Scale: 1
           Value: true
         Value: true
-      Enabled: true
+      Enabled: false
       Keep: 10000
       Name: Odometry
       Position Tolerance: 0.10000000149011612
@@ -610,25 +615,42 @@ Visualization Manager:
         Value: Arrow
       Topic: /aft_mapped_to_init
       Unreliable: false
-      Value: true
-    - Class: rviz/Marker
-      Enabled: false
-      Marker Topic: visualization_marker
-      Name: Marker
-      Namespaces:
-        {}
-      Queue Size: 100
       Value: false
-    - Alpha: 1
+    - Alpha: 0
-      Class: rviz/PointStamped
+      Buffer Length: 2
-      Color: 204; 41; 204
+      Class: rviz/Path
+      Color: 25; 255; 0
       Enabled: true
-      History Length: 1
+      Head Diameter: 0
-      Name: PointStamped
+      Head Length: 0
-      Radius: 0.20000000298023224
+      Length: 0.30000001192092896
-      Topic: /clicked_point
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 25; 255; 0
+      Pose Style: Arrows
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.15000000596046448
+      Shaft Length: 0.15000000596046448
+      Topic: /path
       Unreliable: false
       Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 0; 0; 0
@@ -657,33 +679,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 34.710601806640625
+      Distance: 49.2824821472168
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 6.49392557144165
+        X: 9.622546195983887
-        Y: -0.4694555699825287
+        Y: -18.686565399169922
-        Z: -1.3836920261383057
+        Z: -4.2439751625061035
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3497962951660156
+      Pitch: 0.8597958087921143
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.425689220428467
+      Yaw: 2.7072019577026367
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1383
+  Height: 1056
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000004b5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000618000001a800000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004df00000052fc0100000002fb0000000800540069006d00650100000000000004df000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000383000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000026d00000384fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000384000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001aa000001e20000001600fffffffb0000000a0049006d00610067006501000002730000027f00000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074900000052fc0100000002fb0000000800540069006d0065010000000000000749000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d60000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -692,6 +716,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1247
+  Width: 1865
-  X: 67
+  X: 55
-  Y: 27
+  Y: 24

src/IMU_Processing.hpp

@@ -6,7 +6,7 @@
 #include <fstream>
 #include <csignal>
 #include <ros/ros.h>
-#include <Exp_mat.h>
+#include <so3_math.h>
 #include <Eigen/Eigen>
 #include <common_lib.h>
 #include <pcl/common/io.h>

src/feature_extract.cpp

@@ -5,8 +5,12 @@
 
 // Feature will be updated in next version
 
-typedef pcl::PointXYZINormal PointType;
 using namespace std;
+
+#define IS_VALID(a)  ((abs(a)>1e8) ? true : false)
+
+typedef pcl::PointXYZINormal PointType;
+
 ros::Publisher pub_full, pub_surf, pub_corn;
 
 enum LID_TYPE{MID, HORIZON, VELO16, OUST64};
@@ -167,6 +171,7 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
   pcl::PointCloud<PointType> pl_full, pl_corn, pl_surf;
 
   uint plsize = msg->point_num;
+
   pl_corn.reserve(plsize); pl_surf.reserve(plsize);
   pl_full.resize(plsize);
 
@@ -175,19 +180,28 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
     pl_buff[i].reserve(plsize);
   }
   
-  for(uint i=0; i<plsize; i++)
+  for(uint i=1; i<plsize; i++)
   {
-    if(msg->points[i].line < N_SCANS)
+    if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)
+        && (!IS_VALID(msg->points[i].x)) && (!IS_VALID(msg->points[i].y)) && (!IS_VALID(msg->points[i].z)))
     {
       pl_full[i].x = msg->points[i].x;
       pl_full[i].y = msg->points[i].y;
       pl_full[i].z = msg->points[i].z;
       pl_full[i].intensity = msg->points[i].reflectivity;
       pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
-      pl_buff[msg->points[i].line].push_back(pl_full[i]);
+
+      if((std::abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
+          || (std::abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
+          || (std::abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
+      {
+        pl_buff[msg->points[i].line].push_back(pl_full[i]);
+      }
     }
   }
 
+  if(pl_buff[0].size() <= 7) {return;}
+
   for(int j=0; j<N_SCANS; j++)
   {
     pcl::PointCloud<PointType> &pl = pl_buff[j];
@@ -197,11 +211,13 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
     plsize--;
     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;
+      // std::cout<<vx<<" "<<vx<<" "<<vz<<" "<<std::endl;
     }
     // plsize++;
     types[plsize].range = sqrt(pl[plsize].x*pl[plsize].x + pl[plsize].y*pl[plsize].y);
@@ -400,10 +416,8 @@ void velo16_handler1(const sensor_msgs::PointCloud2::ConstPtr &msg)
   //   last_stat = orders[scanID];
   // }
 
-
   idx++;
 
-
   for(int j=0; j<N_SCANS; j++)
   {
     pcl::PointCloud<PointType> &pl = pl_buff[j];
@@ -496,12 +510,12 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
   }
 
   // Surf
-  plsize2 = plsize - group_size;
+  plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
 
   Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
   Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
 
-  uint i_nex, i2;
+  uint i_nex = 0, i2;
   uint last_i = 0; uint last_i_nex = 0;
   int last_state = 0;
   int plane_type;
@@ -514,6 +528,7 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
     }
     // i_nex = i; 
     i2 = i;
+    // std::cout<<" i: "<<i<<" i_nex "<<i_nex<<"group_size: "<<group_size<<" plsize "<<plsize<<" plsize2 "<<plsize2<<std::endl;
     plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
     
     if(plane_type == 1)
@@ -605,7 +620,7 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
 
   }
 
-  plsize2 = plsize - 3;
+  plsize2 = plsize > 3 ? plsize - 3 : 0;
   for(uint i=head+3; i<plsize2; i++)
   {
     if(types[i].range<blind || types[i].ftype>=Real_Plane)
@@ -777,20 +792,24 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
       
       if(j == uint(last_surface+point_filter_num-1))
       {
-        PointType ap;
         for(uint k=last_surface; k<=j; k++)
         {
-          ap.x += pl[k].x;
+          PointType ap;
-          ap.y += pl[k].y;
+          ap.x = pl[k].x;
-          ap.z += pl[k].z;
+          ap.y = pl[k].y;
-          ap.curvature += pl[k].curvature;
+          ap.z = pl[k].z;
+          ap.curvature = pl[k].curvature;
+          ap.intensity = pl[k].intensity;
+          pl_surf.push_back(ap);
         }
-        ap.x /= point_filter_num;
+
-        ap.y /= point_filter_num;
+        // PointType ap;
-        ap.z /= point_filter_num;
+        // ap.x = pl[last_surface].x;
-        ap.curvature /= point_filter_num;
+        // ap.y = pl[last_surface].y;
-        pl_surf.push_back(ap);
+        // ap.z = pl[last_surface].z;
-        // printf("%d-%d: %lf %lf %lf\n", last_surface, j, ap.x, ap.y, ap.z);
+        // ap.curvature += pl[last_surface].curvature;
+        // pl_surf.push_back(ap);
+
         last_surface = -1;
       }
     }
@@ -819,7 +838,6 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
       last_surface = -1;
     }
   }
-
 }
 
 void pub_func(pcl::PointCloud<PointType> &pl, ros::Publisher pub, const ros::Time &ct)
@@ -854,6 +872,8 @@ int plane_judge(const pcl::PointCloud<PointType> &pl, vector<orgtype> &types, ui
   
   for(;;)
   {
+    if((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
+
     if(types[i_nex].range < blind)
     {
       curr_direct.setZero();
@@ -875,6 +895,7 @@ int plane_judge(const pcl::PointCloud<PointType> &pl, vector<orgtype> &types, ui
   double v1[3], v2[3];
   for(uint j=i_cur+1; j<i_nex; j++)
   {
+    if((j >= pl.size()) || (i_cur >= pl.size())) break;
     v1[0] = pl[j].x - pl[i_cur].x;
     v1[1] = pl[j].y - pl[i_cur].y;
     v1[2] = pl[j].z - pl[i_cur].z;