ZED 相机 && ORB-SLAM2安装环境配置与ROS下的调试

      注：1. 对某些地方进行了更新(红色标注)，以方便进行配置。

          2. ZED ROS Wrapper官方github已经更新，根据描述新的Wrapper可能已经不适用与Ros Indigo了，如果大家想参照这份博客进行安装ZED ROS Wrapper，有可能会出现其他问题。ZED ROS Wrapper github地址：https://github.com/stereolabs/zed-ros-wrapper

      最近想运用ZED相机在ROS系统上实时运行ORB-SLAM2，在环境配置的过程当中遇到了不少问题并且在网上都没有比较系统的解决方法，因此将自己总结的步骤记录下来，希望对大家有帮助。

      本文将包括以下内容：

• OpenCV 2.4.10 与 3.1.0的安装
• ORB-SLAM2的安装与调试（ROS与非ROS）
• ROS Indigo的安装
• CUDA7.5的安装
• ZED SDK与ZED ROS Wrapper的安装与调试
• OpenCV多版本管理
• ROS Topic名称的更改
• ZED ROS Wrapper输出图像参数的更改

      一、 配置环境与阅读说明

     Ubuntu14.04，ROS Indigo，Nvidia GTX 970。

     在此需要说明一下，如果想在Ubuntu 14上进行相关配置请不要使用Nvidia GTX 1000系列显卡，因为1000系列显卡及其驱动会与某一些库文件冲突。

     同时希望读者按照我所列的安装顺序进行安装，因为调换安装顺序可能会导致某些依赖项或文件产生冲突。该顺序是我多次失败后总结出来的，亲测有用。

      二、 配置与安装步骤

      2.1. OpenCV 3.1 安装

     为正常运行ZED SDK需要预先安装OpenCV 3.1，可从opencv.org/downloads.html上下载，具体步骤如下所示：

     首先，安装OpenCV所要求的包：

[compiler] sudo apt-get install build-essential
[required] sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
[optional] sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev

     然后在Opencv 3.1的下载路径中生成build文件夹以便cmake，最后运用cmake编译，具体如下：

cd ~/opencv
mkdir build
cd build

cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..

     之后直接make，OpenCV官网中建议make时开启7个线程：

make -j7 # runs 7 jobs in parallel

     在以上工作完成后执行一下命令完成安装：

sudo make install

     这样Opencv 3.1就会成功的安装到你的电脑当中。

     由于版本问题，在cmake OpenCV 3.1时可能会遇到ippcv_linux下载不正确的问题，解决方法是在网上自行重新下载ippicv_linux包，并将其重新放入 opencv/3rdparty/ippicv/downloads/linux-808b791a6eac9ed78d32a7666804320e 目录中并重新编译。

     关于OpenCV 3.1的更多详细内容请参考官网：http://docs.opencv.org/3.1.0/d7/d9f/tutorial_linux_install.html

     2.2. OpenCV 2.4.10 安装

     虽然ORB-SLAM2现在已经支持OpenCV3.1但是由于与Ubuntu 14.04适配的ROS Indigo自带Opencv2.4.8因此如果不安装Opencv 2.4会导致之后在编译ORB-SLAM2 ROS时出现Opencv版本错误。该错误主要是因为ROS Indigo中的cv_bridge指向OpenCV2.4.8但是在不安装OpenCV2.4时候ORB-SLAM2是使用OpenCV3.1进行编译（这里理论上是可以通过重新安装ROS cv_bridge或相关包解决的，不过我ROS不是太熟，因此采用了另外一种方法）。

     OpenCV 2.4.10的下载和安装大家可以参考一下高翔博士的博客一起做系列中的相关内容：http://www.cnblogs.com/gaoxiang12/p/4633316.html

     2.3. ORB-SLAM2的安装

     现在就可以开始安装ORB-SLAM2了，参考官方网址为：https://github.com/raulmur/ORB_SLAM2

     根据要求我们先下载Eiegn 3：

sudo apt-get install libeigen3-dev

     然后安装可视化工具Pangolin：

     安装依赖项

sudo apt-get install cmake libglew-dev libpython2.7-dev

     直接进入到Pangolin的下载目录，然后执行以下命令：    

mkdir build
cd build
cmake ..
make -j

     如果以上步骤有出现任何错误，请根据官方文档确认所有依赖项以正确安装：https://github.com/stevenlovegrove/Pangolin

     ORB-SLAM2编译与配置：

     进入到ORB-SLAM2目录执行:

