# Scene Understanding and Positioning System
---

## 環境設定
系統平台 : Ubuntu 18.04
Python版本 (Ubuntu): 3.9.7
Python版本 (Anaconda): 3.6.10
OpenCV : 3.2
DBoW2, g2o : 官方ORB-SLAM2中提供的版本，存在於Thirdparty資料夾中
Eigen3 : 3.2
ROS : melodic 2018

---

## 從零開始安裝

0. 重灌Ubuntu 18.04，應該不會有問題，一般我都使用隨身碟安裝

1. 重灌完後先安裝ROS，安裝方式可查看官方的教學
http://wiki.ros.org/melodic/Installation/Ubuntu

2. 再來安裝OpenCV，最花時間的一個步驟

* 安裝開發者工具

    sudo apt update
    sudo apt upgrade
    sudo apt install build-essential cmake unzip pkg-config

* 安裝 image I/O package，以便 opencv 處理這些圖片格式

    sudo apt install libjpeg-dev libpng-dev libtiff-dev

* 安裝 jasper library，其中包含了多種圖像操作庫

    sudo add-apt-repository "deb http://security.ubuntu.com/ubuntu xenial-security main"
    sudo apt update
    sudo apt install libjasper1 libjasper-dev

* 安裝影像流相關 package

    sudo apt install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev
    sudo apt install libxvidcore-dev libx264-dev

* 安裝 opencv 中 highgui module 的依賴 GTK library

    sudo apt install libgtk-3-dev

* 下述 library 可以優化 opencv 的某些操作

    sudo apt install libatlas-base-dev gfortran

* 安裝 python 3.6 的相關 headers 和 libraries

    sudo apt install python3.6-dev

* 獲取 OpenCV (版本3.2)

    wget https://github.com/opencv/opencv/archive/3.2.0.tar.gz

    tar -xvzf 3.2.0.tar.gz

    wget https://github.com/opencv/opencv_contrib/archive/3.2.0.zip

    unzip 3.2.0.zip

* 編譯OpenCV

    cd ~/opencv

    mkdir build

    cd build

    cmake -D CMAKE_BUILD_TYPE=RELEASE \
     -D CMAKE_INSTALL_PREFIX=/usr/local \
     -D WITH_CUDA=OFF \
     -D INSTALL_PYTHON_EXAMPLES=ON \
     -D OPENCV_EXTRA_MODULES_PATH=~/opencv_contrib/modules \
     -D OPENCV_ENABLE_NONFREE=ON \
     -D BUILD_EXAMPLES=ON ..

如果有出現任何Error，可能要上網Google或看下面這個網站
https://www.jianshu.com/p/403ab3aa04ed

* 編譯OpenCV

    sudo make install    # 默認安裝到 /usr/local/bin 目錄中

    sudo ldconfig  # 更新動態鏈接庫

* 檢查安裝是否成功

    ldconfig -p | grep opencv  # -p = print

3. 安裝Eigen3，使用3.2的版本

    wget  https://gitlab.com/libeigen/eigen/-/archive/3.2.10/eigen-3.2.10.tar.gz

    tar -xvzf eigen-3.2.10.tar.gz 

    cd eigen-3.2.10

    mkdir build

    cd build

    cmake ..

    sudo make install

4. 安裝Pangolin，記得是安裝0.5的版本！

    # OpenGL
    sudo apt install libgl1-mesa-dev

    # Glew
    sudo apt install libglew-dev

    # CMake
    sudo apt install cmake

    # python 
    sudo apt install libpython2.7-dev
    sudo python -mpip install numpy pyopengl Pillow pybind11

    # Wayland
    sudo apt install pkg-config
    sudo apt install libegl1-mesa-dev libwayland-dev libxkbcommon-dev wayland-protocols

    # FFMPEG (For video decoding and image rescaling)
    sudo apt install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libavdevice-dev

    # DC1394 (For firewire input)
    sudo apt install libdc1394-22-dev libraw1394-dev

    # libjpeg, libpng, libtiff, libopenexr (For reading still-image sequences)
    sudo apt install libjpeg-dev libpng-dev libtiff5-dev libopenexr-dev

    # build Pangolin (記得要上網找版本0.5的下載，這邊容易出錯)
    git clone https://github.com/stevenlovegrove/Pangolin.git

    cd Pangolin

    mkdir build

    cd build

    cmake ..

    cmake --build .    # 注意最後那個點

安裝完後可以測試一下安裝是否成功

    cd ~/Pangolin/build/examples/HelloPangolin

    ./HelloPangolin 

