From 487dcb01cb7449618dd4056339cfa86e3f84efd1 Mon Sep 17 00:00:00 2001
From: weiBUAA <buaaxw@gmail.com>
Date: Thu, 14 Jan 2021 15:33:31 +0800
Subject: [PATCH] FAST-LIO 1 updated
---
config/fast_lio.yaml | 0
include/common_lib.h | 79 ++-
include/so3_math.h | 105 ++++
launch/mapping_avia.launch | 6 +-
launch/mapping_avia_outdoor.launch | 10 +-
rviz_cfg/loam_livox.rviz | 182 +++---
src/IMU_Processing.hpp | 2 +-
src/feature_extract.cpp | 63 +-
src/laserMapping.cpp | 894 ++++++++++++++++++-----------
9 files changed, 875 insertions(+), 466 deletions(-)
create mode 100644 config/fast_lio.yaml
create mode 100644 include/so3_math.h
diff --git a/config/fast_lio.yaml b/config/fast_lio.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/include/common_lib.h b/include/common_lib.h
index 933e8ee..d5e92ac 100644
--- a/include/common_lib.h
+++ b/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
diff --git a/include/so3_math.h b/include/so3_math.h
new file mode 100644
index 0000000..9f538a2
--- /dev/null
+++ b/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
diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch
index 6f38467..ced1e84 100644
--- a/launch/mapping_avia.launch
+++ b/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" />
diff --git a/launch/mapping_avia_outdoor.launch b/launch/mapping_avia_outdoor.launch
index 2c1a223..a75e9e8 100644
--- a/launch/mapping_avia_outdoor.launch
+++ b/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>
diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz
index cd634ff..3eb8626 100644
--- a/rviz_cfg/loam_livox.rviz
+++ b/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
- - /PointStamped1
- Splitter Ratio: 0.5
- Tree Height: 1066
+ - /Path1
+ Splitter Ratio: 0.520588219165802
+ Tree Height: 839
- 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
- Min Value: -2.179872751235962
+ Max Value: 7.485495090484619
+ 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 Transformer: Intensity
+ Color: 239; 41; 41
+ Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
- Max Color: 255; 0; 0
- Max Intensity: 152
- Min Color: 255; 0; 0
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ 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
- Class: rviz/PointStamped
- Color: 204; 41; 204
+ - Alpha: 0
+ Buffer Length: 2
+ Class: rviz/Path
+ Color: 25; 255; 0
Enabled: true
- History Length: 1
- Name: PointStamped
- Radius: 0.20000000298023224
- Topic: /clicked_point
+ Head Diameter: 0
+ Head Length: 0
+ Length: 0.30000001192092896
+ 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
- Y: -0.4694555699825287
- Z: -1.3836920261383057
+ X: 9.622546195983887
+ Y: -18.686565399169922
+ 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
- X: 67
- Y: 27
+ Width: 1865
+ X: 55
+ Y: 24
diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp
index 63cc2c2..0bf2aab 100644
--- a/src/IMU_Processing.hpp
+++ b/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>
diff --git a/src/feature_extract.cpp b/src/feature_extract.cpp
index 44822c7..a589a3e 100644
--- a/src/feature_extract.cpp
+++ b/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;
- ap.y += pl[k].y;
- ap.z += pl[k].z;
- ap.curvature += pl[k].curvature;
+ PointType ap;
+ ap.x = pl[k].x;
+ ap.y = pl[k].y;
+ 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;
- 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);
+
+ // PointType ap;
+ // ap.x = pl[last_surface].x;
+ // ap.y = pl[last_surface].y;
+ // ap.z = pl[last_surface].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;
diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp
index 8410317..7d8571e 100644
--- a/src/laserMapping.cpp
+++ b/src/laserMapping.cpp
@@ -40,13 +40,15 @@
#include <csignal>
#include <unistd.h>
#include <Python.h>
-#include <Exp_mat.h>
+#include <so3_math.h>
#include <ros/ros.h>
#include <Eigen/Core>
#include <opencv2/core.hpp>
#include <common_lib.h>
+
#include "IMU_Processing.hpp"
#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
#include <opencv2/core/eigen.hpp>
#include <visualization_msgs/Marker.h>
#include <pcl_conversions/pcl_conversions.h>
@@ -62,12 +64,12 @@
#include <geometry_msgs/Vector3.h>
-#ifndef DEPLOY
+#ifdef DEPLOY
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
#endif
-#define INIT_TIME (1.0)
+#define INIT_TIME (0)
#define LASER_POINT_COV (0.0015)
#define NUM_MATCH_POINTS (5)
@@ -75,27 +77,28 @@ std::string root_dir = ROOT_DIR;
int iterCount = 0;
int NUM_MAX_ITERATIONS = 0;
-int laserCloudCenWidth = 20;
-int laserCloudCenHeight = 10;
-int laserCloudCenDepth = 20;
-int laserCloudValidInd[250];
-int laserCloudSurroundInd[250];
+int FOV_RANGE = 3; // range of FOV = FOV_RANGE * cube_len
+int laserCloudCenWidth = 24;
+int laserCloudCenHeight = 24;
+int laserCloudCenDepth = 24;
+
int laserCloudValidNum = 0;
-int laserCloudSurroundNum = 0;
int laserCloudSelNum = 0;
-const int laserCloudWidth = 42;
-const int laserCloudHeight = 22;
-const int laserCloudDepth = 42;
-const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851
+const int laserCloudWidth = 48;
+const int laserCloudHeight = 48;
+const int laserCloudDepth = 48;
+const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;
+
+std::vector<double> T1, T2, s_plot, s_plot2, s_plot3, s_plot4, s_plot5, s_plot6;
/// IMU relative variables
std::mutex mtx_buffer;
std::condition_variable sig_buffer;
bool lidar_pushed = false;
-bool b_exit = false;
-bool b_reset = false;
-bool b_first = true;
+bool flg_exit = false;
+bool flg_reset = false;
+bool flg_map_inited = false;
/// Buffers for measurements
double cube_len = 0.0;
@@ -103,20 +106,36 @@ double lidar_end_time = 0.0;
double last_timestamp_lidar = -1;
double last_timestamp_imu = -1;
double HALF_FOV_COS = 0.0;
+double res_mean_last = 0.05;
+double total_distance = 0.0;
+auto position_last = Zero3d;
+double copy_time, readd_time;
std::deque<sensor_msgs::PointCloud2::ConstPtr> lidar_buffer;
std::deque<sensor_msgs::Imu::ConstPtr> imu_buffer;
//surf feature in map
PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());
-
-std::deque< fast_lio::States > rot_kp_imu_buff;
+PointCloudXYZI::Ptr cube_points_add(new PointCloudXYZI());
//all points
PointCloudXYZI::Ptr laserCloudFullRes2(new PointCloudXYZI());
PointCloudXYZI::Ptr featsArray[laserCloudNum];
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserCloudFullResColor(new pcl::PointCloud<pcl::PointXYZRGB>());
+bool _last_inFOV[laserCloudNum];
+bool cube_updated[laserCloudNum];
+int laserCloudValidInd[laserCloudNum];
+pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudFullResColor(new pcl::PointCloud<pcl::PointXYZI>());
+
+#ifdef USE_ikdtree
+KD_TREE ikdtree;
+std::vector<BoxPointType> cub_needrm;
+std::vector<BoxPointType> cub_needad;
+#else
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());
+#endif
+
+Eigen::Vector3f XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0);
+Eigen::Vector3f XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0);
//estimator inputs and output;
MeasureGroup Measures;
@@ -124,7 +143,7 @@ StatesGroup state;
void SigHandle(int sig)
{
- b_exit = true;
+ flg_exit = true;
ROS_WARN("catch sig %d", sig);
sig_buffer.notify_all();
}
@@ -141,7 +160,17 @@ void pointBodyToWorld(PointType const * const pi, PointType * const po)
po->intensity = pi->intensity;
}
-void RGBpointBodyToWorld(PointType const * const pi, pcl::PointXYZRGB * const po)
+template<typename T>
+void pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi, Eigen::Matrix<T, 3, 1> &po)
+{
+ Eigen::Vector3d p_body(pi[0], pi[1], pi[2]);
+ Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end);
+ po[0] = p_global(0);
+ po[1] = p_global(1);
+ po[2] = p_global(2);
+}
+
+void RGBpointBodyToWorld(PointType const * const pi, pcl::PointXYZI * const po)
{
Eigen::Vector3d p_body(pi->x, pi->y, pi->z);
Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end);
@@ -149,55 +178,85 @@ void RGBpointBodyToWorld(PointType const * const pi, pcl::PointXYZRGB * const po
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
- //po->intensity = pi->intensity;
+ po->intensity = pi->intensity;
float intensity = pi->intensity;
intensity = intensity - std::floor(intensity);
int reflection_map = intensity*10000;
- //std::cout<<"DEBUG reflection_map "<<reflection_map<<std::endl;
+ // //std::cout<<"DEBUG reflection_map "<<reflection_map<<std::endl;
- if (reflection_map < 30)
- {
- int green = (reflection_map * 255 / 30);
- po->r = 0;
- po->g = green & 0xff;
- po->b = 0xff;
- }
- else if (reflection_map < 90)
- {
- int blue = (((90 - reflection_map) * 255) / 60);
- po->r = 0x0;
- po->g = 0xff;
- po->b = blue & 0xff;
- }
- else if (reflection_map < 150)
- {
- int red = ((reflection_map-90) * 255 / 60);
- po->r = red & 0xff;
- po->g = 0xff;
- po->b = 0x0;
- }
- else
- {
- int green = (((255-reflection_map) * 255) / (255-150));
- po->r = 0xff;
- po->g = green & 0xff;
- po->b = 0;
- }
+ // if (reflection_map < 30)
+ // {
+ // int green = (reflection_map * 255 / 30);
+ // po->r = 0;
+ // po->g = green & 0xff;
+ // po->b = 0xff;
+ // }
+ // else if (reflection_map < 90)
+ // {
+ // int blue = (((90 - reflection_map) * 255) / 60);
+ // po->r = 0x0;
+ // po->g = 0xff;
+ // po->b = blue & 0xff;
+ // }
+ // else if (reflection_map < 150)
+ // {
+ // int red = ((reflection_map-90) * 255 / 60);
+ // po->r = red & 0xff;
+ // po->g = 0xff;
+ // po->b = 0x0;
+ // }
+ // else
+ // {
+ // int green = (((255-reflection_map) * 255) / (255-150));
+ // po->r = 0xff;
+ // po->g = green & 0xff;
+ // po->b = 0;
+ // }
+}
+
+int cube_ind(const int &i, const int &j, const int &k)
+{
+ return (i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k);
+}
+
+bool CenterinFOV(Eigen::Vector3f cube_p)
+{
+ Eigen::Vector3f dis_vec = state.pos_end.cast<float>() - cube_p;
+ float squaredSide1 = dis_vec.transpose() * dis_vec;
+
+ if(squaredSide1 < 0.4 * cube_len * cube_len) return true;
+
+ dis_vec = XAxisPoint_world.cast<float>() - cube_p;
+ float squaredSide2 = dis_vec.transpose() * dis_vec;
+
+ float ang_cos = fabs(squaredSide1 <= 3) ? 1.0 :
+ (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1));
+
+ return ((ang_cos > HALF_FOV_COS)? true : false);
+}
+
+bool CornerinFOV(Eigen::Vector3f cube_p)
+{
+ Eigen::Vector3f dis_vec = state.pos_end.cast<float>() - cube_p;
+ float squaredSide1 = dis_vec.transpose() * dis_vec;
+
+ dis_vec = XAxisPoint_world.cast<float>() - cube_p;
+ float squaredSide2 = dis_vec.transpose() * dis_vec;
+
+ float ang_cos = fabs(squaredSide1 <= 3) ? 1.0 :
+ (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1));
+
+ return ((ang_cos > HALF_FOV_COS)? true : false);
}
void lasermap_fov_segment()
{
- laserCloudValidNum = 0;
- laserCloudSurroundNum = 0;
- PointType pointOnYAxis;
- pointOnYAxis.x = LIDAR_SP_LEN;
- pointOnYAxis.y = 0.0;
- pointOnYAxis.z = 0.0;
- pointBodyToWorld(&pointOnYAxis, &pointOnYAxis);
- // std::cout<<"special point:"<<pointOnYAxis.x<<" "<<pointOnYAxis.y<<" "<<pointOnYAxis.z<<" "<<std::endl;
+ laserCloudValidNum = 0;
+
+ pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);
int centerCubeI = int((state.pos_end(0) + 0.5 * cube_len) / cube_len) + laserCloudCenWidth;
int centerCubeJ = int((state.pos_end(1) + 0.5 * cube_len) / cube_len) + laserCloudCenHeight;
@@ -207,7 +266,15 @@ void lasermap_fov_segment()
if (state.pos_end(1) + 0.5 * cube_len < 0) centerCubeJ--;
if (state.pos_end(2) + 0.5 * cube_len < 0) centerCubeK--;
- while (centerCubeI < 3)
+ bool last_inFOV_flag = 0;
+ int cube_index = 0;
+
+ T2.push_back(Measures.lidar_beg_time);
+ double t_begin = omp_get_wtime();
+
+ std::cout<<"centerCubeIJK: "<<centerCubeI<<" "<<centerCubeJ<<" "<<centerCubeK<<std::endl;
+
+ while (centerCubeI < FOV_RANGE + 1)
{
for (int j = 0; j < laserCloudHeight; j++)
{
@@ -215,15 +282,16 @@ void lasermap_fov_segment()
{
int i = laserCloudWidth - 1;
- PointCloudXYZI::Ptr laserCloudCubeSurfPointer =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
+ PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)];
+ last_inFOV_flag = _last_inFOV[cube_index];
for (; i >= 1; i--) {
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- featsArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
+ featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i-1, j, k)];
+ _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i-1, j, k)];
}
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- laserCloudCubeSurfPointer;
+
+ featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer;
+ _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag;
laserCloudCubeSurfPointer->clear();
}
}
@@ -231,21 +299,21 @@ void lasermap_fov_segment()
laserCloudCenWidth++;
}
- while (centerCubeI >= laserCloudWidth - 3) {
+ while (centerCubeI >= laserCloudWidth - (FOV_RANGE + 1)) {
for (int j = 0; j < laserCloudHeight; j++) {
for (int k = 0; k < laserCloudDepth; k++) {
int i = 0;
- PointCloudXYZI::Ptr laserCloudCubeSurfPointer =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
- for (; i < laserCloudWidth - 1; i++)
- {
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- featsArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
+ PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)];
+ last_inFOV_flag = _last_inFOV[cube_index];
+
+ for (; i >= 1; i--) {
+ featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i+1, j, k)];
+ _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i+1, j, k)];
}
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- laserCloudCubeSurfPointer;
+ featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer;
+ _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag;
laserCloudCubeSurfPointer->clear();
}
}
@@ -254,19 +322,21 @@ void lasermap_fov_segment()
laserCloudCenWidth--;
}
- while (centerCubeJ < 3) {
+ while (centerCubeJ < (FOV_RANGE + 1)) {
for (int i = 0; i < laserCloudWidth; i++) {
for (int k = 0; k < laserCloudDepth; k++) {
int j = laserCloudHeight - 1;
- PointCloudXYZI::Ptr laserCloudCubeSurfPointer =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
- for (; j >= 1; j--) {
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- featsArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k];
+ PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)];
+ last_inFOV_flag = _last_inFOV[cube_index];
+
+ for (; i >= 1; i--) {
+ featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j-1, k)];
+ _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j-1, k)];
}
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- laserCloudCubeSurfPointer;
+
+ featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer;
+ _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag;
laserCloudCubeSurfPointer->clear();
}
}
@@ -275,19 +345,20 @@ void lasermap_fov_segment()
laserCloudCenHeight++;
}
- while (centerCubeJ >= laserCloudHeight - 3) {
+ while (centerCubeJ >= laserCloudHeight - (FOV_RANGE + 1)) {
for (int i = 0; i < laserCloudWidth; i++) {
for (int k = 0; k < laserCloudDepth; k++) {
int j = 0;
- PointCloudXYZI::Ptr laserCloudCubeSurfPointer =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
+ PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)];
+ last_inFOV_flag = _last_inFOV[cube_index];
- for (; j < laserCloudHeight - 1; j++) {
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- featsArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k];
+ for (; i >= 1; i--) {
+ featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j+1, k)];
+ _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j+1, k)];
}
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- laserCloudCubeSurfPointer;
+
+ featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer;
+ _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag;
laserCloudCubeSurfPointer->clear();
}
}
@@ -296,19 +367,20 @@ void lasermap_fov_segment()
laserCloudCenHeight--;
}
- while (centerCubeK < 3) {
+ while (centerCubeK < (FOV_RANGE + 1)) {
for (int i = 0; i < laserCloudWidth; i++) {
for (int j = 0; j < laserCloudHeight; j++) {
int k = laserCloudDepth - 1;
- PointCloudXYZI::Ptr laserCloudCubeSurfPointer =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
+ PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)];
+ last_inFOV_flag = _last_inFOV[cube_index];
- for (; k >= 1; k--) {
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)];
+ for (; i >= 1; i--) {
+ featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j, k-1)];
+ _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j, k-1)];
}
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- laserCloudCubeSurfPointer;
+
+ featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer;
+ _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag;
laserCloudCubeSurfPointer->clear();
}
}
@@ -317,24 +389,23 @@ void lasermap_fov_segment()
laserCloudCenDepth++;
}
- while (centerCubeK >= laserCloudDepth - 3)
+ while (centerCubeK >= laserCloudDepth - (FOV_RANGE + 1))
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
- PointCloudXYZI::Ptr laserCloudCubeSurfPointer =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
-
- for (; k < laserCloudDepth - 1; k++)
- {
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)];
+ PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)];
+ last_inFOV_flag = _last_inFOV[cube_index];
+
+ for (; i >= 1; i--) {
+ featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j, k+1)];
+ _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j, k+1)];
}
- featsArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
- laserCloudCubeSurfPointer;
+ featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer;
+ _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag;
laserCloudCubeSurfPointer->clear();
}
}
@@ -343,108 +414,129 @@ void lasermap_fov_segment()
laserCloudCenDepth--;
}
- for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
+ cube_points_add->clear();
+ featsFromMap->clear();
+ bool now_inFOV[laserCloudNum] = {false};
+
+ // std::cout<<"centerCubeIJK: "<<centerCubeI<<" "<<centerCubeJ<<" "<<centerCubeK<<std::endl;
+ // std::cout<<"laserCloudCen: "<<laserCloudCenWidth<<" "<<laserCloudCenHeight<<" "<<laserCloudCenDepth<<std::endl;
+
+ for (int i = centerCubeI - FOV_RANGE; i <= centerCubeI + FOV_RANGE; i++)
{
- for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
+ for (int j = centerCubeJ - FOV_RANGE; j <= centerCubeJ + FOV_RANGE; j++)
{
- for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++)
+ for (int k = centerCubeK - FOV_RANGE; k <= centerCubeK + FOV_RANGE; k++)
{
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)
{
-
- float centerX = cube_len * (i - laserCloudCenWidth);
- float centerY = cube_len * (j - laserCloudCenHeight);
- float centerZ = cube_len * (k - laserCloudCenDepth);
+ Eigen::Vector3f center_p(cube_len * (i - laserCloudCenWidth), \
+ cube_len * (j - laserCloudCenHeight), \
+ cube_len * (k - laserCloudCenDepth));
float check1, check2;
float squaredSide1, squaredSide2;
float ang_cos = 1;
+ bool &last_inFOV = _last_inFOV[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
+ bool inFOV = CenterinFOV(center_p);
- bool isInLaserFOV = false;
-
- for (int ii = -1; ii <= 1; ii += 2)
+ for (int ii = -1; (ii <= 1) && (!inFOV); ii += 2)
{
- for (int jj = -1; jj <= 1; jj += 2)
+ for (int jj = -1; (jj <= 1) && (!inFOV); jj += 2)
{
- for (int kk = -1; (kk <= 1) && (!isInLaserFOV); kk += 2)
+ for (int kk = -1; (kk <= 1) && (!inFOV); kk += 2)
{
-
- float cornerX = centerX + 0.5 * cube_len * ii;
- float cornerY = centerY + 0.5 * cube_len * jj;
- float cornerZ = centerZ + 0.5 * cube_len * kk;
-
- squaredSide1 = (state.pos_end(0) - cornerX)
- * (state.pos_end(0) - cornerX)
- + (state.pos_end(1) - cornerY)
- * (state.pos_end(1) - cornerY)
- + (state.pos_end(2) - cornerZ)
- * (state.pos_end(2) - cornerZ);
-
- squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX)
- + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
- + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);
-
- ang_cos = fabs(squaredSide1 <= 3) ? 1.0 :
- (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1));
+ Eigen::Vector3f corner_p(cube_len * ii, cube_len * jj, cube_len * kk);
+ corner_p = center_p + 0.5 * corner_p;
- if(ang_cos > HALF_FOV_COS) isInLaserFOV = true;
+ inFOV = CornerinFOV(corner_p);
}
}
}
-
- if(!isInLaserFOV)
+
+ now_inFOV[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = inFOV;
+
+ #ifdef USE_ikdtree
+ /*** readd cubes and points ***/
+ if (inFOV)
{
- float cornerX = centerX;
- float cornerY = centerY;
- float cornerZ = centerZ;
-
- squaredSide1 = (state.pos_end(0) - cornerX)
- * (state.pos_end(0) - cornerX)
- + (state.pos_end(1) - cornerY)
- * (state.pos_end(1) - cornerY)
- + (state.pos_end(2) - cornerZ)
- * (state.pos_end(2) - cornerZ);
-
- if(squaredSide1 <= 0.4 * cube_len * cube_len)
+ int center_index = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
+ *cube_points_add += *featsArray[center_index];
+ featsArray[center_index]->clear();
+ if (!last_inFOV)
{
- isInLaserFOV = true;
+ BoxPointType cub_points;
+ for(int i = 0; i < 3; i++)
+ {
+ cub_points.vertex_max[i] = center_p[i] + 0.5 * cube_len;
+ cub_points.vertex_min[i] = center_p[i] - 0.5 * cube_len;
+ }
+ cub_needad.push_back(cub_points);
+ laserCloudValidInd[laserCloudValidNum] = center_index;
+ laserCloudValidNum ++;
+ // std::cout<<"readd center: "<<center_p.transpose()<<std::endl;
}
-
- squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX)
- + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
- + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);
-
- ang_cos = fabs(squaredSide2 <= 0.5 * cube_len) ? 1.0 :
- (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1));
-
- if(ang_cos > HALF_FOV_COS) isInLaserFOV = true;
}
- // std::cout<<"cent point: "<<centerX<<" "<<centerY<<" "<<centerZ<<" infov? "<<isInLaserFOV<<std::endl;
-
- if (isInLaserFOV)
+ #else
+ if (inFOV)
{
- laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j
- + laserCloudWidth * laserCloudHeight * k;
- laserCloudValidNum ++;
+ int center_index = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
+ *featsFromMap += *featsArray[center_index];
+ laserCloudValidInd[laserCloudValidNum] = center_index;
+ laserCloudValidNum++;
}
- laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j
- + laserCloudWidth * laserCloudHeight * k;
- laserCloudSurroundNum ++;
-
+ last_inFOV = inFOV;
+ #endif
}
}
}
}
- featsFromMap->clear();
-
- for (int i = 0; i < laserCloudValidNum; i++)
+ #ifdef USE_ikdtree
+ cub_needrm.clear();
+ cub_needad.clear();
+ /*** delete cubes ***/
+ for (int i = 0; i < laserCloudWidth; i++)
{
- *featsFromMap += *featsArray[laserCloudValidInd[i]];
+ for (int j = 0; j < laserCloudHeight; j++)
+ {
+ for (int k = 0; k < laserCloudDepth; k++)
+ {
+ int ind = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
+ if((!now_inFOV[ind]) && _last_inFOV[ind])
+ {
+ BoxPointType cub_points;
+ Eigen::Vector3f center_p(cube_len * (i - laserCloudCenWidth),\
+ cube_len * (j - laserCloudCenHeight),\
+ cube_len * (k - laserCloudCenDepth));
+ // std::cout<<"center_p: "<<center_p.transpose()<<std::endl;
+
+ for(int i = 0; i < 3; i++)
+ {
+ cub_points.vertex_max[i] = center_p[i] + 0.5 * cube_len;
+ cub_points.vertex_min[i] = center_p[i] - 0.5 * cube_len;
+ }
+ cub_needrm.push_back(cub_points);
+ }
+ _last_inFOV[ind] = now_inFOV[ind];
+ }
+ }
}
+ #endif
+
+ copy_time = omp_get_wtime() - t_begin;
+
+#ifdef USE_ikdtree
+ if(cub_needrm.size() > 0) ikdtree.Delete_Point_Boxes(cub_needrm);
+ // s_plot4.push_back(omp_get_wtime() - t_begin); t_begin = omp_get_wtime();
+ if(cub_needad.size() > 0) ikdtree.Add_Point_Boxes(cub_needad);
+ // s_plot5.push_back(omp_get_wtime() - t_begin); t_begin = omp_get_wtime();
+ if(cube_points_add->points.size() > 0) ikdtree.Add_Points(cube_points_add->points, true);
+#endif
+ s_plot6.push_back(omp_get_wtime() - t_begin);
+ readd_time = omp_get_wtime() - t_begin - copy_time;
}
void feat_points_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
@@ -477,8 +569,7 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
{
ROS_ERROR("imu loop back, clear buffer");
imu_buffer.clear();
- b_reset = true;
- b_first = true;
+ flg_reset = true;
}
last_timestamp_imu = timestamp;
@@ -510,18 +601,20 @@ bool sync_packages(MeasureGroup &meas)
return false;
}
- /*** push imu data, and pop from buffer ***/
+ /*** push imu data, and pop from imu buffer ***/
double imu_time = imu_buffer.front()->header.stamp.toSec();
meas.imu.clear();
while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
{
imu_time = imu_buffer.front()->header.stamp.toSec();
+ if(imu_time > lidar_end_time + 0.02) break;
meas.imu.push_back(imu_buffer.front());
imu_buffer.pop_front();
}
lidar_buffer.pop_front();
lidar_pushed = false;
+ // if (meas.imu.empty()) return false;
// std::cout<<"[IMU Sycned]: "<<imu_time<<" "<<lidar_end_time<<std::endl;
return true;
}
@@ -541,14 +634,18 @@ int main(int argc, char** argv)
("/Laser_map", 100);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 10);
-
+ ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
+ ("/path", 10);
#ifdef DEPLOY
ros::Publisher mavros_pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
- geometry_msgs::PoseStamped msg_body_pose;
#endif
-
+ geometry_msgs::PoseStamped msg_body_pose;
+ nav_msgs::Path path;
+ path.header.stamp = ros::Time::now();
+ path.header.frame_id ="/camera_init";
+
/*** variables definition ***/
- bool dense_map_en, Need_Init = true;
+ bool dense_map_en, flg_EKF_inited = 0, flg_map_inited = 0, flg_EKF_converged = 0;
std::string map_file_path;
int effect_feat_num = 0, frame_num = 0;
double filter_size_corner_min, filter_size_surf_min, filter_size_map_min, fov_deg,\
@@ -603,7 +700,6 @@ int main(int argc, char** argv)
std::cout << "~~~~"<<ROOT_DIR<<" file opened" << std::endl;
else
std::cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << std::endl;
- std::vector<double> T1, s_plot, s_plot2, s_plot3;
//------------------------------------------------------------------------------------------------------
signal(SIGINT, SigHandle);
@@ -611,26 +707,27 @@ int main(int argc, char** argv)
bool status = ros::ok();
while (status)
{
- if (b_exit) break;
+ if (flg_exit) break;
ros::spinOnce();
while(sync_packages(Measures))
{
- if (b_reset)
+ if (flg_reset)
{
ROS_WARN("reset when rosbag play back");
p_imu->Reset();
- b_reset = false;
+ flg_reset = false;
continue;
}
- double t1,t2,t3,t4,match_start, match_time, solve_start, solve_time, pca_time, svd_time;
+ double t0,t1,t2,t3,t4,t5,match_start, match_time, solve_start, solve_time, pca_time, svd_time;
match_time = 0;
solve_time = 0;
pca_time = 0;
svd_time = 0;
- t1 = omp_get_wtime();
+ t0 = omp_get_wtime();
p_imu->Process(Measures, state, feats_undistort);
+ StatesGroup state_propagat(state);
if (feats_undistort->empty() || (feats_undistort == NULL))
{
@@ -641,12 +738,12 @@ int main(int argc, char** argv)
if ((Measures.lidar_beg_time - first_lidar_time) < INIT_TIME)
{
- Need_Init = true;
+ flg_EKF_inited = false;
std::cout<<"||||||||||Initiallizing LiDar||||||||||"<<std::endl;
}
else
{
- Need_Init = false;
+ flg_EKF_inited = true;
}
/*** Compute the euler angle ***/
@@ -661,29 +758,67 @@ int main(int argc, char** argv)
/*** Segment the map in lidar FOV ***/
lasermap_fov_segment();
- /*** downsample the features and maps ***/
+ /*** downsample the features of new frame ***/
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down);
- downSizeFilterMap.setInputCloud(featsFromMap);
- downSizeFilterMap.filter(*featsFromMap);
+ #ifdef USE_ikdtree
+ /*** initialize the map kdtree ***/
+ if((feats_down->points.size() > 1) && (ikdtree.Root_Node == nullptr))
+ {
+ // std::vector<PointType> points_init = feats_down->points;
+ ikdtree.set_downsample_param(filter_size_map_min);
+ ikdtree.Build(feats_down->points);
+ flg_map_inited = true;
+ continue;
+ }
+
+ if(ikdtree.Root_Node == nullptr)
+ {
+ flg_map_inited = false;
+ std::cout<<"~~~~~~~Initiallize Map iKD-Tree Failed!"<<std::endl;
+ continue;
+ }
+ int featsFromMapNum = ikdtree.size();
+ #else
+ if(featsFromMap->points.empty())
+ {
+ downSizeFilterMap.setInputCloud(feats_down);
+ }
+ else
+ {
+ downSizeFilterMap.setInputCloud(featsFromMap);
+ }
+ downSizeFilterMap.filter(*featsFromMap);
int featsFromMapNum = featsFromMap->points.size();
+ #endif
int feats_down_size = feats_down->points.size();
std::cout<<"[ mapping ]: Raw feature num: "<<feats_undistort->points.size()<<" downsamp num "<<feats_down_size<<" Map num: "<<featsFromMapNum<<" laserCloudValidNum "<<laserCloudValidNum<<std::endl;
/*** ICP and iterated Kalman filter update ***/
PointCloudXYZI::Ptr coeffSel_tmpt (new PointCloudXYZI(*feats_down));
PointCloudXYZI::Ptr feats_down_updated (new PointCloudXYZI(*feats_down));
+ std::vector<double> res_last(feats_down_size, 1000.0); // initial
- if (featsFromMapNum > 100)
+ if (featsFromMapNum >= 5)
{
+ t1 = omp_get_wtime();
+
+ #ifdef USE_ikdtree
+ std::vector<PointVector> Nearest_Points(feats_down_size);
+ #else
kdtreeSurfFromMap->setInputCloud(featsFromMap);
+
+ #endif
+
std::vector<bool> point_selected_surf(feats_down_size, true);
std::vector<std::vector<int>> pointSearchInd_surf(feats_down_size);
int rematch_num = 0;
bool rematch_en = 0;
-
+ flg_EKF_converged = 0;
+ deltaR = 0.0;
+ deltaT = 0.0;
t2 = omp_get_wtime();
for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++)
@@ -702,20 +837,33 @@ int main(int argc, char** argv)
/* transform to world frame */
pointBodyToWorld(&pointOri_tmpt, &pointSel_tmpt);
-
std::vector<float> pointSearchSqDis_surf;
+ #ifdef USE_ikdtree
+ auto &points_near = Nearest_Points[i];
+ #else
auto &points_near = pointSearchInd_surf[i];
+ #endif
+
if (iterCount == 0 || rematch_en)
{
- /** Find the closest surface/line in the map **/
+ point_selected_surf[i] = true;
+ /** Find the closest surfaces in the map **/
+ #ifdef USE_ikdtree
+ ikdtree.Nearest_Search(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf);
+ #else
kdtreeSurfFromMap->nearestKSearch(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf);
- if (pointSearchSqDis_surf[NUM_MATCH_POINTS - 1] < 3)
+ #endif
+ float max_distance = pointSearchSqDis_surf[NUM_MATCH_POINTS - 1];
+
+ if (max_distance > 3)
{
- point_selected_surf[i] = true;
+ point_selected_surf[i] = false;
}
}
-
- if (! point_selected_surf[i]) continue;
+
+ if (point_selected_surf[i] == false) continue;
+
+ // match_time += omp_get_wtime() - match_start;
double pca_start = omp_get_wtime();
@@ -723,12 +871,18 @@ int main(int argc, char** argv)
cv::Mat matA0(NUM_MATCH_POINTS, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB0(NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all(-1));
cv::Mat matX0(NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all(0));
-
+
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
+ #ifdef USE_ikdtree
+ matA0.at<float>(j, 0) = points_near[j].x;
+ matA0.at<float>(j, 1) = points_near[j].y;
+ matA0.at<float>(j, 2) = points_near[j].z;
+ #else
matA0.at<float>(j, 0) = featsFromMap->points[points_near[j]].x;
matA0.at<float>(j, 1) = featsFromMap->points[points_near[j]].y;
matA0.at<float>(j, 2) = featsFromMap->points[points_near[j]].z;
+ #endif
}
//matA0*matX0=matB0
@@ -754,9 +908,15 @@ int main(int argc, char** argv)
bool planeValid = true;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
+ #ifdef USE_ikdtree
+ if (fabs(pa * points_near[j].x +
+ pb * points_near[j].y +
+ pc * points_near[j].z + pd) > 0.1)
+ #else
if (fabs(pa * featsFromMap->points[points_near[j]].x +
pb * featsFromMap->points[points_near[j]].y +
pc * featsFromMap->points[points_near[j]].z + pd) > 0.1)
+ #endif
{
planeValid = false;
point_selected_surf[i] = false;
@@ -771,13 +931,22 @@ 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.92)
+ if ((s > 0.85))// && ((std::abs(pd2) - res_last[i]) < 3 * res_mean_last))
{
+ // if(std::abs(pd2) > 5 * res_mean_last)
+ // {
+ // point_selected_surf[i] = false;
+ // res_last[i] = 0.0;
+ // continue;
+ // }
point_selected_surf[i] = true;
coeffSel_tmpt->points[i].x = pa;
coeffSel_tmpt->points[i].y = pb;
coeffSel_tmpt->points[i].z = pc;
coeffSel_tmpt->points[i].intensity = pd2;
+
+ // if(i%50==0) std::cout<<"s: "<<s<<"last res: "<<res_last[i]<<" current res: "<<std::abs(pd2)<<std::endl;
+ res_last[i] = std::abs(pd2);
}
else
{
@@ -789,38 +958,32 @@ int main(int argc, char** argv)
}
double total_residual = 0.0;
+ laserCloudSelNum = 0;
+
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])
+ if (point_selected_surf[i] && (res_last[i] <= 2.0))
{
laserCloudOri->push_back(feats_down->points[i]);
coeffSel->push_back(coeffSel_tmpt->points[i]);
- total_residual += error_abs;
+ total_residual += res_last[i];
+ laserCloudSelNum ++;
}
}
- laserCloudSelNum = laserCloudOri->points.size();
- double ave_residual = total_residual / laserCloudSelNum;
- // ave_res_last
-
- effect_feat_num = coeffSel->size();
- if(iterCount == 1) std::cout << "[ mapping ]: Effective feature num: "<<effect_feat_num<<std::endl;
+ res_mean_last = total_residual / laserCloudSelNum;
+ std::cout << "[ mapping ]: Effective feature num: "<<laserCloudSelNum<<" res_mean_last "<<res_mean_last<<std::endl;
match_time += omp_get_wtime() - match_start;
solve_start = omp_get_wtime();
-
- if (laserCloudSelNum < 50) {
- continue;
- }
/*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/
Eigen::MatrixXd Hsub(laserCloudSelNum, 6);
Eigen::VectorXd meas_vec(laserCloudSelNum);
Hsub.setZero();
- omp_set_num_threads(4);
- #pragma omp parallel for
+ // omp_set_num_threads(4);
+ // #pragma omp parallel for
for (int i = 0; i < laserCloudSelNum; i++)
{
const PointType &laser_p = laserCloudOri->points[i];
@@ -842,11 +1005,11 @@ int main(int argc, char** argv)
}
Eigen::Vector3d rot_add, t_add, v_add, bg_add, ba_add, g_add;
- Eigen::VectorXd solution(DIM_OF_STATES);
+ Eigen::Matrix<double, DIM_OF_STATES, 1> solution;
Eigen::MatrixXd K(DIM_OF_STATES, laserCloudSelNum);
/*** Iterative Kalman Filter Update ***/
- if (Need_Init)
+ if (!flg_EKF_inited)
{
/*** only run in initialization period ***/
Eigen::MatrixXd H_init(Eigen::Matrix<double, 9, DIM_OF_STATES>::Zero());
@@ -869,41 +1032,60 @@ int main(int argc, char** argv)
{
auto &&Hsub_T = Hsub.transpose();
H_T_H.block<6,6>(0,0) = Hsub_T * Hsub;
- Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES> &&K_1 = (H_T_H + (state.cov / LASER_POINT_COV).inverse()).inverse();
+ Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES> &&K_1 = \
+ (H_T_H + (state.cov / LASER_POINT_COV).inverse()).inverse();
K = K_1.block<DIM_OF_STATES,6>(0,0) * Hsub_T;
- solution = K * meas_vec;
- state += solution;
+
+ // solution = K * meas_vec;
+ // state += solution;
+
+ auto vec = state_propagat - state;
+ solution = K * (meas_vec - Hsub * vec.block<6,1>(0,0));
+ state = state_propagat + solution;
rot_add = solution.block<3,1>(0,0);
t_add = solution.block<3,1>(3,0);
+ flg_EKF_converged = false;
+
+ if (((rot_add.norm() * 57.3 - deltaR) < 0.01) \
+ && ((t_add.norm() * 100 - deltaT) < 0.015))
+ {
+ flg_EKF_converged = true;
+ }
+
deltaR = rot_add.norm() * 57.3;
- deltaT = t_add.norm() * 100.0;
+ deltaT = t_add.norm() * 100;
}
euler_cur = RotMtoEuler(state.rot_end);
#ifdef DEBUG_PRINT
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;
+ std::cout<<"dR & dT: "<<deltaR<<" "<<deltaT<<" res norm:"<<res_mean_last<<std::endl;
#endif
/*** Rematch Judgement ***/
rematch_en = false;
- if ((deltaR < 0.01 && deltaT < 0.015) || ((rematch_num == 0) && (iterCount == (NUM_MAX_ITERATIONS - 2))))
+ if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (NUM_MAX_ITERATIONS - 2))))
{
rematch_en = true;
rematch_num ++;
+ std::cout<<"rematch_num: "<<rematch_num<<std::endl;
}
/*** Convergence Judgements and Covariance Update ***/
if (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))
{
- if (!Need_Init)
+ if (flg_EKF_inited)
{
/*** Covariance Update ***/
G.block<DIM_OF_STATES,6>(0,0) = K * Hsub;
state.cov = (I_STATE - G) * state.cov;
+ total_distance += (state.pos_end - position_last).norm();
+ position_last = state.pos_end;
+
+ std::cout<<"position: "<<state.pos_end.transpose()<<" total distance: "<<total_distance<<std::endl;
}
solve_time += omp_get_wtime() - solve_start;
break;
@@ -912,48 +1094,90 @@ int main(int argc, char** argv)
}
std::cout<<"[ mapping ]: iteration count: "<<iterCount+1<<std::endl;
- }
-
- bool if_cube_updated[laserCloudNum] = {0};
- for (int i = 0; i < feats_down_size; i++)
- {
- PointType &pointSel = feats_down_updated->points[i];
- int cubeI = int((pointSel.x + 0.5 * cube_len) / cube_len) + laserCloudCenWidth;
- int cubeJ = int((pointSel.y + 0.5 * cube_len) / cube_len) + laserCloudCenHeight;
- int cubeK = int((pointSel.z + 0.5 * cube_len) / cube_len) + laserCloudCenDepth;
+ t3 = omp_get_wtime();
- if (pointSel.x + 0.5 * cube_len < 0) cubeI--;
- if (pointSel.y + 0.5 * cube_len < 0) cubeJ--;
- if (pointSel.z + 0.5 * cube_len < 0) cubeK--;
-
- if (cubeI >= 0 && cubeI < laserCloudWidth &&
- cubeJ >= 0 && cubeJ < laserCloudHeight &&
- cubeK >= 0 && cubeK < laserCloudDepth) {
- int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
- featsArray[cubeInd]->push_back(pointSel);
- if_cube_updated[cubeInd] = true;
- }
- }
- for (int i = 0; i < laserCloudValidNum; i++)
- {
- int ind = laserCloudValidInd[i];
-
- if(if_cube_updated[ind])
+ /*** add new frame points to map ikdtree ***/
+ #ifdef USE_ikdtree
+ PointVector points_history;
+ ikdtree.acquire_removed_points(points_history);
+
+ memset(cube_updated, 0, sizeof(cube_updated));
+
+ for (int i = 0; i < points_history.size(); i++)
{
- downSizeFilterMap.setInputCloud(featsArray[ind]);
- downSizeFilterMap.filter(*featsArray[ind]);
- }
- }
- t3 = omp_get_wtime();
+ PointType &pointSel = points_history[i];
+ int cubeI = int((pointSel.x + 0.5 * cube_len) / cube_len) + laserCloudCenWidth;
+ int cubeJ = int((pointSel.y + 0.5 * cube_len) / cube_len) + laserCloudCenHeight;
+ int cubeK = int((pointSel.z + 0.5 * cube_len) / cube_len) + laserCloudCenDepth;
+
+ if (pointSel.x + 0.5 * cube_len < 0) cubeI--;
+ if (pointSel.y + 0.5 * cube_len < 0) cubeJ--;
+ if (pointSel.z + 0.5 * cube_len < 0) cubeK--;
+
+ if (cubeI >= 0 && cubeI < laserCloudWidth &&
+ cubeJ >= 0 && cubeJ < laserCloudHeight &&
+ cubeK >= 0 && cubeK < laserCloudDepth)
+ {
+ int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
+ featsArray[cubeInd]->push_back(pointSel);
+
+ }
+ }
+
+ // omp_set_num_threads(4);
+ // #pragma omp parallel for
+ for (int i = 0; i < feats_down_size; i++)
+ {
+ /* transform to world frame */
+ pointBodyToWorld(&(feats_down->points[i]), &(feats_down_updated->points[i]));
+ }
+ t4 = omp_get_wtime();
+ ikdtree.Add_Points(feats_down_updated->points, true);
+ #else
+ bool cube_updated[laserCloudNum] = {0};
+ for (int i = 0; i < feats_down_size; i++)
+ {
+ PointType &pointSel = feats_down_updated->points[i];
+
+ int cubeI = int((pointSel.x + 0.5 * cube_len) / cube_len) + laserCloudCenWidth;
+ int cubeJ = int((pointSel.y + 0.5 * cube_len) / cube_len) + laserCloudCenHeight;
+ int cubeK = int((pointSel.z + 0.5 * cube_len) / cube_len) + laserCloudCenDepth;
+
+ if (pointSel.x + 0.5 * cube_len < 0) cubeI--;
+ if (pointSel.y + 0.5 * cube_len < 0) cubeJ--;
+ if (pointSel.z + 0.5 * cube_len < 0) cubeK--;
+
+ if (cubeI >= 0 && cubeI < laserCloudWidth &&
+ cubeJ >= 0 && cubeJ < laserCloudHeight &&
+ cubeK >= 0 && cubeK < laserCloudDepth) {
+ int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
+ featsArray[cubeInd]->push_back(pointSel);
+ cube_updated[cubeInd] = true;
+ }
+ }
+ for (int i = 0; i < laserCloudValidNum; i++)
+ {
+ int ind = laserCloudValidInd[i];
+
+ if(cube_updated[ind])
+ {
+ downSizeFilterMap.setInputCloud(featsArray[ind]);
+ downSizeFilterMap.filter(*featsArray[ind]);
+ }
+ }
+ #endif
+ t5 = omp_get_wtime();
+ }
+
/******* Publish current frame points in world coordinates: *******/
laserCloudFullRes2->clear();
*laserCloudFullRes2 = dense_map_en ? (*feats_undistort) : (* feats_down);
int laserCloudFullResNum = laserCloudFullRes2->points.size();
- pcl::PointXYZRGB temp_point;
+ pcl::PointXYZI temp_point;
laserCloudFullResColor->clear();
{
for (int i = 0; i < laserCloudFullResNum; i++)
@@ -970,37 +1194,37 @@ int main(int argc, char** argv)
}
/******* 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);
- // }
+ {
+ laserCloudFullResColor->clear();
+ pcl::PointXYZI 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;
- // pcl::toROSMsg(*featsFromMap, laserCloudMap);
- // laserCloudMap.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar);
- // laserCloudMap.header.frame_id = "/camera_init";
- // pubLaserCloudMap.publish(laserCloudMap);
+ sensor_msgs::PointCloud2 laserCloudMap;
+ pcl::toROSMsg(*featsFromMap, laserCloudMap);
+ laserCloudMap.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar);
+ laserCloudMap.header.frame_id = "/camera_init";
+ pubLaserCloudMap.publish(laserCloudMap);
/******* Publish Odometry ******/
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
- (euler_cur(2), - euler_cur(0), - euler_cur(1));
+ (euler_cur(0), euler_cur(1), euler_cur(2));
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar);
- odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
- odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
- odomAftMapped.pose.pose.orientation.z = geoQuat.x;
+ odomAftMapped.pose.pose.orientation.x = geoQuat.x;
+ odomAftMapped.pose.pose.orientation.y = geoQuat.y;
+ odomAftMapped.pose.pose.orientation.z = geoQuat.z;
odomAftMapped.pose.pose.orientation.w = geoQuat.w;
odomAftMapped.pose.pose.position.x = state.pos_end(0);
odomAftMapped.pose.pose.position.y = state.pos_end(1);
@@ -1021,73 +1245,77 @@ int main(int argc, char** argv)
transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped" ) );
- #ifdef DEPLOY
+
msg_body_pose.header.stamp = ros::Time::now();
msg_body_pose.header.frame_id = "/camera_odom_frame";
msg_body_pose.pose.position.x = state.pos_end(0);
- msg_body_pose.pose.position.y = - state.pos_end(1);
- msg_body_pose.pose.position.z = - state.pos_end(2);
- msg_body_pose.pose.orientation.x = - geoQuat.y;
- msg_body_pose.pose.orientation.y = - geoQuat.z;
- msg_body_pose.pose.orientation.z = geoQuat.x;
+ msg_body_pose.pose.position.y = state.pos_end(1);
+ msg_body_pose.pose.position.z = state.pos_end(2);
+ msg_body_pose.pose.orientation.x = geoQuat.x;
+ msg_body_pose.pose.orientation.y = geoQuat.y;
+ msg_body_pose.pose.orientation.z = geoQuat.z;
msg_body_pose.pose.orientation.w = geoQuat.w;
+ #ifdef DEPLOY
mavros_pose_publisher.publish(msg_body_pose);
#endif
+ /******* Publish Path ********/
+ msg_body_pose.header.frame_id = "/camera_init";
+ path.poses.push_back(msg_body_pose);
+ pubPath.publish(path);
+
/*** save debug variables ***/
- t4 = omp_get_wtime();
frame_num ++;
- aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t3 - t1) / frame_num;
- // aver_time_consu = aver_time_consu * 0.5 + (t4 - t1) * 0.5;
+ aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
+ // aver_time_consu = aver_time_consu * 0.8 + (t5 - t0) * 0.2;
T1.push_back(Measures.lidar_beg_time);
s_plot.push_back(aver_time_consu);
- // s_plot2.push_back(double(deltaR));
- // s_plot3.push_back(double(deltaT));
+ s_plot2.push_back(t5 - t3);
+ s_plot3.push_back(match_time);
+ s_plot4.push_back(float(feats_down_size/10000.0));
+ s_plot5.push_back(t5 - t0);
- std::cout<<"[ mapping ]: time: selection "<<t2-t1 <<" match "<<match_time<<" pca "<<pca_time<<" solve "<<solve_time<<" total "<<t4 - t1<<std::endl;
- fout_out << std::setw(10) << Measures.lidar_beg_time << " " << euler_cur.transpose()*57.3 << " " << state.pos_end.transpose() << " " << state.vel_end.transpose() \
- <<" "<<state.bias_g.transpose()<<" "<<state.bias_a.transpose()<< std::endl;
- // fout_out << std::setw(10) << Measures.lidar_beg_time << " " << t3-t1 << " " << effect_feat_num << std::endl;
+ // std::cout<<"[ mapping ]: time: copy map "<<copy_time<<" readd: "<<readd_time<<" match "<<match_time<<" solve "<<solve_time<<"acquire: "<<t4-t3<<" map incre "<<t5-t4<<" total "<<aver_time_consu<<std::endl;
+ // fout_out << std::setw(10) << Measures.lidar_beg_time << " " << euler_cur.transpose()*57.3 << " " << state.pos_end.transpose() << " " << state.vel_end.transpose() \
+ // <<" "<<state.bias_g.transpose()<<" "<<state.bias_a.transpose()<< std::endl;
+ fout_out<<std::setw(8)<<laserCloudSelNum<<" "<<Measures.lidar_beg_time<<" "<<t2-t0<<" "<<match_time<<" "<<t5-t3<<" "<<t5-t0<<std::endl;
}
status = ros::ok();
rate.sleep();
}
//--------------------------save map---------------
- std::string surf_filename(map_file_path + "/surf.pcd");
- std::string corner_filename(map_file_path + "/corner.pcd");
- std::string all_points_filename(map_file_path + "/all_points.pcd");
+ // std::string surf_filename(map_file_path + "/surf.pcd");
+ // std::string corner_filename(map_file_path + "/corner.pcd");
+ // std::string all_points_filename(map_file_path + "/all_points.pcd");
- PointCloudXYZI surf_points, corner_points;
- surf_points = *featsFromMap;
- fout_out.close();
- fout_pre.close();
- if (surf_points.size() > 0 && corner_points.size() > 0)
+ // PointCloudXYZI surf_points, corner_points;
+ // surf_points = *featsFromMap;
+ // fout_out.close();
+ // fout_pre.close();
+ // if (surf_points.size() > 0 && corner_points.size() > 0)
+ // {
+ // pcl::PCDWriter pcd_writer;
+ // std::cout << "saving...";
+ // pcd_writer.writeBinary(surf_filename, surf_points);
+ // pcd_writer.writeBinary(corner_filename, corner_points);
+ // }
+
+ #ifdef DEPLOY
+ if (!T1.empty())
{
- pcl::PCDWriter pcd_writer;
- std::cout << "saving...";
- pcd_writer.writeBinary(surf_filename, surf_points);
- pcd_writer.writeBinary(corner_filename, corner_points);
+ // plt::named_plot("add new frame",T1,s_plot2);
+ // plt::named_plot("search and pca",T1,s_plot3);
+ // plt::named_plot("newpoints number",T1,s_plot4);
+ plt::named_plot("total time",T1,s_plot5);
+ plt::named_plot("average time",T1,s_plot);
+ // plt::named_plot("readd",T2,s_plot6);
+ plt::legend();
+ plt::show();
+ plt::pause(0.5);
+ plt::close();
}
- else
- {
- // #ifdef DEBUG_PRINT
- #ifndef DEPLOY
- if (!T1.empty())
- {
- plt::named_plot("time consumed",T1,s_plot);
- // plt::named_plot("R_residual",T1,s_plot2);
- // plt::named_plot("T_residual",T1,s_plot3);
- plt::legend();
- plt::show();
- plt::pause(0.5);
- plt::close();
- // plt::save("/home/xw/catkin_like_loam/src/LIEK_LOAM/a.png");
- }
- std::cout << "no points saved";
- // #endif
- #endif
- }
- //--------------------------
- // loss_output.close();
+ std::cout << "no points saved";
+ #endif
+
return 0;
}