[feature] added imu_preint module.

algorithms/src/LocalizationAndMapping/imu_preint/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(imu_preint)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+#add_compile_options(-std=c++11 -O0)
+add_compile_options(-std=c++17 -O3)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+#set(CMAKE_BUILD_TYPE DEBUG)
+set(CMAKE_BUILD_TYPE RELEASE)
+find_package(OpenCV 3.4.5 REQUIRED)
+find_package(PCL 1.8 REQUIRED)
+find_package(Glog REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(GTSAM REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  geometry_msgs
+  image_transport
+  roscpp
+  rosbag
+  sensor_msgs
+  nav_msgs
+  tf  
+  tf2 
+  tf2_ros
+  visualization_msgs
+)
+include_directories(
+        ${catkin_INCLUDE_DIRS}
+        ${EIGEN3_INCLUDE_DIR}
+        ${GTSAM_INCLUDE_DIR}
+)
+catkin_package()
+set(REQUIRED_LIBS
+        ${OpenCV_LIBS}
+        ${GLOG_LIBRARY}
+        ${catkin_LIBRARIES}
+        ${PCL_LIBRARIES}
+        ${GTSAM_LIBRARIES}
+        glog
+        gtsam
+)
+add_executable(offline_calc src/offline_calc.cpp)
+target_link_libraries(offline_calc ${REQUIRED_LIBS})
+
+add_executable(imu_preint_node src/imu_preint_node.cpp)
+target_link_libraries(imu_preint_node ${REQUIRED_LIBS})
+
+
+
+

algorithms/src/LocalizationAndMapping/imu_preint/config/imu_extrinsic.yaml

@@ -0,0 +1,10 @@
+%YAML:1.0
+---
+T_imu_lidar: !!opencv-matrix
+  rows: 4
+  cols: 4
+  dt: f
+  data: [1,0,0,0,
+        0,1,0,0,
+        0,0,1,0,
+        0,0,0,1]

algorithms/src/LocalizationAndMapping/imu_preint/launch/imu_preint.launch

@@ -0,0 +1,8 @@
+<launch>
+   <arg name="do_visualization" value="true"/>
+   <!--param name="use_sim_time" type="bool" value="true"-->
+   <param name="imu_preint_result_visualization" type="bool" value="$(arg do_visualization)" />
+   <param name="imu_lidar_extrinsic_path" type="string" value="$(find imu_preint)/config/imu_extrinsic.yaml"/>
+   <param name="imu_topic_name" type="string" value="/external_imu"/>
+   <node name="imu_preint_node" pkg="imu_preint" type="imu_preint_node" output="screen"/>
+</launch>

algorithms/src/LocalizationAndMapping/imu_preint/package.xml

@@ -0,0 +1,91 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imu_preint</name>
+  <version>0.0.0</version>
+  <description>The imu_preint package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">LaurentWang</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/vision_lidar_fusion</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>rosbag</depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>image_transport</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>image_transport</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
+