chmod +x build.sh
./build.sh

     在此过程中，程序会自动安装G2O库。一般来说这一步不会出现任何问题。

      安装完成后可以下载开源数据集检测ORB-SLAM2是否能正确运行，下面给出双目EUROC数据集的运行命令以作参考：

for terminal

./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml /home/li/openslam/database/Stereo_data/EUROC/V1_01_easy/mav0/cam0/data /home/li/openslam/database/Stereo_data/EUROC/V1_01_easy/mav0/cam1/data Examples/Stereo/EuRoC_TimeStamps/V101.txt

for IDE - Kdevelop

"/home/li/openslam/ORB_SLAM2/Vocabulary/ORBvoc.txt" "Examples/Stereo/EuRoC.yaml" "/home/li/openslam/database/Stereo_data/EUROC/V1_01_easy/mav0/cam0/data" "/home/li/openslam/database/Stereo_data/EUROC/V1_01_easy/mav0/cam1/data" "Examples/Stereo/EuRoC_TimeStamps/V101.txt"

      2.4. ROS Indigo安装

      Ubuntu 14.04适配的ROS版本为Indigo和jade，因为我在配置ORB-SLAM2 ROS Jade中出现了错误，所以推荐大家使用ROS Indigo，并且ROS Indigo也是Ubuntu 14.04的主流版本。

      ROS Indigo安装主要参考ros wiki： http://wiki.ros.org/cn/indigo/Installation/Ubuntu 

      按照ros wiki中的步骤就可以没有任何错误的安装好ROS Indigo

      在根据WiKi安装完整版时可能会出现与更新有关的报错，只需要在控制台中输入(sudo) apt-get update命令重新更新并重新执行安装命令即可安装成功。

      安装好ROS Indigo后，各位可以使用以下命令，查看打开文件最后是否出现 source /opt/ros/indigo/setup.bash。如若出现且在控制台中可以运行roscore则说明ROS安装正确。

gedit ~/.bashrc

      请注意，以后所配置的ROS空间都可以加入到.bashrc文件中，以免每次都需要申明ROS环境空间。下面的命令可以查询当前启用的ROS环境空间：

echo $ROS_PACKAGE_PATH

      2.5. ORB-SLAM2 & ROS安装

      现在就可以安装ORB-SLAM2 ROS了，安装步骤十分简单。

      首先用之前所述命令打开.bashrc文件，并加入ORB-SLAM2的ros包路径，以配置环境，示例如下：

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/li/openslam/ORB_SLAM2/Examples/ROS

      添加路径后请通过该命令查看是否添加成功：

echo $ROS_PACKAGE_PATH

      在添加完路径后，即使以上命令输出路径中包含ORB-SLAM2的ROS包，但是在编译的时候仍然无法识别出来（出现报错rospackage路径不正确）。此时就需要重启电脑，然后重新按照以下命令对ORB-SLAM2 ROS进行编译。

      确认添加成功后进入ORB-SLAM2目录执行：

chmod +x build_ros.sh
./build_ros.sh

     完成ORB-SLAM2 ROS的编译与配置工作。在此过程中，某些时候可能会出现Eigen中Eigen Core文件无法找到的情况，这是因为相关文件的软链接出现了问题，使用以下命令修复一下就可以了（此问题也有可能在编译ZED ROS Wrapper时出现）：

pkg-config --cflags eigen3 #检测Eigen目录路径
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen #请根据检测后的路径进行更改

      2.6. CUDA7.5安装

      现在终于可以开始安装CUDA了。

      需要说明一下，为什么选择这个顺序在这个时候安装CUDA。这是因为我在配置的过程中发现如果先安装CUDA再去安装OpenCV 2.4.10会遇到'Compute 11'的错误，该错误需要更改依据Opencv 3.1的cmake格式更改cmake命令，比较麻烦。

      除此以外，我还发现有时候安装完CUDA以及CUDA自带的driver后，lib文件中的一些共享库的链接会断开，从而在安装别的一些依赖项时报错，所以在多次安装出错后，我总结出来以上的安装顺序，使得所有的依赖项和环境都能够正确的安装和配置。

      根据ZED SDK要求，Ubuntu 14最好使用CUDA7.5，因此从Nvidia官网上下载安装文件以及安装指南，然后执行以下步骤：

      首先，做相关的安装前电脑配置检查，该部分具体内容请详细参照CUDA 7.5安装指南第二章。    

      然后，安装CUDA runfile文件。由于CUDA需要Nvidia驱动程序，且该程序不能与Ubuntu开源的 Nouveau共存，因此首先要关闭掉Nouveau。

