RGBD-slam和DVO-slam安装及运行

RGBD-slam

主要参考https://github.com/felixendres/rgbdslam_v2

1 下载

（1）源码包下载

cd <ros_ws>/src
git clone https://github.com/felixendres/rgbdslam_v2.git

尖括号<>中的内容需根据自己需要自行配置的意思
（2）下载作者提供的g2o

cd </home/xxx/Download/softpackages>
git clone https://github.com/felixendres/g2o.git
cd g2o & mkdir build install & cd build
cmake .. -DCMAKE_INSTALL_PREFIX=</home/xxx/Download/softpackages>/g2o/install
make & make install

（3） 修改以下文件：

1）src/misc.cpp

801：
-	inline int round(float d)
+  inline int ROUND(float d)

877：
-      int old_rx_center = round((p.x / p.z)* fx + cx);
-      int old_ry_center = round((p.y / p.z)* fy + cy);
+      int old_rx_center = ROUND((p.x / p.z)* fx + cx);
+      int old_ry_center = ROUND((p.y / p.z)* fy + cy);

1012：
-      int old_rx_center = round((p.x / p.z)* fx + cx);
-      int old_ry_center = round((p.y / p.z)* fy + cy);
+      int old_rx_center = ROUND((p.x / p.z)* fx + cx);
+      int old_ry_center = ROUND((p.y / p.z)* fy + cy);

2）package.xml
加两行：

+  <build_depend>cv_bridgebuild_depend>
+  <run_depend>cv_bridgerun_depend>

3）launch/rgbdslam.launch

6：
-<param name="config/topic_image_depth"             value="/camera/depth_registered/sw_registered/image_rect_raw"/>
+<param name="config/topic_image_depth" value="/camera/depth/image"/>

Tips:

本文将依赖的包均安装在/home/xxx/Download/softpackages下
cv_bridge使用版本：melodic分支
OpenCV：3.4.14
G2o：c++03分支

4）Cmakelist.txt
1、 关闭gpu，可选项

-set(USE_SIFT_GPU  1 CACHE BOOL "build with support for siftgpu") 
+set(USE_SIFT_GPU  0 CACHE BOOL "build with support for siftgpu")

2、添加c++11标准

+add_compile_options(-std=c++11)

3、设置cv_bridge

+# cv_bridge
+set(cv_bridge_INCLUDE_DIRS /home/xxx/Download/softpackages/vision_opencv/cv_bridge/install/include)
+set(cv_bridge_LIBRARY_DIRS /home/xxx/Download/softpackages/vision_opencv/cv_bridge/install/lib)
+include_directories(${cv_bridge_INCLUDE_DIRS})
+link_directories(${cv_bridge_LIBRARY_DIRS})

4、设置g2o

# g2o #######################
+set(G2O_DIR /home/xxx/Download/softpackages/g2o/build)
find_package(G2O REQUIRED)

5、 设置opencv

+set(OpenCV_DIR /home/xxx/Download/softpackages/opencv/build)
find_package(OpenCV REQUIRED)

6、设置链接选项

-SET(LIBS_LINK GL GLU cholmod cxsparse Qt5::Widgets Qt5::OpenGL Qt5::Concurrent ${G2O_LIBRARIES} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS})
+SET(LIBS_LINK GL GLU cholmod cxsparse Qt5::Widgets Qt5::OpenGL Qt5::Concurrent ${G2O_LIBRARIES} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS} cv_bridge)

5）G2o的G2OConfig.cmake文件内容：