如果顯示一個彩色立方體，並且可以通過鼠標左、右鍵和滾輪按住拖拽，就表示 Pangolin 安裝成功了。

5. 編譯ORB-SLAM2
需要在ROS的建出的環境中編譯
ROS的介紹可看這個網站 : https://ithelp.ithome.com.tw/users/20112348/ironman/1965

基本上就是

    mkdir catkin_ws
    cd catkin_ws
    mkdir src
    catkin_make

之後要編譯ROS環境的C++程式就在src資料夾下進行，所以ORBSLAM也是下載放進src中
(python的程式碼直接丟進src資料夾就能用了)

從 github 下載source code

    git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2

    cd ORB_SLAM2

    chmod +x build.sh

    ./build.sh

如果在編譯過程中顯示 <usleep> 相關的錯誤，則需要在 ~/ORB_SLAM2/include/System.h 相關文件中添加 #include <unistd.h>

用 ROS 模式跑 demo，還要額外編譯 ROS 文件。
首先將 ROS 所在目錄加入 ROS_PACKAGE_PATH 環境變量中，具體操作是將下述命令添加到 .bashrc 文件末尾（別忘了替換下述命令中的 PATH）：

    export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS

source 更新變量(.bashrc)之後開始編譯：

    chmod +x build_ros.sh

    ./build_ros.sh

在編譯過程中可能會出現一個錯誤：undefined reference to symbol '_ZN5boost6system15system_categoryEv'.
這是因為在編譯時沒有添加 boost_system 共享庫文件。
修改 Examples/ROS/ORB_SLAM2/CMakeLists.txt 文件中的如下內容：

    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    
    )

只需要在最後加上 -lboost_system

6. 在ROS中編譯USB-Camera

先在~/.bashrc 最下面添加

    source /opt/ros/melodic/setup.bash`
    source ~/catkin_ws/devel/setup.bash`

* 編譯usb_cam
[usb_cam下載鏈接] (https://github.com/bosch-ros-pkg/usb_cam)
把usb_cam放到catkin_ws/src下

    cd catkin_ws/src/usb_cam
    mkdir build
    cd build
    cmake ..
    make

* 把usb camera數據發佈到topic

    roslaunch usb_cam usb_cam-test.launch

* 修改ros_mono.cc
進入～/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src
打開 ros_mono.cc 把程序裡面的topic改為 /usb_cam/image_raw
具體還要查看自己的rviz（即上一步打開的攝像頭窗口的標題）
然後進入～/catkin_ws/src/ORB_SLAM

    chmod +x build_ros.sh
    ./build_ros.sh

* 啟動ORB_SLAM

    rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Asus.yaml

記得啟動ROS的程式前，都需要先啟動roscore

以上資料皆來自於:
https://www.jianshu.com/p/31c95d9a5f97
https://blog.csdn.net/xmy306538517/article/details/59501718
有遇到任何問題可以在上面的網站查看

---

## 我建立好的環境的執行方式

總共需要開五個Terminal，分別用來執行
1. 從client接受影像的server
2. Scene Understanding module (用來輸出椅子的方向和距離)
3. YOLOv5 (用來處理Occlusion的問題)
4. ORBSLAM2
5. roscore

以下的操作都是在ROS資料夾(如catkin)下執行

1. 在catkin_ws資料夾下開啟roscore

2. 在src/video_server/src資料夾下，開啟接收影像的server

    conda activate Im3D
    
    source ~/catkin_ws/devel/setup.bash
    
    python csharp_server.py

如果出現OSError: [Errno 98] Address already in use

就用netstat -tlnp|grep 12003來找出占用這個port的程式的[PID]
使用 kill -9 [PID]刪除他，稍等一下後再開啟

3. 在src/Implicit3DUnderstanding/資料夾下，開啟場景理解模組

    conda activate Im3D
    
    source ~/catkin_ws/devel/setup.bash
    
    python server_tcp.py

4. 在src/yolov5資料夾下，開啟遮擋後處理的模組

    conda activate Im3D
    
    source ~/catkin_ws/devel/setup.bash
    
    python yolo_tcp.py

5. 在src/ORB_SLAM2資料夾下，開啟空間定位模組

    rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Asus.yaml

---

## 一些細節

關於server的程式碼在src/video_server/src/csharp_server.py

程式碼可以參考https://gist.github.com/kittinan/e7ecefddda5616eab2765fdb2affed1b

關於場景理解的程式碼在src/Implicit3DUnderstanding/server_tcp.py

關於遮擋處理的程式碼在src/yolov5/yolo_tcp.py

關於空間定位模組的程式碼在src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_mono.cc

主要要修改程式碼就在以上提到的這幾個檔案中修改就好