---
# System prepended metadata

title: V-slam資料收集 for 工研院

---

V-slam資料收集 for 工研院
===
ROS(Robot Operating System) 
install:http://wiki.ros.org/ROS/Installation

![](https://i.imgur.com/3QUJSyU.png)

![](https://i.imgur.com/bFLf2ra.png)



ROS Package
===
ROS需要在特定的工作區間下通過ros編譯運行自己的程式
Node皆運行在樹莓派上

```
cd
mkdir -p catkin_ws/src/
cd catkin_ws/
catkin_make
source ~/catkin_ws/devel/setup.bash
```

Cameras Node(C++)
---
* create package
    ```
    cd catkin_ws/src/
    catkin_create_pkg vslam roscpp std_msgs cv_bridge sensor_msgs image_transport
    #vslam 為Package名稱
    #roscpp std_msgs cv_bridge sensor_msgs image_transport 需要include的模組
    ```

* 在vslam/src中放入要編譯的[code下載](https://drive.google.com/file/d/1QCdW29D36o09OhxGeYkHw6sQKIjsE6fE/view?usp=sharing)
![](https://i.imgur.com/CaDSnoy.png)

* 接著修改CMakeLists.txt 
 加入 add_executable & target_link_libraries

    ```
    //...上略
    ## Declare a C++ executable
    ##...
    ##...

    add_executable(cam_Pub.exe src/cam_Pub.cpp)
    target_link_libraries(cam_Pub.exe
        ${catkin_LIBRARIES}
        ${OpenCV_LIBS}
    )

    //...下略
    ```

* 編譯
    ```
    cd ~/catkin_ws
    catkin_make
    ```
* 完成後可在~/catkin_ws/devel/lib看到執行檔 
![](https://i.imgur.com/mdarPKt.png)

* 運行
    ```
    1. 開啟一個 Terminal 建立 ROS master
    roscore

    2. 開啟第二個 Terminal 執行程式
    source ~/catkin_ws/devel/setup.bash
    rosrun vslam cam_Pub.exe

    3. 開啟第三個 Terminal 觀測
    rostopic hz /image 或用 rqt_image_view 查看
    ```

IMU Node(Python)
---
Python不須像C++經過編譯，可直接執行 
各IMU如何讀取的方式皆有不同,這邊以MUP-9250為主,
[code下載](https://drive.google.com/file/d/1SFycx0N2bgIGF33v46PPt5jpM1yATYee/view?usp=sharing)
傳輸的資料型態為[Imu Message](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/Imu.html)

![](https://i.imgur.com/ki8Z3Uj.png)
NED(North-X-axis, East-Y-axis and Down-Zaxis)

```
1. 開啟一個 Terminal 建立 ROS master
roscore

2. 開啟第二個 Terminal 執行程式
python3 imu_ros_Pub.py

3. 開啟第三個 Terminal 觀測
rostopic echo /imu_9250
```


標定
===
標定皆在PC上運行


Camera標定
---
使用IMX219-160單目魚眼相機
[code下載](https://drive.google.com/drive/folders/1ONekU6dlmkQ_sVAr-Q6XlOiyOIHgr7Fe?usp=sharing)

使用方法
* 使用自己的相機在不同角度對棋盤格拍照(20張以上)放入圖片檔案內
* 對get_K_and_D.py內的參數進行設定
 ![](https://i.imgur.com/BT1TB5r.png)
* 執行get_K_and_D.py
 ![](https://i.imgur.com/lSzoDxB.png)
 K為內參矩陣(Intrinsic Matrix)
 D為畸變參數(Distortion Parameter)
 
使用參數對圖片進行校正
![](https://i.imgur.com/zTYbh9g.jpg =50%x)![](https://i.imgur.com/FRDi2Kk.jpg =50%x)



.

IMU標定
---

* 首先需安裝[ceres](http://ceres-solver.org/installation.html) 
    安裝依賴項
    ```
    # CMake
    sudo apt-get install cmake
    # google-glog + gflags
    sudo apt-get install libgoogle-glog-dev libgflags-dev
    # Use ATLAS for BLAS & LAPACK
    sudo apt-get install libatlas-base-dev
    # Eigen3
    sudo apt-get install libeigen3-dev
    # SuiteSparse and CXSparse (optional)
    sudo apt-get install libsuitesparse-dev
    ```
    安裝 Ceres
    ```
    cd 
    mkdir -p ceres-bin
    cd ceres-bin
    git clone https://github.com/ceres-solver/ceres-solver
    cd ceres-solver
    mkdir build
    cd build
    cmake ..
    make
    sudo make install

    sudo apt-get install libdw-dev
    ```
* 下載編譯code_utils
    ```
    cd catkin_ws/src/ #如果沒有創建一個
    git clone https://github.com/gaowenliang/code_utils
    ==========================
    接著修改code_utils/src/sumpixel_test.cpp
    #include "backward.hpp" -> #include "code_utils/backward.hpp" 
    normalize( img, img2, 0, 255, CV_MINMAX);中的 CV_MINMAX -> cv::NORM_MINMAX #有兩處
    ==========================
    cd ~/catkin_ws
    catkin_make
    ```
* 下載編譯imu_utils
    需完成編譯code_utils後在操作
    ```
    cd ~/catkin_ws/src/
    git clone https://github.com/gaowenliang/imu_utils.git
    cd ~/catkin_ws
    catkin_make
    ```
* 標定準備
先在樹莓派上錄製imu.bag
讓IMU靜止不動兩個小時(時間越長越好)，錄製IMU的bag
首先先執行 IMU Node(Python) 在樹莓派上
開新的Terminal
    ```
    rosbag record /imu_9250 -O imu
    #時間到按ctrl+c結束錄製
    ```
    到~/catkin_ws/src/imu_utils/launch 底下新增自己的 launch 檔案
    ```
    <launch>
        <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
            <param name="imu_topic" type="string" value= "/imu_9250"/><!--imu topic 名稱-->
            <param name="imu_name" type="string" value= "9250"/>
            <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/><!--放標定結果的路徑-->
            <param name="max_time_min" type="int" value= "120"/><!--錄製imubag的時長,120為120min-->
            <param name="max_cluster" type="int" value= "100"/>
        </node>
    </launch>
    ```
* 開始標定
    先將bag檔案傳到跑標定程式的PC上(在樹莓派上應該也可以)
    開一個Terminal
    ```
    roscore
    ```
    開第二個Terminal
    ```
    ＃cd到imu bag檔案的路徑
    rosbag play  --pause  -r 200 imu.bag # 200倍速播放rosbag
    ```
    開第三個Terminal
    ```
    cd ~/catkin_ws
    source devel/setup.bash
    roslaunch imu_utils my_imu.launch
    ```
* 標定結果 
    標定結果的路徑可看launch尋找
    ![](https://i.imgur.com/NrwuFCh.png =30%x)
    單位原作者打錯了,應該為
    ```
    gyr_n: rad/√s     = rad/s^0.5
    gyr_w: (rad/s)/√s = rad/s^1.5
    acc_n: (m/s)/√s   = m/s^1.5
    acc_w: (m/s²)/√s  = m/s^2.5
    ```
    [參考](https://github.com/gaowenliang/imu_utils/issues/36) [參考2](https://github.com/gaowenliang/imu_utils/issues/30#issuecomment-793304356)

Cameras ＆ IMU聯合標定
---
install kalibr
[安裝流程](https://github.com/ethz-asl/kalibr/wiki/installation)
```
#編譯遇到這問題
#fatal error: libv4l2.h: No such file or directory

打這
sudo apt-get install libv4l-dev
```
* 產生標定圖

    ```
    cd ~/kalibr_workspace
    source ~/kalibr_workspace/devel/setup.bash
    rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.02 --tspace 0.3
    ```
* 錄製bag
    ```
    rosbag record /imu_9250 /image  -O imu_cam_kalibr
    ```
    [如何錄製](https://www.youtube.com/watch?v=puNXsnrYWTY)
    {%youtube puNXsnrYWTY %}
* 標定準備
需要以下4個輸入
    * imu_cam.bag 
    包含圖像和 IMU 數據的 ROS bag
    * camchain.yaml
    相機的校正參數,格式如下
        ```
        cam0:
          cam_overlaps: []
          camera_model: pinhole
          distortion_coeffs: [-0.03381979493008404, -0.002564048518541289, 0.0011786084137134709, -0.0021784473475429064]
          distortion_model: equidistant
          intrinsics: [309.06186961534786, 309.1327446505413, 330.92153214915896, 216.00863692319797]
          resolution: [640, 480]
          rostopic: /image
        ```
    * imu.yaml
    格式如下
        ```
        rostopic: /imu_9250
        update_rate: 100.0 #Hz

        accelerometer_noise_density: 5.1265334403965163e-02 #continous
        accelerometer_random_walk: 1.5701833626164503e-03
        gyroscope_noise_density: 2.6016129941557925e-03 #continous
        gyroscope_random_walk: 5.8833811832597509e-05
        ```
    * target.yaml
    標定圖的配置,格式如下
        ```
        target_type: 'aprilgrid' #gridtype
        tagCols: 6               #number of apriltags
        tagRows: 6               #number of apriltags
        tagSize: 0.02           #size of apriltag, edge to edge [m]
        tagSpacing: 0.3          #ratio of space between tags to tagSize
        codeOffset: 0            #code offset for the first tag in the aprilboard
        ```    
* 開始標定
    ```
    cd ~/kalibr_workspace
    source ~/kalibr_workspace/devel/setup.bash

    rosrun kalibr kalibr_calibrate_imu_camera --bag [filename.bag] --cam [camchain.yaml] --imu [imu.yaml] --target [target.yaml] #--bag-from-to 5 150
    ```
    
     如果出現RuntimeError: Optimization failed!
到~/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python
 底下有個kalibr_calibrate_imu_camera.py
 搜尋timeOffsetPadding,大約在200行
![](https://i.imgur.com/zrygyZh.png)
預設值是0.03,把它增大即可,記得到重新編譯
    ```
    cd ~/kalibr_workspace/
    catkin build -DCMAKE_BUILD_TYPE=Release -j4
    ```
    ---
    #ModuleNotFoundError: No module named    'wx'
    在這邊[下載](https://extras.wxpython.org/wxPython4/extras/linux/gtk3/ubuntu-20.04/)安裝

* 標定結果

  轉移矩陣
  ![](https://i.imgur.com/q8K9bNL.png)



