使用自己的INDEMIND相机来运行ORBSLAM2单目,双目和深度模式(小觅相机和realsense通用)

配置ROS

ROS环境准备(以１6.04 ROS Kinetic为例)

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /
, → etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key␣
, → C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get update
sudo apt-get install ros-kinetic-desktop-full
sudo rosdep init
rosdep update
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

创建自己的工作空间:

mkdir -p ~/catkin_ws/src  //创建文件夹
cd ~/catkin_ws/src        //定位到src
catkin_init_workspace     //对工作空间进行初始化

配置ORBSLAM

可以直接git,网速不好的可以直接去下压缩包:
下载地址:https://webdiis.unizar.es/~raulmur/orbslam/:.

编译ORBSLAM2 ROS

按照github上步骤准备好依赖库之后,执行:

cd catkin_ws/src
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh
#我们要用到他们ros库,所以执行
chmod +x build_ros.sh
./build_ros.sh

如果执行顺利,我们可以在Examples的ROS/ORBSLAM2下看到生成好的Mono,MonoAR,RGBD,Stereo文件.

常见错误及解决

运行build-ros.sh时出现问题一:

[rosbuild] rospack found package "ORB_SLAM2" at "", but the current   directory is "/home/angelo/ORB_SLAM2/Examples/ROS/ORB_SLAM2".  You should   double-check your ROS_PACKAGE_PATH to ensure that packages are found in the   correct precedence order.

解决方案:

 sudo ln -s ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2 /opt/ros/<ros版本名>/share/ORB_SLAM2

运行build-ros.sh时出现问题二:这个问题的核心在与链接boost库出了问题:

/usr/bin/ld: CMakeFiles/RGBD.dir/src/ros_rgbd.cc.o: undefined reference to symbol '_ZN5boost6system15system_categoryEv'
/usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status
CMakeFiles/RGBD.dir/build.make:217: recipe for target '../RGBD' failed
make[2]: *** [../RGBD] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/RGBD.dir/all' failed
make[1]: *** [CMakeFiles/RGBD.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
/usr/bin/ld: CMakeFiles/Stereo.dir/src/ros_stereo.cc.o: undefined reference to symbol '_ZN5boost6system15system_categoryEv'
/usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status
CMakeFiles/Stereo.dir/build.make:217: recipe for target '../Stereo' failed
make[2]: *** [../Stereo] Error 1
CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/Stereo.dir/all' failed
make[1]: *** [CMakeFiles/Stereo.dir/all] Error 2

解决方案:打开Examples/ROS/ORB_SLAM2的Cmakelists,在

set(LIBS 
${OpenCV_LIBS} 
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
#增加本行,有很多解决办法,这样最为简便
-lboost_system 
)

安装及配置相机INDEMIND相机SDK

Github地址: https://github.com/INDEMIND/IMSEE-SDK.
支持平台
SDK基于cmake构建，跨Ubuntu、Ｗin10平台。其中支持x64/tx2/firefly rk系列等不同架构下Ubuntu的支持。目前已完全测试通过的平台有：

Ubuntu 16.04 on x64
Ubuntu 16.04 on TX2
Ubuntu 16.04 on RK3328

编译ROS样例

已经写好了makefile文件,所以非常简单

cd <sdk>
make ros

运行

cd <sdk>
sudo su # 进入权限模式
source ros/devel/setup.bash
roslaunch imsee_ros_wrapper start.launch # 如果您想预览rviz，则运行roslaunch imsee_ros_wrapper display.launch

查看发布节点

rostopic list

我们需要的Topic名称如下:

/imsee/camera_info#相机信息
/imsee/depth#深度图
/imsee/image/left#左相机图像
/imsee/image/right#右相机图像
/imsee/imu#imu信息

这样我们就可以直接改变ORBSLAM2ROS的话题来读取相机内容

ORBSLAM2ROS读取相机内容

cd ~/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src

 ros_mono.cc #单目ros源文件
 ros_rgbd.cc #深度相机ros源文件
 ros_stereo.cc #双目ros源文件

以双目相机为例,找到ros订阅话题的部分,将其中的内容改为如下所示:

ros::NodeHandle nh;
//其中话题的部分改为IdemindSDK话题订阅的话题名称.此处位左目相机和右目相机
message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/imsee/image/left", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/imsee/image/right", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));

ros::spin();

启动相机和ORBSLAM2_ROS

启动相机

cd <sdk>
sudo su # 进入权限模式
source ros/devel/setup.bash
roslaunch imsee_ros_wrapper start.launch # 如果您想预览rviz，则运行roslaunch imsee_ros_wrapper display.launch

启动ORBSLAM2_ROS

cd ~/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2
#ORBvoc.txt为词典内容,示例自带的Asus.yaml为相机的内参格式,可以按照自己的标定结果更改,false为是否矫正标志位
rosrun ORB_SLAM2 Stereo  ../../../Vocabulary/ORBvoc.txt Asus.yaml false

如果报这个错误,就重新打开一个终端,开roscore:

[ERROR] [1591685038.629400679]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...
#重新打开一个终端,输入:
roscore

三个节点的用法

其中注意Stereo节点最后一个参数意为是否矫正,一般设为false就可以了,更深入的细节可以按需查看文档:

#单目
Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings
#深度相机
Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings
#双目相机
Usage: rosrun ORB_SLAM2 Stereo path_to_vocabulary path_to_settings do_rectify

运行结果

至此,成功运行啦: