Skimap_ros 利用RGBD创建Octomap(一)

1. 奥比中光astra RGBD相机安装

1.1 安装依赖
$ sudo apt-get install build-essential freeglut3 freeglut3-dev

1.2 检查udev版本，需要libudev.so.1，如果没有则添加

#check udev version, Orbbec Driver need libudev.so.1, if can't find it, can make symbolic link from libudev.so.x.x,
#which usually locate in /lib/x86_64-linux-gnu or /lib/i386-linux-gnu
$ ldconfig -p | grep libudev.so.1
$ cd /lib/x86_64-linux-gnu
$ sudo ln -s libudev.so.x.x.x libudev.so.1

1.3 下载 OpenNI
$ wget http://www.orbbec3d.net/Tools_SDK_OpenNI/2-Linux.zip

1.4 选择解压 OpenNI-Linux-x64-2.3
$ unzip OpenNI-Linux-x64-2.3.zip
$ cd OpenNI-Linux-x64-2.2    

1.5 安装
$ sudo chmod a+x install.sh
$ sudo ./install.sh

1.6 重插设备

1.7 加入环境
$ source OpenNIDevEnvironment

1.8 编译例子
$ cd Samples/SimpleViewer
$ make
1.9 连接设备，执行例子
$ cd Bin/x64-Release
$ ./SimpleViewer 

$ cd Bin/x64-Release
$ ./SimpleViewer 
无没有问题，则显示正常视图
1.10 安装ROS包
安装astra_camera和astra_launch(indigo 可以根据系统不同替换成kinetic等）
$ sudo apt-get install ros-indigo-astra-camera ros-indigo-astra-launch  
测试

1.11 新终端，执行astra_launch
$ roslaunch astra_launch astra.launch  
使用rqt_image_view, 选择对应话题显示图像

1.12 发现rgb并没有正确显示？ 需要UVC支持，安装libuvc和libuvc_ros

libuvc和libuvc_ros安装
安装libuvc支持
$ cd ~
$ git clone https://github.com/ktossell/libuvc
$ cd libuvc
$ mkdir build
$ cd build
$ cmake ..
$ make && sudo make install

安装libuvc_ros
$ cd ~/catkin_ws/src
$ git clone https://github.com/ktossell/libuvc_ros
$ cd ..
$ catkin_make 

注意：使用catkin_make，如果报错，注意libusb.h的位置。
使用locate libusb.h，并将其放到合适位置：
$ sudo cp /usr/include/libusb-1.0/libusb.h  /usr/local/include/libuvc/
修改下libuvc.h中include中的路径。

测试
在启动roscore后，使用：
$ rosrun libuvc_camera camera_node
rqt_image_view ，查看rgb

2. skipmap 安装测试

2.1 创建workspace 
找到某个目录下，我这里选择Documents
$ mkdir skimap_ws/src
$ cd skimap_ws/src

2.2 下载
$ git clone https://github.com/m4nh/skimap_ros.git
安装依赖项，opencv，eigen，boost都是比较常见的，OpenMP 大家安装不熟悉
$ sudo apt-get install build-essential
$ sudo apt-get install gfortran
$ cd /tmp
$ wget http://www.mpich.org/static/downloads/1.4.1/mpich2-1.4.1.tar.gz
$ tar xzvf mpich2-1.4.1.tar.gz  
$ cd mpich2-1.4.1/
$ ./configure
$ make
$ sudo make install
查看版本：
$ mpich2version

2.3 编译
$ cd ..
$ catkin_make

2.4 下载[tiago_lar.bag](https://drive.google.com/file/d/0B02158j5inr3Tm9nQjhIQ3Fua3c/view?usp=drive_web) demo数据

2.5 运行
$ source ./devel/setup.bash
$ roslaunch skimap_ros skimap_live.launch
$ rosbag play 你下载的tiago_bar.bag路径

按照作者给的步骤，运行slamdunk_tracker.launch 总是出错，

ERROR: cannot launch node of type [skimap_ros/slamdunk_tracker]: can't locate node [slamdunk_tracker] in package [skimap_ros]

感觉是Cmakelist出了问题，发现作者只写了：

add_executable(skimap_live src/nodes/skimap_live.cpp)
target_link_libraries(skimap_live ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})

add_executable(slamdunk_tracker src/nodes/slamdunk_tracker.cpp)
target_link_libraries(slamdunk_tracker ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})

没有把slamdunk_tracker.cpp加进去，所以我追加了

add_executable(skimap_map_service src/nodes/skimap_map_service.cpp)
target_link_libraries(skimap_map_service ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
add_dependencies(skimap_map_service skimap_ros_generate_messages_cpp)

但是在catkin_make时，总是出现feature2d方面的错误，因为作者用了sift，所以opencv要装opencv_contrib模块。opencv 和contrib我都安装了3.4.1版本，但是还是出现错误。我以后还会继续解决这个问题。但是这个不影响建图，真正的建图是在skimap_live.cpp里。

3 与ORB_SLAM2 结合

skimap可以只需要位姿数据，以及RGB和深度图就能重建出来八叉树地图。所以可以把ORB_SLAM 里面的pose取出来，转换成tf发布出来，给skimap就可以。

3.1 发布tf
在ORB_SLAM2 的 ros_rgbd.cc 作如下修改

    cv::Mat camera_pose;
    camera_pose= mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    
    cv::Mat TWC = camera_pose.inv();  
    cv::Mat RWC= TWC.rowRange(0,3).colRange(0,3);  
    cv::Mat tWC= TWC.rowRange(0,3).col(3);

    tf::Matrix3x3 M(RWC.at<float>(0,0),RWC.at<float>(0,1),RWC.at<float>(0,2),
        RWC.at<float>(1,0),RWC.at<float>(1,1),RWC.at<float>(1,2),
        RWC.at<float>(2,0),RWC.at<float>(2,1),RWC.at<float>(2,2));
    tf::Vector3 V(tWC.at<float>(0), tWC.at<float>(1), tWC.at<float>(2));
    tf::Quaternion q;
    M.getRotation(q);
    q.normalize();

    tf::Transform transform = tf::Transform(M, V);
   //关于应该发布何时的ros::Time，还需要再确认
    broadcaster->sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world","camera"));

不要忘记，增加
#include <tf/transform_broadcaster.h> 以及在CMakeList.txt中增加 find_package(tf)

3.2 运行astra 以及skimap_live
$ roslaunch astra_launch astra.launch
$ roslaunch skimap_ros skimap_live.launch

3.3 运行结果
可能是电脑问题，运行速度比较卡，而且只要跟丢，ORB就挂掉，导致地图也停止创建。里面可能有一些小问题需要解决。可以看到，路面创建的还是比较平坦的，墙面以及障碍物位置都还不错。