V-slam資料收集 for 工研院
===
ROS(Robot Operating System)
install:http://wiki.ros.org/ROS/Installation
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)
* 接著修改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看到執行檔
* 運行
```
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)
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內的參數進行設定
* 執行get_K_and_D.py
K為內參矩陣(Intrinsic Matrix)
D為畸變參數(Distortion Parameter)
使用參數對圖片進行校正
.
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尋找
單位原作者打錯了,應該為
```
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行
預設值是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/)安裝
* 標定結果
轉移矩陣
Sign in with Wallet
Connect another wallet