lsmod | grep nouveau #检测Nouveau是否开启，有输出则开启，无输出则为开启
#若Nouveau开启，执行以下命令
sudo gedit /etc/modprobe.d/blacklist-nouveau.conf #创建blacklist文件
# 在创建的文件中输入以下内容关闭掉Nouveau
blacklist nouveau
options nouveau modeset=0
# 更新list，使得更改生效
sudo update-initramfs -u

      通常在完成以上步骤后，更改并未立即生效，需要登出用户或重启电脑才可生效。

      当你重新进入电脑界面后发现分辨率降低，则代表blacklist多半设置成功，Nouveau已经被关闭，不过为了确保万无一失，请在输入该命令确认状态

lsmod | grep nouveau #检测Nouveau是否开启，有输出则开启，无输出则为开启

      确认无输出后，按ctrl+alt+f1进入文本界面，输入你的ID和密码（在文本模式中请不要使用小键盘）。

      然后请输入以下命令：

sudo service lightdm stop

      输入后可俺ctrl+alt+f7看能否进入图形界面，若不能则设置成功。然后重新进入文字界面，输入以下命令安装CUDA7.5与相关组件：

sudo sh cuda_7.5.18_linux.run 

      在安装过程中，可以选则是否安装显卡驱动，CUDA toolkit以及OpenGL库等，这个可以自行选择。需要注意OpenGL选择请参照安装指南所属。

      安装完成后，查看显示信息，如果显示installed则安装成功。输入

sudo service lightdm start

      回到图形界面，输入账户密码，如果没有出现循环登陆界面则说明CUDA7.5基本安装成功。

      最后一步，配置CUDA的环境，输入：

export PATH=/usr/local/cuda-7.5/bin:$PATH
export LD_LIBRARY_PATH=/usr/local/cuda-7.5/lib64

       然后输入

cat /proc/driver/nvidia/version #检查显卡驱动版本
nvcc -V #检查CUDA版本

      若输出无误则代表CUDA安装成功。由于CUDA环境配置需要在每次开机后都进行设置，因此为了简便建议执行以下操作：

sudo gedit /etc/profile
export PATH=/usr/local/cuda-7.5/bin:$PATH
export LD_LIBRARY_PATH=/usr/local/cuda-7.5/lib64

      更多内容各位可参考CUDA安装指南和博客：http://blog.csdn.net/masa_fish/article/details/51882183

      2.7. ZED SDK安装

      根据自身软件环境我选择的是 'ZED_SDK_Linux_Ubuntu14_CUDA75_v1.1.0.run' 进行安装。

      命令如下：

chmod +x your_installation_file_name 
./your_installation_file_name

      然后就会进入SDK安装了，安装时最开始需要用户按Q键确认license等信息才能安装。安装好后ZED存放于路径/usr/local/zed/中，相关操作文件在/tool/中，在运行ZED SDK前请下载相机对应的 calibration file放入/setting/文件中。

      2.8. ZED ROS Wrapper安装

      安装好 SDK后就可以安装ZED和ROS配套的包了。

      进入ZED ROS Wrapper的下载目录地址输入完成安装：

cd ~/catkin_ws
catkin_make
source ./devel/setup.bash

      在此过程中，某些时候可能会出现Eigen中Eigen Core文件无法找到的情况，这是因为相关文件的软链接出现了问题，使用以下命令修复一下就可以了：

pkg-config --cflags eigen3 #检测Eigen目录路径
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen #请根据检测后的路径进行更改

      也可能出现报错" On Ubuntu 14, change line 30 of CMakeLists.txt to CUDA 7.5"，根据错误提示我们只需要在ZED ROS Wrapper CMakeLists.txt文件的CUDA版本改为7.5就可以了，该文件的路径为：zed_ros/catkin_ws/src/zed-ros-wrapper。

