diff --git a/CMakeLists.txt b/CMakeLists.txt index 31fe073..fda9a54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(fast_lio) +cmake_policy(SET CMP0074 NEW) SET(CMAKE_BUILD_TYPE "Debug") @@ -79,6 +80,7 @@ add_executable(loam_feat_extract src/feature_extract.cpp) target_link_libraries(loam_feat_extract ${catkin_LIBRARIES} ${PCL_LIBRARIES}) add_executable(loam_laserMapping src/laserMapping.cpp) +add_dependencies(loam_laserMapping ${catkin_EXPORTED_TARGETS}) target_link_libraries(loam_laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${PYTHON_LIBRARIES}) target_include_directories(loam_laserMapping PRIVATE ${PYTHON_INCLUDE_DIRS}) # target_compile_definitions(loam_laserMapping PRIVATE ROOT_DIR="${CMAKE_CURRENT_SOURCE_DIR}") diff --git a/README.md b/README.md index f4c0909..5ee289e 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,14 @@ 2. Automaticaly initialized at most steady environments; 3. Parallel KD-Tree Search to decrease the computation; 4. Robust feature extraction; -5. Surpports for different FOV. + +It should be noted current version of FAST-LIO does not support Velodyne LiDAR, we may support them after March 2021. + +**Developers** + +[Wei Xu 徐威](https://github.com/XW-HKU): Laser mapping and pose optimization; + +[Zheng Liu 刘政](https://github.com/Zale-Liu): Features extraction. To know more about the details, please refer to our related paper:) @@ -12,7 +19,7 @@ To know more about the details, please refer to our related paper:) [FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196) -**Our related video**: Commming soon. +**Our related video**: https://youtu.be/iYCY6T79oNU
@@ -46,8 +53,11 @@ Clone the repository and catkin_make: catkin_make source devel/setup.bash ``` +*Remarks:* +- If you want to use a custom build of PCL, add the following line to ~/.bashrc +```export PCL_ROOT={CUSTOM_PCL_PATH}``` ## 3. Directly run -### 3.1 For indoor environments and high frame-rate (such as 100hz) +### 3.1 For indoor environments (support maximum 50hz frame rate) Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then ``` .... @@ -76,14 +86,7 @@ Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWm roslaunch fast_lio mapping_avia.launch rosbag play YOUR_DOWNLOADED.bag ``` -### 4.2 High-rate rosbag (Livox Avia LiDAR with 100Hz frame-rate) - -Download [high_rate_avia](https://drive.google.com/file/d/1UM6O3PRN3b730ZeuvKKT3yuOLNQuz8Yf/view?usp=sharing) and then -``` -roslaunch fast_lio mapping_avia.launch -rosbag play YOUR_DOWNLOADED.bag -``` -### 4.3 Outdoor rosbag (Livox Avia LiDAR) +### 4.2 Outdoor rosbag (Livox Avia LiDAR)
diff --git a/config/fast_lio.yaml b/config/fast_lio.yaml new file mode 100644 index 0000000..e69de29 diff --git a/include/Exp_mat.h b/include/Exp_mat.h index 84ba4f8..7ab2897 100644 --- a/include/Exp_mat.h +++ b/include/Exp_mat.h @@ -3,7 +3,7 @@ #include #include -#include +#include // #include #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 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 +#include #include #include #include @@ -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 (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::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 &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 &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 operator-(const StatesGroup& b) + { + Eigen::Matrix 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 &a, const Eigen::Ma return std::move(rot_kp); } -template -Eigen::Matrix RotMtoEuler(const Eigen::Matrix &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 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..40d1aed --- /dev/null +++ b/include/so3_math.h @@ -0,0 +1,105 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include +#include +// #include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix 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 +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix 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 +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix 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 +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix 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 +Eigen::Matrix RotMtoEuler(const Eigen::Matrix &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 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 @@ - + - + - + diff --git a/launch/mapping_avia_outdoor.launch b/launch/mapping_avia_outdoor.launch index 2c1a223..03bde28 100644 --- a/launch/mapping_avia_outdoor.launch +++ b/launch/mapping_avia_outdoor.launch @@ -20,18 +20,18 @@ - + - + - + - + diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index cd634ff..b2ef4ba 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 @@ -49,7 +47,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: currPoints + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -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: 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: 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: 000000ff00000000fd00000004000000000000017900000384fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000384000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001aa000001e20000001600fffffffb0000000a0049006d00610067006501000002730000027f00000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074900000052fc0100000002fb0000000800540069006d0065010000000000000749000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005ca0000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 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 #include #include -#include +#include #include #include #include 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 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; ipoints[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 &pl = pl_buff[j]; @@ -197,11 +211,13 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) plsize--; for(uint i=0; i &pl = pl_buff[j]; @@ -496,12 +510,12 @@ void give_feature(pcl::PointCloud &pl, vector &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 &pl, vector &types, pcl::P } // i_nex = i; i2 = i; + // std::cout<<" i: "< &pl, vector &types, pcl::P } - plsize2 = plsize - 3; + plsize2 = plsize > 3 ? plsize - 3 : 0; for(uint i=head+3; i=Real_Plane) @@ -777,20 +792,24 @@ void give_feature(pcl::PointCloud &pl, vector &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 &pl, vector &types, pcl::P last_surface = -1; } } - } void pub_func(pcl::PointCloud &pl, ros::Publisher pub, const ros::Time &ct) @@ -854,6 +872,8 @@ int plane_judge(const pcl::PointCloud &pl, vector &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 &pl, vector &types, ui double v1[3], v2[3]; for(uint j=i_cur+1; 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 59b4f9a..7d8571e 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -40,13 +40,15 @@ #include #include #include -#include +#include #include #include -#include +#include #include + #include "IMU_Processing.hpp" #include +#include #include #include #include @@ -62,12 +64,12 @@ #include -#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 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 lidar_buffer; std::deque 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::Ptr laserCloudFullResColor(new pcl::PointCloud()); +bool _last_inFOV[laserCloudNum]; +bool cube_updated[laserCloudNum]; +int laserCloudValidInd[laserCloudNum]; +pcl::PointCloud::Ptr laserCloudFullResColor(new pcl::PointCloud()); + +#ifdef USE_ikdtree +KD_TREE ikdtree; +std::vector cub_needrm; +std::vector cub_needad; +#else pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); +#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 +void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &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 "<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() - cube_p; + float squaredSide1 = dis_vec.transpose() * dis_vec; + + if(squaredSide1 < 0.4 * cube_len * cube_len) return true; + + dis_vec = XAxisPoint_world.cast() - 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() - cube_p; + float squaredSide1 = dis_vec.transpose() * dis_vec; + + dis_vec = XAxisPoint_world.cast() - 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:"<= 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: "<= 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: "< HALF_FOV_COS) isInLaserFOV = true; } - // std::cout<<"cent point: "<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: "< 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]: "< ("/aft_mapped_to_init", 10); - + ros::Publisher pubPath = nh.advertise + ("/path", 10); #ifdef DEPLOY ros::Publisher mavros_pose_publisher = nh.advertise("/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 << "~~~~"< 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||||||||||"<points.size() > 1) && (ikdtree.Root_Node == nullptr)) + { + // std::vector 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!"<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: "<points.size()<<" downsamp num "< res_last(feats_down_size, 1000.0); // initial - if (featsFromMapNum > 100) + if (featsFromMapNum >= 5) { + t1 = omp_get_wtime(); + + #ifdef USE_ikdtree + std::vector Nearest_Points(feats_down_size); + #else kdtreeSurfFromMap->setInputCloud(featsFromMap); + + #endif + std::vector point_selected_surf(feats_down_size, true); std::vector> 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 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(j, 0) = points_near[j].x; + matA0.at(j, 1) = points_near[j].y; + matA0.at(j, 2) = points_near[j].z; + #else matA0.at(j, 0) = featsFromMap->points[points_near[j]].x; matA0.at(j, 1) = featsFromMap->points[points_near[j]].y; matA0.at(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: "<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: "<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 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::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 &&K_1 = (H_T_H + (state.cov / LASER_POINT_COV).inverse()).inverse(); + Eigen::Matrix &&K_1 = \ + (H_T_H + (state.cov / LASER_POINT_COV).inverse()).inverse(); K = K_1.block(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"<= 2 || (iterCount == NUM_MAX_ITERATIONS - 1)) { - if (!Need_Init) + if (flg_EKF_inited) { /*** Covariance Update ***/ G.block(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: "<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 "< 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; }