Skimap_ros uses RGBD to create Octomap (1)

1. Obi Zhongguang astra RGBD camera installation

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 installation test

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 Combine with ORB_SLAM2

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

3.1 Publish tf
and make the following modifications in ros_rgbd.cc of ORB_SLAM2

    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 Running results It
may be a computer problem, the running speed is relatively slow, and as long as the tracking is lost, the ORB will hang up, causing the map to stop creating. There may be some minor issues in there that need to be addressed.