## Allan Variance
(圖來源自[kalibr, imu noise model](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model)、[IMU 知識筆記](https://mapostech.com/imu/))

具時間序列的資料其隨機誤差一般用 Allan Variance 分析, 概念是將資料依特定時間長度分段，各段分別取平均，再求相鄰段平均值的差值序列，最後計算差值序列的標準差。
<center>

<image width=35% src=https://hackmd.io/_uploads/S1Wwc9zike.png>

</center>

當用較短的時間長度分段時，對應至資料的高頻變化也就是高頻雜訊，用較長的時間長度分段時，對應至資料的低頻變化也就是低頻雜訊。

下圖典型　Allan Variance 分析圖(log-log AD plot) 用不同斜率來判斷是哪種類型的雜訊，下圖的縱軸：對差值序列的標準差取 log; 橫軸 : 對特定時間長度取 log.

<center>

<image width=70% src=https://hackmd.io/_uploads/Hy-r7jGo1x.png>

</center>

查找方式 : 按照下方圖表, 先尋找數據上有多少斜率種類的擬合直線, 再依哪個特定時間段$\tau$ 求交點, 那就會是該雜訊的deviation. 如果想要找 ```Random Walk``` 雜訊種類的標準差, 需要找到 ```-1/2```斜率的擬合直線與 $\tau=1$ 求交點，對應出的 deviation.

<center>

<image width=70% src=https://hackmd.io/_uploads/BJPWfifjkg.png>

</center>

在 kalibr 中, 會考慮 :
1. 量測雜訊
高頻雜訊, 標準差為 $\tau=1$ 與```1/2```斜率 random walk 的交點
2. bias雜訊
低頻雜訊, 標準差為 $\tau=3$ 與```-1/2```斜率 rate random walk 的交點

如下圖所示:
<center>

<image width=70% src=https://hackmd.io/_uploads/SyUuyTzjyg.png>

</center>

### Reference
1. [kalibr, imu noise model](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model)
2. [IMU 知識筆記](https://mapostech.com/imu/)

## IMU Intrinsic Calibration Pipline
我們使用 [yahboom imu device](http://www.yahboom.net/study/IMU) + kalibr 推薦的 [allan_variance_ros](https://github.com/ori-drs/allan_variance_ros) 於 ROS1-noetic from kalibr docker.

### 環境建置 2025.03.03
#### yahboom imu device
<center>

<image width=55% src=https://hackmd.io/_uploads/BJ5552Go1l.png>

</center>

首先按照 [yahboom imu device ](http://www.yahboom.net/study/IMU) 文件說明進行, 務必全部看完.
安裝檔只會用到 ```ros1_imu_ws.zip```, ```IMU standard PC software(V6.2.60)```.
以下只會提及重點~~~
1. 設定 imu device 的採樣率至 200HZ
需要調整 baudrate .
<center>

<image width=75% src=https://hackmd.io/_uploads/HJhMooGi1e.png>

</center>

ros1 確認 imu 200hz 的方式 :
<center>

<image width=45% src=https://hackmd.io/_uploads/BJG4aozsJe.png>

</center>

2. 關閉陀螺儀的自動校正(for 靜止狀態)
為了做 instric calibration, 所以需要進行調整

<center>

<image width=75% src=https://hackmd.io/_uploads/Hk03jjziJl.png>

</center>

#### ROS1-noetic from kalibr docker
<center>

<image width=55% src=https://hackmd.io/_uploads/Hyd0q3Gjyl.png>

</center>

首先先去 [kalibr installation](https://github.com/ethz-asl/kalibr/wiki/installation) 下載 ROS1-noetic from kalibr docker :

<center>

<image width=75% src=https://hackmd.io/_uploads/B1W6vofsyg.png>

</center>

確認 docker images 名稱 by ```$docker images```, 因為接下來要以映像檔為主額外下載其他套件(包含 yahboom imu device deserializer).

<center>

<image width=75% src=https://hackmd.io/_uploads/ByfcuoMj1g.png>

</center>

下載 yahboom imu device 的壓縮檔(python code) : ```ros1_imu_ws.zip``` .

安裝 docker by dockerfile :
::: spoiler 撰寫 dockerfile.

# Use ros1 + Ubuntu 20.04 as the base image from https://github.com/ethz-asl/kalibr/wiki/installation
FROM kalibr

MAINTAINER <your name>
LABEL maintainer="<your email>"

# Avoid interactive prompts during installation
ARG DEBIAN_FRONTEND=noninteractive

# Install development tools and dependencies
RUN apt-get update && apt-get install -y \
    vim \
    git \
    meld \
    unzip \
    python3-pip \
    python3-tk \
    udev \
    lsof \
    ros-$ROS_DISTRO-usb-cam \
    ros-$ROS_DISTRO-imu-tools
RUN pip install pyserial
RUN git config --global diff.external meld 

# [kalibr]
RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
SHELL ["/bin/bash", "-c"]

# [IMU] refer to http://www.yahboom.net/study/IMU
# [IMU] 1-1. unzip ros1_imu_ws.zip [Retronix version] 
COPY ros1_imu_ws.zip /root/
RUN unzip /root/ros1_imu_ws.zip -d /root/
# [IMU] 1-2. Compile feature pack and add into enviroment path
RUN source /opt/ros/noetic/setup.bash && \
  cd /root/ros_imu_ws/ && \
  catkin_make -j && \
  cd src/scripts/ && \
  sudo chmod 777 *.py && \
  echo "source ~/ros_imu_ws/devel/setup.sh" >> ~/.bashrc && \
  source ~/.bashrc
  

# [alan variance] https://github.com/ori-drs/allan_variance_ros
RUN cd /catkin_ws/src && \
	git clone https://github.com/ori-drs/allan_variance_ros.git
RUN cd /catkin_ws/ && \
	source /opt/ros/noetic/setup.bash && catkin build allan_variance_ros && source devel/setup.bash	
	

# Set the default command to run when the container starts
CMD ["bash"]

:::

#### 指令集

- 建置 dokcer 指令 for dockerfile:

    docker build -t kalibr_ros1 .

下指令的資料夾結構 :
<image width=35% src=https://hackmd.io/_uploads/rJ5o0oMjJg.png>

- 啟動 docker 的指令 :

    docker run -it \
      -e DISPLAY=$DISPLAY \
      -e XAUTHORITY=/run/user/1000/gdm/Xauthority \
      -v /run/user/1000/gdm/Xauthority:/run/user/1000/gdm/Xauthority \
      -v /tmp/.X11-unix:/tmp/.X11-unix \
      --device=/dev/video0:/dev/video0 \
      --net=host \
      --privileged \
      kalibr_ros1

- 啟動 yahboom imu 指令集 :

    # 1. 啟動 imu 的環境
    cd ~/WitImu_ws
    colcon build
    source ~/WitImu_ws/install/setup.bash 

    # 2. 啟動 imu device
    roslaunch wit_ros_imu rviz_and_imu.launch
    
    # 3. 播放 imu
    roslaunch wit_ros_imu rviz_and_imu.launch & rostopic echo /wit/imu
    
    # 4. 錄製 imu
    roslaunch wit_ros_imu rviz_and_imu.launch & rosbag record -O imu_data.bag /wit/imu
    
    # 5. 查看 錄製.bag 的簡易資訊
    rosbag info imu_data.bag

ros1 的 imu 通用格式 by ```rostopic echo /wit/imu```
<center>

<image width=75% src=https://hackmd.io/_uploads/S1MUx2Giyx.png>

</center>

### allan_variance_ros tool
1. 錄製靜置的 imu 長達 8hr~24hr
需先確認 imu 採樣頻率!!!

    # 啟動 imu 的環境
    cd ~/WitImu_ws
    colcon build
    source ~/WitImu_ws/install/setup.bash 
    
    # 錄製　imu
    roslaunch wit_ros_imu rviz_and_imu.launch & rosbag record -O imu_data.bag /wit/imu

2. [allan_variance_ros command](https://github.com/ori-drs/allan_variance_ros)

    # 啟動 allan_variance_ros 環境
    cd /catkin_ws
    source devel/setup.bash
        
    rosrun allan_variance_ros cookbag.py --input imu_data.bag --output cooked_rosbag.bag
    rosrun allan_variance_ros allan_variance . imu_config.yaml
    rosrun allan_variance_ros analysis.py --data allan_variance.csv

Note : imu_config.yaml 是自己配置的, 如下所述:
<image width=55% src=https://hackmd.io/_uploads/By2Oz3fj1l.png>
squence_time 可以藉由 ```rosbag info imu_data.bag``` 查看.

3. 輸出結果圖
量測雜訊的標準差為 ```t=1s``` 與 white noise fit line (slope is ```1/2```) 的交點.
Bias雜訊的標準差為 ```t=3s``` 與 random rate fit line (slope is ```-1/2```) 的交點.
- 加速度器
<image width=85% src=https://hackmd.io/_uploads/BkL5b6Giye.png>
- 陀螺儀
<image width=85% src=https://hackmd.io/_uploads/BycsZ6fikg.png>
- 三種雜訊種類
該工具會分析三種雜訊種類於imu上 : 分別是斜率 ```-1/2```的 random walk, 斜率```0```的 bias instability, 斜率 ```1/2```的 rate random walk.
- 量測雜訊就是斜率 ```-1/2```的 random walk, 高頻雜訊, (對應至$\tau = 1$)
- 零偏雜訊就是斜率```1/2```的 rate random walk, 低頻雜訊, (對應至$\tau = 3$)
- 擬合雜訊模型 for kalibr 外參校正使用
allan_variance_ros tool 可輸出 kalibr 所需格式. ```noise_density```就是量測雜訊的標準差.
<image width=65% src=https://hackmd.io/_uploads/SJfRbpzoke.png>
- 需要注意的是 : ==通常會對雜訊標準差乘上10倍==, 再給 slam 使用以及外參標定流程喔 !!!

## Camera-IMU Extrinsic pipline
流程參考 [link](https://www.youtube.com/watch?v=rBT5O5TEOV4)
1. 執行 Docker 並共用資料夾

    FOLDER=/home/andrew/Documents/Code/SLAM/Calibration/data
    sudo docker run -it \
        -e DISPLAY=$DISPLAY \
        -e XAUTHORITY=/run/user/1000/gdm/Xauthority \
        -v /run/user/1000/gdm/Xauthority:/run/user/1000/gdm/Xauthority \
        -v /tmp/.X11-unix:/tmp/.X11-unix \
        -v "$FOLDER:/data" \
        --net=host \
        kalibr_allan

2. (optional) 產生 apriltag 標定板

    # generate target.pdf
    cd kalibr/aslam_offline_calibration/kalibr/python
    python3 kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.02 --tspace 0.3 

3. 拍攝錄製成 bag

    rosbag record -O imu_april.bag /camera/color/image_raw /oak/right/image_rectified /camera/depth/image_raw /imu/data

<center>
<image width=55% src=https://hackmd.io/_uploads/HJr6dz-sll.png>
</center>
4. (optional) 雙目標定

    rosrun kalibr kalibr_calibrate_cameras --bag imu_stereo.bag --topics /camera/color/image_raw /oak/right/image_rectified --models pinhole-radtan pinhole-radtan --target april_6x6.yaml

5. camera + imu 標定

    rosrun kalibr kalibr_calibrate_imu_camera --target april_6x6.yaml --imu imu_bmi270.yaml --imu-models calibrated --cam cam_april-camchain.yaml --bag imu_april.bag

:::spoiler 參數檔
- april_6x6.yaml
<center>
<image width=80% src=https://hackmd.io/_uploads/BJ17AG-iee.png>
</center>
- imu_bmi270.yaml
直接取自 allan_ros_tool, 記得對雜訊乘上倍數. i.g. 白雜訊 * 5, 隨機遊走 * 10
<center>
<image width=50% src=https://hackmd.io/_uploads/rySdpf-jgx.png>
</center>
- (雙目) cam_april-camchain.yaml

        <center>
        <image width=100% src=https://hackmd.io/_uploads/SkLnAG-sxx.png>
        </center>
    - (單目) cam_april-camchain.yaml
        <center>
        <image width=100% src=https://hackmd.io/_uploads/BkbS1QWixl.png>
        </center>
    :::

### 校正工程的細節
- 相機內參標定
    盡量降低曝光時間, 減少 motion blur
- IMU 內參標定
    透過 allan variance ros 校正出來的雜訊變異量, 需要放大. 白雜訊 * 5, 隨機遊走 * 10.
- 聯合標定
    - 設定 sigma 為 1 進行校正
    - 標定手法 : 三軸都要旋轉到, 不能 constant 角速度
- 驗證
    - 相機 
        - projection error 是否在 1 pixel.
        - 投影點是否遵從高斯分布(橢圓形)
            如果不遵從, 代表相機模型可能是錯的. 以下方圖示為例, 遵從高斯分佈.
            <center>
            <image width=55% src=https://hackmd.io/_uploads/rkRDjzWsxe.png>
            </center>
    - IMU
        - 採樣點是否是 unitform
            如果採樣點不規則, 可能是 bandwidth 開不夠大. 以下方圖為例, imu 200 hz 正常運作.
            <center>
            <image width=45% src=https://hackmd.io/_uploads/ByDsizbole.png>
            </center>
        - 確認加速度與陀螺儀是否都在紅框內(3 sigma)
            如果沒有, 代表imu內參沒有求好. 以下方圖示為例, 在紅框內代表正常.
            <center>
            <image width=65% src=https://hackmd.io/_uploads/SJoeoG-jgg.png>
            </center>
    - 檢查變化量是否夠低
        以下方紅框為例, 變化量是低的.
        <center>
        <image width=85% src=https://hackmd.io/_uploads/ryQq9MWsge.png>
        </center>
- IMU 內參的單位
    ![image](https://hackmd.io/_uploads/rypcm7fSxg.png)


### Reference
1. https://blog.csdn.net/kanhao100/article/details/130073156
2. https://tomas789.medium.com/iphone-calibration-camera-imu-and-kalibr-33b8645fb0aa
3. https://hackmd.io/@PFABrcAkRWSshatDzBNr9w/rJvgZ91rE
4. https://blog.csdn.net/qq_41694024/article/details/130598248