ubuntu16.04中ORBSLAM2_with_PointCloud使用kinect2实现室内稠密点云地图创建

ubuntu16.04中ORBSLAM2_with_PointCloud使用kinect2实现室内稠密点云地图创建

首先实现ORBSLAM2_with_PointCloud在数据集下的运行，参考链接如下：
Ubuntu16.04下编译安装ORBSLAM2_with_pointcloud_map记录
安装了iai_kinect2，我的博客有可以参考

1.创建build_ros.sh 脚本

在ORB_SLAM2_modified目录下 拷贝ORB_SLAM2目录下的 build_ros.sh 脚本，也可以自己创建一个

echo "Building ROS nodes"
 
cd Examples/ROS/ORB_SLAM2
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j8

2.修改ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt

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 REQUIRED)
find_package(G2O REQUIRED)   ####
find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)
find_package( PCL 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}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension g2o_types_sim3 g2o_types_sba
${PCL_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
# ${PROJECT_SOURCE_DIR}/../../../lib/libboost_filesystem.so
# ${PROJECT_SOURCE_DIR}/../../../lib/libboost_system.so
# ${PROJECT_SOURCE_DIR}/../../../lib/libopencv_core.so   ###
)
 
# 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

//源码
   message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
   message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);
//修改为以下代码
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/kinect2/qhd/image_color_rect", 1);  
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/kinect2/qhd/image_depth_rect", 1);

4.ORB_SLAM2_modified下创建

kinect2.yaml

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 501.3
Camera.fy: 499.8 
Camera.cx: 479.7 
Camera.cy: 236.0

Camera.k1: 0.00643 
Camera.k2: 0.0150
Camera.p1: 0.00944
Camera.p2: 0.00212
Camera.p3: 0.0267
# 以上参数为calib_color.yaml中所得
Camera.width: 960 
Camera.height: 540 

# 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

5.编辑配置文件

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS
注：用自己的路径代替上面的 PATH

sudo gedit ~/.bashrc
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/yinyin/catkin_ws/src/ORBSLAM2_with_pointcloud_map-master/ORB_SLAM2_modified/Examples/ROS
source ~/.bashrc

ORB_SLAM2_modified下编译

sudo chmod +x build_ros.sh
./build_ros.sh

6.运行

打开一个新终端

roscore

再打开一个终端，启动kinetic2

rosrun kinect2_bridge kinect2_bridge 

在ORB_SLAM2_modified文件里打开一个新终端

 rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt kinect2.yaml 

ps:可能刚开始点云地图不会显示只是红绿界面,等一会滑动鼠标即可看到，问题不大

参考博客：https://blog.csdn.net/qq_25349629/article/details/88528765