first commit

CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dre_slam)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+set(CMAKE_BUILD_TYPE Release)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+   roscpp
+   cv_bridge
+   image_transport
+   geometry_msgs
+   message_filters
+   pcl_ros
+)
+
+catkin_package(
+)
+
+## System dependencies are found with CMake's conventions
+find_package(PCL REQUIRED)
+find_package(OpenCV 3 REQUIRED)
+find_package(Eigen3  REQUIRED)
+find_package(Ceres REQUIRED )
+find_package(Sophus REQUIRED)
+find_package(octomap REQUIRED)
+
+include_directories(
+	${catkin_INCLUDE_DIRS}
+	${OpenCV_INCLUDE_DIRS}
+	${EIGEN3_INCLUDE_DIR}
+	${PCL_INCLUDE_DIRS}
+	${CERES_INCLUDE_DIRS}
+	${Sophus_INCLUDE_DIRS}
+	${OCTOMAP_INCLUDE_DIRS}
+	${PROJECT_SOURCE_DIR}
+	${PROJECT_SOURCE_DIR}/core/include/
+        ${PROJECT_SOURCE_DIR}/third_party/
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}_lib
+	core/src/dre_slam.cpp
+ 	core/src/config.cpp
+ 	core/src/ORBextractor.cpp
+ 	core/src/camera.cpp
+ 	core/src/tracking.cpp
+ 	core/src/local_mapping.cpp
+ 	core/src/loop_closing.cpp
+ 	core/src/map.cpp
+ 	core/src/map_point.cpp
+ 	core/src/feature_detector.cpp
+ 	core/src/frame.cpp
+ 	core/src/keyframe.cpp
+ 	core/src/encoder_integration.cpp
+ 	core/src/common.cpp
+ 	core/src/optimizer.cpp
+ 	core/src/dynamic_pixel_detector.cpp
+ 	core/src/dynamic_pixel_culling.cpp
+ 	core/src/ros_puber.cpp
+ 	core/src/octomap_fusion.cpp
+ 	core/src/sub_octomap.cpp
+ 	core/src/sub_octomap_construction.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_lib
+	${catkin_LIBRARIES}
+	${OpenCV_LIBS}
+	${EIGEN3_LIBS}
+	${PCL_LIBRARIES}
+	${CERES_LIBRARIES}
+	${Sophus_LIBRARIES}
+	${PROJECT_SOURCE_DIR}/third_party/DBoW2/lib/libDBoW2.so
+	${PROJECT_SOURCE_DIR}/object_detector/lib/libobject_detector.so
+	${OCTOMAP_LIBRARIES}
+)
+
+
+# dre_slam_node
+add_executable(dre_slam_node
+	node/dre_slam_node.cpp)
+target_link_libraries(dre_slam_node
+	${PROJECT_NAME}_lib
+	${catkin_LIBRARIES}
+)

config/comparative_test.yaml

@@ -0,0 +1,87 @@
+%YAML:1.2
+
+#--------------------------------------------------------------------------------------------
+# Camera params. Pinhole
+#--------------------------------------------------------------------------------------------
+cam_rgb_topic_: /kinect2/qhd/image_color
+cam_depth_topic_: /kinect2/qhd/image_depth_rect
+
+cam_fx_: 525.2866213437447
+cam_fy_: 525.2178123117577
+cam_cx_: 472.85738972861157
+cam_cy_: 264.77181506420266
+cam_k1_: 0.04160142651680036
+cam_k2_: -0.04771035303381654
+cam_p1_: -0.0032638387781624705
+cam_p2_: -0.003985120051161831
+cam_k3_: 0.01110263483766991
+
+cam_height_: 540
+cam_width_: 960
+
+cam_depth_factor_: 0.001   # Depth scale factor.
+
+cam_dmax_: 8.0   # Max depth value to be used. (m)
+cam_dmin_: 0.1   # Min depth value to be used. (m) 
+
+cam_fps_: 20     # Camera FPs.
+
+#--------------------------------------------------------------------------------------------
+# Robot intrinsic and extrinsic
+#--------------------------------------------------------------------------------------------
+#### Intrinsic
+encoder_topic_: /rbot/encoder
+
+odom_kl_: 4.0652e-5   # left wheel factor
+odom_kr_: 4.0668e-5   # right wheel factor
+odom_b_: 0.3166       # wheel space
+odom_K_: 0.008        # Noise factor.
+
+#### Extrinsic Trc
+Trc_: !!opencv-matrix
+   rows: 3
+   cols: 4
+   dt: d
+   data: [ 0.0, -0.034899497, 0.999390827, 0.124, -1.0, 0.0, 0.0, -0.1, 0.0, -0.999390827, -0.034899497, 0.0 ]
+
+#--------------------------------------------------------------------------------------------
+# RGB-D Encoder Tracking
+#--------------------------------------------------------------------------------------------
+#### ORB feature 
+ret_ft_n_features_: 1600   # Number of ORB features per frame.
+
+#### Tracking
+ret_tk_dist_th_: 4.0    # Local map search radius. (m)
+ret_tk_angle_th_: 1.0   # Local map search angle. (rad)
+ret_tk_db_: 0.20         # Base threshold for erroneous match discard. (m)
+ret_tk_kd_: 0.025        # Scale factor of the threshold for erroneous match discard.
+
+#### Keyframe decision
+# Condation 1.
+ret_kd_fps_factor_: 0.9
+# Condation 2: 
+ret_kd_dist_th_: 0.3    # Max distance (m)
+ret_kd_angle_th_: 0.5   # Max angle (rad)
+
+#--------------------------------------------------------------------------------------------
+# Dynamic Pixels Culling
+#--------------------------------------------------------------------------------------------
+dpc_n_near_kfs_: 5                # Number of near keyframes.
+dpc_npts_per_cluster_: 6000       # Number of points per cluster.
+dpc_n_sel_pts_per_cluster_: 100   # Number of points per cluster to be selected for dynamic cluster decision.
+dpc_search_square_size_: 9        # 9 pixels
+
+#--------------------------------------------------------------------------------------------
+# Sparse Mapping 
+#--------------------------------------------------------------------------------------------
+#### Local Mapping
+sm_lm_window_size_: 8  # local BA window size sp_lm_window_size_ KFs.
+
+
+#--------------------------------------------------------------------------------------------
+# OctoMap Construction
+#--------------------------------------------------------------------------------------------
+oc_voxel_size_: 0.1   # Voxel size of the OctoMap (m).
+oc_submap_size_: 5    # Sub-OctoMap size (KFs)
+
+