else() # Ubuntu Desktop
    SET(OCV_VERSION "3.1.0")
    SET(CUDA_VERSION "7.5") #更改版本号为7.5
    message("*** On Ubuntu 14, change line 30 of CMakeLists.txt to CUDA 7.5")

      此过程中还会出现OpenCV报错，这是因为ZED ROS Wrapper所需求的OpenCV版本为3.1，但是之前为了使ORB-SLAM2适配ROS Indigo我们将电脑的默认OpenCV版本设置为了2.4.10。可以通过以下命令查看电脑当前的OpenCV版本号。

pkg-config --modversion opencv  

       现在我们电脑中默认的OpenCV版本为2.4.10但是其中也安装了3.1.0版本的，所以我们可以通过OpenCV多版本管理，使得在编译ZED ROS Wrapper时使用OpenCV3.1.0。

      2.9. OpenCV多版本管理

      OpenCV的多版本管理可以在CMake或Makefile下进行。由于我们是应用的ORB-SLAM2因此采用CMake下指定OpenCV版本的方式。

      首先我们进入OpenCV3.1.0的安装目录，在/build目录中找到OpenCVConfig.cmake文件。该文件通常会指定CMake寻找OpenCV的路径，如：

# ======================================================
# Include directories to add to the user project:
# ======================================================

# Provide the include directories to the caller
set(OpenCV_INCLUDE_DIRS "/home/li/software/opencv-3.1.0/build" "/home/li/software/opencv-3.1.0/include" "/home/li/software/opencv-3.1.0/include/opencv")

# ======================================================
# Link directories to add to the user project:
# ======================================================

      要想使自己的工程调用该版本的OpenCV，只需要确保在被调用工程的CMakeLists.txt文件中将OpenCV_DIR指定为上述路径即可。在ZED ROS Wrapper中，我们在CMakeLists.txt中加入如下语句：

# OpenCV多版本管理
set(OpenCV_DIR "/home/li/software/opencv-3.1.0/build") # 指定路径
message (STATUS "This is opencv_dir" ${OpenCV_DIR}) # 打印确认路径是否正确

      完成更改后保存，重新执行2.9中的步骤就可以成功完成ZED ROS Wrapper的安装了。ZED ROS Wrapper更改后完整代码如下：

cmake_minimum_required(VERSION 2.8.7)

project(zed_wrapper)

# if CMAKE_BUILD_TYPE is not specified, take 'Release' as default
IF(NOT CMAKE_BUILD_TYPE)
    SET(CMAKE_BUILD_TYPE Release)
ENDIF(NOT CMAKE_BUILD_TYPE)

find_package(ZED 1.1 REQUIRED)

#If you have compilation issues, edit OCV_VERSION and CUDA_VERSION according to the following values :
#ZED SDK 1.1.1 : (https://www.stereolabs.com/developers/#download_anchor)
#   TX1 Jetpack 2.3 : OpenCV 2.4, CUDA 8.0
#   TX1 Jetpack < 2.3 : OpenCV 2.4, CUDA 7.0
#   TK1 : OpenCV 2.4, CUDA 6.5
#   Ubuntu 14 : OpenCV 3.1, CUDA 7.5
#   Ubuntu 16 : OpenCV 3.1, CUDA 8.0

