Update to v1.0.0

CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(point_lio_unilidar)
+
+SET(CMAKE_BUILD_TYPE "Debug")
+
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
+
+add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
+message("DROOT_DIR = ${DROOT_DIR}")
+
+message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
+  include(ProcessorCount)
+  ProcessorCount(N)
+  message("Processer number:  ${N}")
+  if(N GREATER 5)
+    add_definitions(-DMP_EN)
+    add_definitions(-DMP_PROC_NUM=4)
+    message("core for MP:  3")
+  elseif(N GREATER 3)
+    math(EXPR PROC_NUM "${N} - 2")
+    add_definitions(-DMP_EN)
+    add_definitions(-DMP_PROC_NUM="${PROC_NUM}")
+    message("core for MP:  ${PROC_NUM}")
+  else()
+    add_definitions(-DMP_PROC_NUM=1)
+  endif()
+else()
+  add_definitions(-DMP_PROC_NUM=1)
+endif()
+
+find_package(OpenMP QUIET)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")
+
+find_package(PythonLibs REQUIRED)
+find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  roscpp
+  rospy
+  std_msgs
+  pcl_ros
+  tf
+  # livox_ros_driver
+  message_generation
+  eigen_conversions
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.8 REQUIRED)
+
+message(Eigen: ${EIGEN3_INCLUDE_DIR})
+
+include_directories(
+	${catkin_INCLUDE_DIRS} 
+  ${EIGEN3_INCLUDE_DIR}
+  ${PCL_INCLUDE_DIRS}
+  ${PYTHON_INCLUDE_DIRS}
+  include
+)
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
+  DEPENDS EIGEN3 PCL
+  INCLUDE_DIRS
+)
+
+add_executable(pointlio_mapping 
+  src/laserMapping.cpp 
+  include/ikd-Tree/ikd_Tree.cpp 
+  src/parameters.cpp 
+  src/preprocess.cpp 
+  src/Estimator.cpp
+)
+target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})

Log/guide.md

@@ -0,0 +1 @@
+Here saved the debug records which can be drew by the ../log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h.

Log/plot.py

@@ -0,0 +1,46 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+a_out=np.loadtxt('mat_out.txt')
+#######for normal#######
+fig, axs = plt.subplots(3,2)
+lab_out = ['', 'out-x', 'out-y', 'out-z']
+plot_ind = range(7,10)
+time=a_out[:,0]
+axs[0,0].set_title('Attitude')
+axs[1,0].set_title('Translation')
+axs[2,0].set_title('Velocity')
+axs[0,1].set_title('bg')
+axs[1,1].set_title('ba')
+axs[2,1].set_title('Gravity')
+for i in range(1,4):
+	for j in range(6):
+	    axs[j%3, j//3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
+	for j in range(6):
+		axs[j%3, j//3].grid()
+		axs[j%3, j//3].legend()
+plt.grid()
+    #######for normal#######
+
+
+#### Draw IMU data
+#fig, axs = plt.subplots(2)
+#imu=np.loadtxt('imu_pbp.txt')
+#time=imu[:,0]
+#axs[0].set_title('Gyroscope')
+#axs[1].set_title('Accelerameter')
+#lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
+#lab_2 = ['acc-x', 'acc-y', 'acc-z']
+#for i in range(3):
+    #if i==1:
+#    	axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
+#    	axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
+#for i in range(2):
+    #axs[i].set_xlim(386,389)
+#    axs[i].grid()
+#    axs[i].legend()
+#plt.grid()
+
+plt.show()

Log/plot_imu.py

@@ -0,0 +1,125 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+#### Draw IMU data
+fig, axs = plt.subplots(2)
+imu=np.loadtxt('imu_pbp.txt')
+time=imu[:,0]
+axs[0].set_title('Gyroscope')
+axs[1].set_title('Accelerameter')
+lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
+lab_2 = ['acc-x', 'acc-y', 'acc-z']
+for i in range(3):
+    #if i==1:
+    	axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
+    	axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
+for i in range(2):
+    #axs[i].set_xlim(386,389)
+    axs[i].grid()
+    axs[i].legend()
+plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(0,1977)
+#len_time2 = np.arange(1977, 3954)
+#len_time3 = np.arange(3954,5931)
+#len_time4 = np.arange(5931,7908)
+#len_time5 = np.arange(7908,9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
+#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
+#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
+#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
+#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(9885,9885+1977)
+#len_time2 = np.arange(9885+1977,9885+3954)
+#len_time3 = np.arange(9885+3954,9885+5931)
+#len_time4 = np.arange(9885+5931,9885+7908)
+#len_time5 = np.arange(9885+7908,9885+9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
+#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
+#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
+#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
+#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+# #### Draw time calculation
+# plt.figure(3)
+# fig = plt.figure()
+# font1 = {'family' : 'Times New Roman',
+# 'weight' : 'normal',
+# 'size'   : 12,
+# }
+# c="red"
+# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
+# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
+# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
+# # n = a_out[:,1].size
+# # time_mean = a_out[:,1].mean()
+# # time_se   = a_out[:,1].std() / np.sqrt(n)
+# # time_err  = a_out[:,1] - time_mean
+# # feat_mean = a_out[:,2].mean()
+# # feat_err  = a_out[:,2] - feat_mean
+# # feat_se   = a_out[:,2].std() / np.sqrt(n)
+# ax1 = fig.add_subplot(111)
+# ax1.set_ylabel('Effective Feature Numbers',font1)
+# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
+# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
+# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
+# ax1.set_ylim([0, 3000])
+
+# ax2 = ax1.twinx()
+# ax2.spines['right'].set_color('red')
+# ax2.set_ylabel('Compute Time (ms)',font1)
+# ax2.yaxis.label.set_color('red')
+# ax2.tick_params(axis='y', colors='red')
+# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.set_xlim([0.5, 3.5])
+# ax2.set_ylim([0, 100])
+
+# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
+# # # print(time_se)
+# # # print(a_out3[:,2])
+# plt.grid()
+# plt.savefig("time.pdf", dpi=1200)
+plt.show()