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就掛掉，導致地圖也停止創建。里面可能有一些小問題需要解決。可以看到，路面創建的還是比較平坦的，牆面以及障礙物位置都還不錯。