##For Jetson, OpenCV4Tegra is based on OpenCV2.4
exec_program(uname ARGS -p OUTPUT_VARIABLE CMAKE_SYSTEM_NAME2)
if ( CMAKE_SYSTEM_NAME2 MATCHES "aarch64" ) # Jetson TX1
    SET(OCV_VERSION "2.4")
    SET(CUDA_VERSION "8.0")
    SET(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
elseif(CMAKE_SYSTEM_NAME2 MATCHES "armv7l" ) # Jetson TK1
    SET(OCV_VERSION "2.4")
    SET(CUDA_VERSION "6.5")
    SET(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
else() # Ubuntu Desktop
    SET(OCV_VERSION "3.1.0")
    SET(CUDA_VERSION "7.5")
    message("*** On Ubuntu 14, change line 30 of CMakeLists.txt to CUDA 7.5")
endif()
# OpenCV多版本管理
set(OpenCV_DIR "/home/li/software/opencv-3.1.0/build")
message (STATUS "This is opencv_dir" ${OpenCV_DIR})
find_package(OpenCV ${OCV_VERSION} COMPONENTS core highgui imgproc REQUIRED)
find_package(CUDA ${CUDA_VERSION} REQUIRED)

find_package(PCL REQUIRED)

find_package(catkin REQUIRED COMPONENTS
  image_transport
  roscpp
  rosconsole
  sensor_msgs
  dynamic_reconfigure
  tf2_ros
  pcl_conversions
  nodelet
)

generate_dynamic_reconfigure_options(
  cfg/Zed.cfg
)

catkin_package(
  CATKIN_DEPENDS
    roscpp
    rosconsole
    sensor_msgs
    opencv
    image_transport
    dynamic_reconfigure
    tf2_ros
    pcl_conversions
)

###############################################################################
# INCLUDES

# Specify locations of header files.
include_directories(
    ${catkin_INCLUDE_DIRS}
    ${CUDA_INCLUDE_DIRS}
    ${ZED_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
)

link_directories(${ZED_LIBRARY_DIR})
link_directories(${CUDA_LIBRARY_DIRS})
link_directories(${OpenCV_LIBRARY_DIRS})
link_directories(${PCL_LIBRARY_DIRS})

###############################################################################

###############################################################################
# EXECUTABLE

add_definitions(-std=c++11)# -m64) #-Wall)

set(LINK_LIBRARIES
  ${catkin_LIBRARIES}
  ${ZED_LIBRARIES}
  ${CUDA_LIBRARIES} ${CUDA_nppi_LIBRARY} ${CUDA_npps_LIBRARY}
  ${OpenCV_LIBS}
  ${PCL_LIBRARIES})

add_library(ZEDWrapper src/zed_wrapper_nodelet.cpp)
target_link_libraries(ZEDWrapper ${LINK_LIBRARIES})
add_dependencies(ZEDWrapper ${PROJECT_NAME}_gencfg)

add_executable(zed_wrapper_node src/zed_wrapper_node.cpp)
target_link_libraries(zed_wrapper_node ZEDWrapper ${LINK_LIBRARIES})
add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)

###############################################################################

#Add all files in subdirectories of the project in
# a dummy_target so qtcreator have access to all files
FILE(GLOB_RECURSE extra_files ${CMAKE_SOURCE_DIR}/*)
add_custom_target(dummy_${PROJECT_NAME} SOURCES ${extra_files})

###############################################################################
# INSTALL

install(TARGETS
  ZEDWrapper
  zed_wrapper_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(FILES
  nodelet_plugins.xml
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY
  launch
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

      2.10. ROS Topic更改

      在完成以步骤后我们仍需要更改ROS或ZED ROS Wrapper中topic的名称才能使得ORB-SLAM2 ROS与ZED相机相配合。

      根据ZED ROS Wrapper官网（http://www.stereolabs.com/blog/index.php/2015/09/07/use-your-zed-camera-with-ros/）中的信息，我们可以查询到ZED ROS Wrapper所发布的topic名称。我们可以选择更改ZED的相关文件使得其匹配ORB-SLAM2 ROS的topic名字，也可以反过来更改更改ORB-SLAM2的topic名字。我选择的是更改ORB-SLAM2 ROS所接收的Topic名称。

      ORB-SLAM2 ROS相关的Topic名称在路径ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/中的相关文件中（式例如截图所示，ros_stereo_rect.cc为我自己创建的）。      

      选择你所想要更改的文件，并将其内容更改为如下所示：

message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/zed/left/image_raw_color", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/zed/right/image_raw_color", 1);

      保存，并重新2.6中ROS部分重新编译。然后分别运行ZED ROS Wrapper和ORB-SLAM2 ROS即可实现ORB-SLAM2实时使用ZED相机的输入图像进行运算的功能了。

      2.11. ZED ROS Wrapper输出的图像相关内容的更改

      ZED ROS Wrapper中输出的图像的像素，fps等参数都是我们可以自行更改的。只需要更改zed-ros-wrapper/launch/zed_camera.launch文件中的参数即可。该文件中各参数定义如下：

      这样我们就能选择与我们自己编写的ORB-SLAM2 setting file相匹配的输入图像了。

      至此，ORB-SLAM2与ZED相关的配置与调试工作就基本全部完成，我们可以在ROS下进行实时的调试与测试，也可以在非ORB情况下通过ZED相机构建自己的数据集检测ORB-SLAM2在不同情况下的运行效果。

     如果博客中内容有任何错误还望大家告知，祝各位配置顺利。