get_filename_component(G2O_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 


set(G2O_INCLUDE_DIRS "/home/xxx/workspace/openslam_nav/src/rgbdslam_v2/lib/g2o/install/include") 
set(G2O_LIBRARIES)

# Set variable for individual library and also append to G2O_LIBRARIES
macro(find_g2o_library VARNAME LIBRARYNAME)
find_library(${VARNAME}
  NAMES "g2o_${LIBRARYNAME}"
  PATHS
  /home/xxx/workspace/openslam_nav/src/rgbdslam_v2/lib/g2o/install/lib
  NO_DEFAULT_PATH
  )
#Append to the global list of all libraries
list(APPEND G2O_LIBRARIES ${${VARNAME}})
endmacro(find_g2o_library VARNAME LIBRARYNAME)

# Now populate the variables
# Find the core elements
find_g2o_library(G2O_STUFF_LIBRARY stuff)
find_g2o_library(G2O_CORE_LIBRARY core)

# Find the CLI library
find_g2o_library(G2O_CLI_LIBRARY cli)

# Find the pluggable solvers
find_g2o_library(G2O_SOLVER_CHOLMOD solver_cholmod)
find_g2o_library(G2O_SOLVER_CSPARSE solver_csparse)
find_g2o_library(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
find_g2o_library(G2O_SOLVER_DENSE solver_dense)
find_g2o_library(G2O_SOLVER_PCG solver_pcg)
find_g2o_library(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
find_g2o_library(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
find_g2o_library(G2O_SOLVER_EIGEN solver_eigen)

# Find the predefined types
find_g2o_library(G2O_TYPES_DATA types_data)
find_g2o_library(G2O_TYPES_ICP types_icp)
find_g2o_library(G2O_TYPES_SBA types_sba)
find_g2o_library(G2O_TYPES_SCLAM2D types_sclam2d)
find_g2o_library(G2O_TYPES_SIM3 types_sim3)
find_g2o_library(G2O_TYPES_SLAM2D types_slam2d)
find_g2o_library(G2O_TYPES_SLAM3D types_slam3d)

2 编译

(1) 编译

cd <ros_ws>
catkin_make -DCATKIN_WHITELIST_PACKAGES=rgbdslam

(2) 查看：rgbdslam链接的g2o版本

ldd rgbdslam |grep g2o
libg2o_stuff.so => /home/xxx/workspace/openslam_nav/src/dvo_slam/thirdparty/g2o/install/lib/libg2o_stuff.so (0x00007f089599c000)
……

若链接到/opt/ros/melodic下，则会报未定义的问题，如下

libg2o_stuff.so => /opt/ros/melodic/lib/libg2o_stuff.so

则有两种解决办法：
1） 将/opt/ros/melodic下的g2o的库在编译前移走
2）使用patchelf更改链接路径：

cd <ros_ws>/devel/lib/rgbdslam
patchelf --replace-needed libg2o_stuff.so /home/xxx/workspace/openslam_nav/src/rgbdslam_v2/lib/g2o/lib/libg2o_stuff.so rgbdslam

3 运行

cd <ros_ws>
source devel/setup.sh
roslaunch rgbdslam rgbdslam.launch
#kitti rgbd dataset
rosbag play rgbd_dataset_freiburg1_360.bag

参考链接：
https://github.com/felixendres/rgbdslam_v2
https://wiki.ros.org/rgbdslam_electric/evaluation

DVO 编译运行：

主要参考：https://github.com/songuke/dvo_slam

下载DVO源码及依赖库文件

1.下载dvo-slam

cd <ros_ws>/src
git clone https://github.com/songuke/dvo_slam.git
cd dvo_slam
mkdir thirdparty

2.下载编译Sophus

cd thirdparty
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout b474f05
mkdir build
cd build
cmake ..
make

3.下载编译g2o

1)下载g2o

cd thirdparty
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
git checkout 67d5fa7

2）下载Eigen 3.2.8.

cd thirdparty/g2o/EXTERNAL
git clone https://github.com/eigenteam/eigen-git-mirror.git
git checkout 3.2.8

3）编译g2o

cd thirdparty/g2o
mkdir build install
cd build
cmake .. -DG2O_BUILD_APPS:BOOL=OFF -DG2O_BUILD_EXAMPLES:BOOL=OFF -DG2O_EIGEN3_INCLUDE=/home/xxx/workspace/openslam_nav/src/dvo_slam/thirdparty/g2o/EXTERNAL/eigen-3.2.8 -DCMAKE_INSTALL_PREFIX=/home/xxx/workspace/openslam_nav/src/dvo_slam/thirdparty/g2o/install
make&make install

4）创建G2OConfig.cmake，对应修改G2O_INCLUDE_DIRS及设置G2O_LIBRARIES，参见G2OConfig.cmake

4.修改dvo的几个package的CMakeLists.txt

（1）dvo_core的CMakeLists.txt
Path：dvo_slam/src/dvo_core/CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(dvo_core)

set(Sophus_DIR ${PROJECT_SOURCE_DIR}/../../thirdparty/Sophus)
set(Eigen3_DIR ${PROJECT_SOURCE_DIR}/../../thirdparty/g2o/EXTERNAL/eigen-3.2.8)
find_package(catkin REQUIRED)
find_package(cmake_modules REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread)
find_package(OpenCV REQUIRED)
find_package(TBB REQUIRED)
find_package(PCL REQUIRED COMPONENTS common filters)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES dvo_core dvo_visualization
  DEPENDS OpenCV PCL
)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse -msse2 -msse3 -msse4 -ftree-vectorize")

include_directories(
  "include"
  #${Sophus_INCLUDE_DIRS}
  ${Sophus_DIR}
  ${catkin_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_library(dvo_core 
    src/core/interpolation.cpp
    src/core/intrinsic_matrix.cpp
    src/core/least_squares.cpp
    src/core/math_sse.cpp
    src/core/rgbd_image.cpp
    src/core/rgbd_image_sse.cpp
    src/core/point_selection.cpp
    src/core/surface_pyramid.cpp
    src/core/weight_calculation.cpp
    
src/util/histogram.cpp

src/visualization/camera_trajectory_visualizer.cpp

src/dense_tracking.cpp
src/dense_tracking_impl.cpp
src/dense_tracking_config.cpp
)

target_link_libraries(dvo_core
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
${catkin_LIBRARIES}
tbb
boost_thread
)

add_library(dvo_visualization
  src/visualization/async_point_cloud_builder.cpp
    src/visualization/point_cloud_aggregator.cpp
#    src/visualization/pcl_camera_trajetory_visualizer.cpp
)

target_link_libraries(dvo_visualization
  dvo_core
)

（2）dvo_ros的CMakeLists.txt
Path：dvo_slam/src/dvo_ros/CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(dvo_ros)

find_package(catkin REQUIRED COMPONENTS roscpp tf tf_conversions eigen_conversions dynamic_reconfigure dvo_core sensor_msgs image_transport message_filters cv_bridge visualization_msgs interactive_markers pcl_ros)
find_package(cmake_modules REQUIRED)

# dynamic reconfigure
generate_dynamic_reconfigure_options(cfg/CameraDenseTracker.cfg)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES dvo_ros
)

include_directories(
  "include"
  ${catkin_INCLUDE_DIRS}
)


add_library(${PROJECT_NAME} 
    src/camera_base.cpp
    src/camera_dense_tracking.cpp
        
    src/visualization/ros_camera_trajectory_visualizer.cpp
)

target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
)

add_library(camera_tracker_nodelet
    src/camera_tracker_nodelet.cpp
)

target_link_libraries(camera_tracker_nodelet
    ${PROJECT_NAME}
    ${catkin_LIBRARIES}
)

add_dependencies(camera_tracker_nodelet ${PROJECT_NAME}_gencfg)

add_executable(camera_tracker
     src/camera_tracker_node.cpp
)

target_link_libraries(camera_tracker
    ${PROJECT_NAME}
    ${catkin_LIBRARIES}
)

add_dependencies(camera_tracker ${PROJECT_NAME}_gencfg)

（3）dvo_slam的CMakeLists.txt
Path：dvo_slam/src/dvo_slam/CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(dvo_slam)

#set(BUILD_SHARED_LIBS OFF)
set(G2O_DIR ${PROJECT_SOURCE_DIR}/../../thirdparty/g2o/build)
find_package(G2O REQUIRED)

LIST(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure dvo_core dvo_ros sensor_msgs image_transport message_filters cv_bridge visualization_msgs interactive_markers)
find_package(cmake_modules REQUIRED)
find_package(SuiteSparse REQUIRED)
find_package(OpenGL)

IF(G2O_FOUND)
message(STATUS "found g2o ${G2O_FOUND}")
ELSE()
message(ERROR "g2o missing")
ENDIF()

# dynamic reconfigure
generate_dynamic_reconfigure_options(cfg/GraphVisualizer.cfg cfg/KeyframeSlam.cfg)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES dvo_slam
  DEPENDS G2O 
  DEPENDS dvo_core
)

set(Sophus_DIR ${PROJECT_SOURCE_DIR}/../../thirdparty/Sophus)

include_directories(
  "include"
  ${G2O_INCLUDE_DIRS}
  ${dvo_slam_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
  ${CSPARSE_INCLUDE_DIR}
  ${Sophus_INCLUDE_DIRS}
  ${Sophus_DIR}
)
message("${Sophus_INCLUDE_DIRS} " ${Sophus_INCLUDE_DIRS})

# messages
#rosbuild_genmsg()

# VTK library
#find_package(VTK REQUIRED)
#include_directories(${VTK_INCLUDE_DIRS})

add_library(${PROJECT_NAME} 
src/keyframe_tracker.cpp
  src/keyframe_graph.cpp
  src/keyframe_constraint_search.cpp
  src/camera_keyframe_tracking.cpp

  src/config.cpp

  src/tracking_result_evaluation.cpp
  src/local_map.cpp
  src/local_tracker.cpp

  src/constraints/constraint_proposal.cpp
  src/constraints/constraint_proposal_voter.cpp
  src/constraints/constraint_proposal_validator.cpp

  src/serialization/map_serializer_interface.cpp
  src/serialization/map_serializer.cpp

  src/visualization/graph_visualizer.cpp

  src/types.cpp
)

target_link_libraries(${PROJECT_NAME}
  ${G2O_LIBRARIES}
  ${catkin_LIBRARIES}
  ${OPENGL_LIBRARIES}
)

#add_executable(camera_keyframe_tracker
#  src/camera_keyframe_tracker_node.cpp
#)

#target_link_libraries(camera_keyframe_tracker
#  ${PROJECT_NAME}
#)

add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)

（4）dvo_benchmark的CMakeLists.txt
Path：dvo_slam/src/dvo_benchmark/CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(dvo_benchmark)

#set(BUILD_SHARED_LIBS OFF)

set(G2O_DIR ${PROJECT_SOURCE_DIR}/../../thirdparty/g2o/build)
find_package(G2O REQUIRED)

LIST(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")

find_package(catkin REQUIRED COMPONENTS roscpp dvo_core dvo_ros dvo_slam dynamic_reconfigure)
find_package(cmake_modules REQUIRED)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES dvo_benchmark
)

include_directories(
  "include"
  ${catkin_INCLUDE_DIRS}
  ${G2O_INCLUDE_DIRS}
)


add_executable(benchmark
     src/benchmark_slam.cpp
)

#add_executable(experiment
#     src/experiment.cpp
#)

target_link_libraries(benchmark
#  vtkCommon
  ${G2O_LIBRARIES}
  ${catkin_LIBRARIES}
)

# create output directory - just convenience for our users
#file(MAKE_DIRECTORY "${PROJECT_SOURCE_DIR}/output")
#file(MAKE_DIRECTORY "${PROJECT_SOURCE_DIR}/output/video")

install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch

5. 编译dvo_slam

按以下顺序编译dvo_slam

catkin_make -DCATKIN_WHITELIST_PACKAGES=dvo_core -DCMAKE_BUILD_TYPE=Release
catkin_make -DCATKIN_WHITELIST_PACKAGES=dvo_ros -DCMAKE_BUILD_TYPE=Release
catkin_make -DCATKIN_WHITELIST_PACKAGES=dvo_slam -DCMAKE_BUILD_TYPE=Release
catkin_make -DCATKIN_WHITELIST_PACKAGES=dvo_benchmark -DCMAKE_BUILD_TYPE=Release

遇到问题：

/home/xxx/workspace/openslam_nav/src/dvo_slam/src/dvo_ros/src/camera_tracker_nodelet.cpp:24:24: error: expected constructor, destructor, or type conversion before ‘(’ token(’
PLUGINLIB_DECLARE_CLASS(dvo_ros, camera_tracker, dvo_ros::CameraTrackerNodelet, nodelet::Nodelet);

解决办法：
dvo_slam/src/dvo_ros/src/camera_tracker_nodelet.cpp +24

PLUGINLIB_EXPORT_CLASS(dvo_ros::CameraTrackerNodelet, nodelet::Nodelet)

6. 运行dvo_slam

roslaunch launch/benchmark.launch dataset:=<RGBD dataset folder>
eg:
roslaunch launch/benchmark.launch dataset:=/home/xxx/Download/bags/RGBD-TUM/tgz/rgbd_dataset_freiburg1_360
rviz -f world