config/extention_test.yaml

@@ -0,0 +1,85 @@
+%YAML:1.2
+
+#--------------------------------------------------------------------------------------------
+# Camera params. Pinhole
+#--------------------------------------------------------------------------------------------
+cam_rgb_topic_: /kinect2/qhd/image_color
+cam_depth_topic_: /kinect2/qhd/image_depth_rect
+
+cam_fx_: 525.2866213437447
+cam_fy_: 525.2178123117577
+cam_cx_: 472.85738972861157
+cam_cy_: 264.77181506420266
+cam_k1_: 0.04160142651680036
+cam_k2_: -0.04771035303381654
+cam_p1_: -0.0032638387781624705
+cam_p2_: -0.003985120051161831
+cam_k3_: 0.01110263483766991
+
+cam_height_: 540
+cam_width_: 960
+
+cam_depth_factor_: 0.001   # Depth scale factor.
+
+cam_dmax_: 8.0   # Max depth value to be used. (m)
+cam_dmin_: 0.1   # Min depth value to be used. (m) 
+
+cam_fps_: 10     # Camera FPs.
+
+#--------------------------------------------------------------------------------------------
+# Robot intrinsic and extrinsic
+#--------------------------------------------------------------------------------------------
+#### Intrinsic
+encoder_topic_: /rbot/encoder
+
+odom_kl_: 4.0652e-5   # left wheel factor
+odom_kr_: 4.0668e-5   # right wheel factor
+odom_b_: 0.3166       # wheel space
+odom_K_: 0.008        # Noise factor.
+
+#### Extrinsic Trc
+Trc_: !!opencv-matrix
+   rows: 3
+   cols: 4
+   dt: d
+   data: [  0.0, 0.275637, 0.9612617, 0.124, -1.0, 0.0, 0.0, -0.1, 0.0, -0.9612617, 0.275637, 0.0 ]
+
+#--------------------------------------------------------------------------------------------
+# RGB-D Encoder Tracking
+#--------------------------------------------------------------------------------------------
+#### ORB feature 
+ret_ft_n_features_: 1600   # Number of ORB features per frame.
+
+#### Tracking
+ret_tk_dist_th_: 4.0    # Local map search radius. (m)
+ret_tk_angle_th_: 1.0   # Local map search angle. (rad)
+ret_tk_db_: 0.20         # Base threshold for erroneous match discard. (m)
+ret_tk_kd_: 0.025        # Scale factor of the threshold for erroneous match discard.
+
+#### Keyframe decision
+# Condation 1.
+ret_kd_fps_factor_: 0.9 
+# Condation 2: 
+ret_kd_dist_th_: 0.3    # Max distance (m)
+ret_kd_angle_th_: 0.5   # Max angle (rad)
+
+#--------------------------------------------------------------------------------------------
+# Dynamic Pixels Culling
+#--------------------------------------------------------------------------------------------
+dpc_n_near_kfs_: 5                # Number of near keyframes.
+dpc_npts_per_cluster_: 6000       # Number of points per cluster.
+dpc_n_sel_pts_per_cluster_: 100   # Number of points per cluster to be selected for dynamic cluster decision.
+dpc_search_square_size_: 9        # 9 pixels
+
+#--------------------------------------------------------------------------------------------
+# Sparse Mapping 
+#--------------------------------------------------------------------------------------------
+#### Local Mapping
+sm_lm_window_size_: 8  # local BA window size sp_lm_window_size_ KFs.
+
+
+#--------------------------------------------------------------------------------------------
+# OctoMap Construction
+#--------------------------------------------------------------------------------------------
+oc_voxel_size_: 0.1   # Voxel size of the OctoMap (m).
+oc_submap_size_: 5    # Sub-OctoMap size (KFs)