ORBSLAM2_with_PointCloud编译ROS包并使用kinect2实现室内稠密点云地图创建
0. 相关依赖项等安装请参考上一篇博文：Ubuntu16.04编译高博的ORBSLAM2_with_pointcloud_map
关于kinect2的ROS驱动，请参考https://github.com/code-iai/iai_kinect2
ORBSLAM2提供了ROS的demo，可参考https://github.com/raulmur/ORB_SLAM2
readme中 7. ROS Examples
1. 拷贝ORB_SLAM2目录下的 build_ros.sh 脚本，也可以自己创建一个，该脚本内容如下：（此处将最后一步改为了make -j4,因为用make -j容易卡死）
echo "Building ROS nodes"
cd Examples/ROS/ORB_SLAM2
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j42. 修改ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt ，在其中加入PCL和G2O库的支持，内容如下，注释有###的语句为修改的语句：
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
IF(NOT ROS_BUILD_TYPE)
SET(ROS_BUILD_TYPE Release)
ENDIF()
MESSAGE("Build type: " ${ROS_BUILD_TYPE})
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
find_package(OpenCV 3 REQUIRED)###
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
find_package( G2O REQUIRED )###
find_package( PCL 1.7 REQUIRED )###
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}###
)
add_definitions( ${PCL_DEFINITIONS} )###
link_directories( ${PCL_LIBRARY_DIRS} )###
set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PCL_LIBRARIES}###
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension g2o_types_sim3 g2o_types_sba###
)
# Node for monocular camera
rosbuild_add_executable(Mono
src/ros_mono.cc
)
target_link_libraries(Mono
${LIBS}
)
# Node for RGB-D camera
rosbuild_add_executable(RGBD
src/ros_rgbd.cc
)
target_link_libraries(RGBD
${LIBS}
)
3. 修改ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc中订阅图像的话题，我用的是/kinect2/sd/image_color_rect 和/kinect2/sd/image_depth_rect
然后根据TUM1.yaml的格式创建ORB_SLAM2_modified/kinect2.yaml，主要修改相机参数，内容如下：
（关于相机的参数，可以自己标定，也可以直接从kinect发布的消息中获取，kinect2发布3种分辨率的RGB图和深度图，分别为hd,qhd,sd，此处采用sd，分辨率为 512×424。
在启动kinect节点的情况下，可以通过rostopic echo 查看相机信息，例如：
rostopic echo /kinect2/sd/camera_info）
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 368.143798
Camera.fy: 368.143798
Camera.cx: 256.731994
Camera.cy: 209.726501
Camera.k1: 0.084289
Camera.k2: -0.273043
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.101328
Camera.width: 512
Camera.height: 424
# Camera frames per second
Camera.fps: 30.0
# IR projector baseline times fx (aprox.)
Camera.bf: 40.0
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Close/Far threshold. Baseline times.
ThDepth: 50.0
# Deptmap values factor
DepthMapFactor: 1000.0
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000
# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2
# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8
# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
#--------------------------------------------------------------------------------------------
# PointCloud Mapping
#--------------------------------------------------------------------------------------------
PointCloudMapping.Resolution: 0.034. 运行脚本，进行编译
1）
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS注意：请用自己的路径代替上面的 PATH
例如：export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/andyoyo/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS
2）
sudo chmod +x build_ros.sh
./build_ros.sh5. 运行
先打开一个终端，运行roscore
roscore再打开一个终端，启动kinetic2
rosrun kinect2_bridge kinect2_bridge 最后在运行build_ros.sh脚本的终端（不然可能识别不到ORB_SLAM2这个包）运行ORB_SLAM2包中的RGBD节点
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt kinect2.yaml 结果如下图所示
感觉效果还不错哦～