【VINS-MONO测试】安卓手机采集mono+imu数据

上一个vins-mono环境配置测试完成后，初步建立好实验环境，接下来开始进行数据采集（cam+imu）、打包、标定、运行。

记录一下后续要更新的内容吧：
ros打包、另外两种标定方式、手机在线测试、另一种app的测试情况、IMU数据的调参

更新一下：时间是23-05-11，之前有些理解不到位的地方做了修改，多余的步骤删减。

一、采集数据

cam+imu传感器组合可以看作视觉惯导slam的一种组合方法，基于安卓平台可以在人人都有的安卓手机上进行实验。
本次实验采用的app：说明

之前用的是第一个版本，部分机型确实会出现闪退的情况，作者也开源了2.0版本，主要更新了对焦后一直跳动的bug，有需要的可以去下载安装，两个app不能同时存在于你的手机上。
https://github.com/OSUPCVLab/mobile-ar-sensor-logger/releases

对于版本这个问题，因为某些机型确实会闪退，还是根据需要来选择版本吧！

该App能调用摄像头的同时调用手机中的IMU，离线采集，方便导入，适配华为手机效果较好。

我自己测试了很多手机，从五六年前发布的华为nova3（Kirin 970）、到18年的华为旗舰Mate20（Kirin 980）、nova6（Kirin 990）最后还试了遥遥领先的Mate40 Pro（kirin 9000）和发热严重（doge）…的Mate50Pro，可以说从低端到入门，再到高端旗舰都试了一遍，效果可以说非常好了。（我甚至觉得nova效果比旗舰都好hh，异常的稳定）。

其他品牌例如安卓之光小米11Ultra、手感极好的小米12S、红米K30S Ultra、红米K50 Ultra、vivo X80（发哥9000）、oppo find x3、荣耀V8（还有一些不记得了），只能说差点意思。当然，这个环节影响最大的是IMU标定那一步，当时我在最后说直接利用公制参数来联合标定其实也是不准确的（如果你的手机标定实在太差，记得有一次某个品牌标定十次，十次都天差地别…，也是可以直接上公制参数的）。

哦对了，之前说到了手机越贵越新效果也就越好其实也是有点问题的，手机并不是越新越好，太高的版本系统不兼容老App的问题也存在。

安装完成后，开始记录record，结束stop。分辨率建议640×480。
该数据记录保存在本地的Android文件夹下，类似小米华为系列的手机需要打开开发者模式并进入USB调试才可以找到。为了方便，我们将文件直接将文件发送到电脑也好，找到一个不需要权限的文件夹放入也好，反正怎么简单怎么来~

发现本地文件：

是以一个采集时间来命名的文件，进入发现里面有许多数据，其中movie.mp4是视频流信息，gyro_accel.csv是imu的含有时间戳、加速度计和陀螺仪的数据，frame_timestamp.mp4是视频帧时间戳数据。其它目前用不到，涉及地磁、GPS等信息，也说明该app可以多数据源融合

新更新的2.0版本砍掉了不需要的地磁和GNSS数据，整体非常精简！（作者也很热心，有问题基本都是很快回复的~~谢谢大佬）

二、打包

打包目前有两种方法，一种是直接在ROS下打包，另一种是通过kalibr_bagcreater.py脚本来进行打包。
我们采用kalibr提供的脚本打包方式，打包脚本程序kalibr_bagcreater
拷贝到Ubuntu下，试运行发现缺少一个叫utility_functions.py的第三方库，添加以后发现可以运行了。
库的地址： utility_functions.py
本地创建一个文件，起名dataset2，放入视频和imu的时间戳以及MP4

编译后会生成一个pyc文件，是生成的中间文件，不需要在意。
记录一下输入参数的过程：
通过阅读kalibr_bagcreater.py代码，输入-h或–help后会显示帮助文档，按照提示输入参数。

按照这样填入参数，运行！当然你也可以指定包的名字。

出现打包的画面，说明正在录制。

如果不填写保存位置的参数，得到的.bag文件也是自动保存到这个文件夹下，我们发现出现了一个output.bag的文件。

这样就打包完成了，接下来任务就是如何配置launch文件，设置yaml参数，让bag包在vins-mono下运行。

三、简单测试

第一次打包完成后，可以简单的在vins下跑一跑，看看有没有问题。在运行之前，要修改launch启动文件和yaml配置文件。
简单起见，就在本地文件夹vins-mono下添加修改即可。
1.修改yaml文件。
进入catkin_ws工作空间，找到存放源代码的文件夹src，进入vins-mono，点击config。在config下新建一个名为android的文件夹，其中新建文件：

将隔壁的euroc下的euroc_config.yaml中的内容复制过来

因为还不知道相机和imu的内参和畸变参数以及cam to imu的矩阵，所以暂且只修改相机和imu的topics以及相机的分辨率信息。即

imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
************
#  根据你当时录制的规格来写，建议640×480
image_width: 1280
image_height: 720

注意：该文件就是最后的android参数配置文件，下面的各种参数是需要填写的，待到后面将cam和imu联合标定以后，会得到相关的参数！

！！！
这里其实是有点小问题的，当时没注意分辨率这个东西，建议录制的时候全程采用640*480的规格，这个格式比较常用，而且长时间录制也不会造成所占空间过大的问题，如果特别大的话，1280×720，外加一个30帧的采集频率，打包都得好长时间了，部分手机还可能出现运行过程中闪退的问题，点名Mate50。

2.修改launch文件
launch文件是一个启动文件，roslaunch以后，启动了android配置文件yaml。
增加launch文件，直接将/vins_mono/vins_estimator/launch文件夹下的euroc.launch文件，拷贝一份，重命名为android.launch，修改config_path变量（启动后直接去找android_config.yaml）：
这里有一个玄学问题，待到后面roslaunch以后，经常会出现杀死进程的提示，不知道为啥，或许是虚拟机的问题。

可能的情况有两种：
1.确实是你对应错了，仔细检查路径，字母的拼写。
2.重启一下你的虚拟机，独立系统我还没遇见过。

以上两处配置好以后，可以保证在vins-mono下运行了

roscore
roslaunch vins_estimator android.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play ~/catkin_ws/Dates/

因为没有联合标定，各种参数也没写，所以直接飘了。（哎，标定好了也经常飘…我的问题我的问题）

四、标定

标定部分包括相机的标定和imu的标定，以及组合slam特有的联合标定。

1. 相机标定：

相机的标定用的是kalibr工具包。此外也有OpenCV标定（c++）、MATLAB标定工具箱。后续我也会尝试一下，包括上面的ros打包吧。听说MATLAB用棋盘格效果也很好，后续也可能用双目相机试试，再开一篇新的文章。
补充一下：这位大哥写的也很好，也可以参考，他这个是ubuntu1804的：kalibr工具箱

安装工具箱所需要的依赖库：
这些是通用依赖：

sudo apt-get install -y \
    git wget autoconf automake nano \
    libeigen3-dev libboost-all-dev libsuitesparse-dev \
    doxygen libopencv-dev \
    libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev

这些根据自己的版本来安装

# Ubuntu 16.04
sudo apt-get install -y python2.7-dev python-pip python-scipy \
    python-matplotlib ipython python-wxgtk3.0 python-tk python-igraph
# Ubuntu 18.04
sudo apt-get install -y python3-dev python-pip python-scipy \
    python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph
# Ubuntu 20.04
sudo apt-get install -y python3-dev python3-pip python3-scipy \
    python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph

建立工作空间：

mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

下载kalibr源码并编译：
这里可以gitclone也可以直接download zip。我这里是download了。下载后将.zip解压到 ~/kalibr_workspace/src文件夹下：

cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j12   # 很大，建议尽可能多给资源

安装完成！最新的包是36个，如果是显示36个successful，那就通过了。

接下来是对相机进行标定。准备一个打包好的包含标定板图像的output.bag以及标定板的配置文件.yaml，将其放到创建好的kalibr_workspace中，命名为test1，进入文件夹：

source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_cameras --bag output.bag --topics  /cam0/image_raw --models pinhole-radtan --target aprilgrid.yaml

等待标定～

这样就是标定成功了，同时你的文件夹下会出现相机的标定参数和一个标定精度的pdf：
此处表示的是标定情况，越聚集效果越好，最好不超过1.0这个范围。

2.IMU标定:

将手机静止1-2h，手机越新、越贵，标定的效果越好 。时间越久越好，oled屏幕注意烧屏（我的安卓之光就烧了）

如果真的想体验一下手机IMU标定的，可以这样（xiaomi为例）：
1.下载一个MIUI+在你的电脑上（反正就是找一个互联投屏软件，你要是LCD屏幕当我没说）
2.链接好，连同一个wifi，蓝牙也打开，靠的近一点
3.用电脑来操作你的手机屏幕，这样你的手机屏幕是黑的，但是硬件是工作的！
4.结束的时候，直接在电脑端操控就可以了
tips: 华为手机的话，我平板也是华为的，用平板来控制，一个道理，保护好自己的手机，烧屏的钱都能买个二手的nova了…

为了简单方便，我们用imu_utils来标定imu，在安装编译之前，首先需要明确的是，标定得到的参数是程序给你算好的，程序运行就要依赖一些库文件，code_imu依赖全局ceres库，其次是不能对code_imu 和imu_utils同时编译，imu_utils又依赖于code_imu。
所以，我们的顺序是先安装ceres-solver，再安装code_imu，最后安装imu_utils。

1.安装依赖项

sudo apt-get install libdw-dev

2.编译

imu_utils下载地址为：https://github.com/gaowenliang/imu_utils
code_utils下载地址为：https://github.com/gaowenliang/code_utils
下载后解压，此时注意先后顺序！

先把code_utils放在工作空间catkin_ws的src下面，然后编译。

cd catkin_ws
catkin_make   # 哦，这里catkin_make是ros空间下的make，本质也是编译

此时出现报错

/home/hao/catkin_ws/src/code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory



头文件是有的，但是没读到，说明包含的路径层数有问题：
在code_utils下面的src中找到sumpixel_test.cpp，修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译。注意英文标点符号。

编译成功以后，再把imu_utils放到工作空间catkin_ws的src下面，进行编译。

cd catkin_ws
catkin_make

编译成功！下面是测试对imu的标定情况。

3.打包imu.bag

将手机静置至少2h（>120分钟） 。同上文第二步一样，将采集的数据保存到电脑中，进行打包。kalibr制包器也是可以将单一的imu数据打包成rosbag的，不过需要对代码进修小的改动。

这里说明一下吧，我自己亲测的话，时间确实是越久越好，大概3~4个小时就可以了，没必要时间特别长也没有硬性要求，一来打包过程比较久，二来万一闪退了啥的，还傻乎乎的以为在标定呢…

在kalibr_bagcreater.py中，第569行的代码：

 else:
        raise Exception('Invalid/Empty video file and image folder')

换成pass，跳过这一提示没有image的else，这样就不会提示，正常打包。：

 else:
        pass

这一步修改的文件建议直接保存好，以后实验还会经常用

4.配置launch文件

在imu_utils/launch文件夹下，创建一个新的android.launch文件。

需要说明的是，max_time_min这一行，最后的数值表示的是记录最长时间，若是采集的数据时长超过这一时间，则会出现错误，需要注意！保存路径可以自己定，在第二行那个位置。

5.标定imu

没source的source一下

roslaunch imu_utils android.launch

可以看出系统正在等待imu的数据，接下来是对imu.bag进行play bag。
找到包的位置或者写明包的位置，-r 200的意思是200倍的速度进行播放，否则数据太多，要进行很长时间。-s 10的意思是从第十秒开始play，这样可以避免一开始手机的不平稳引起的误差。

rosbag play -r 200 output.bag 
rosbag play -r 200 output.bag -s 10

记录结束以后（上图标定窗口会有变化），ctrl+c进行中断，可以在本地的imu-utils/data文件中找到HUAWEI_imu_param.yaml。直接搜索查找吧，不少东西里面。这其中的数据 是作为以后cam+imu联合标定时的重要参数。

3mono+IMU联合标定：

联合标定也是在kalibr工具箱中的功能，相关指令：

source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_imu_camera --target aprilgrid.yaml --cam camchain.yaml --imu imu.yaml --bag cam_imu.bag 

根据参数来准备需要的配置文件：
aprilgrid.yaml文件是标定板的相关配置文件。其中：

tagsize：是大正方形的边长。下面是官方样图：

camchain.yaml文件是对cam单独标定所得到的。其中的相机模型、畸变参数、畸变模型、相机内参和分辨率如图：

imu.yaml文件是imu的配置文件。由之前标定的imu数据得到：

我这里只是一个参考，这一步是最麻烦的，也是最棘手的，你要根据你标定好的参数来进行放缩，虽然不能数值一样，但是起码要做到数量级差不多，不太好总结，后面补充一个demo吧。

cam_imu.bag是包含了各种不同位姿的image和不同方向角度与速度，全方位激活了加速度计、陀螺仪的信息源的rosbag格式。
其实正确的姿势应该是标定相机时保证手机不动，去移动标定板；联合标定的时候，是标定板不动，按照视频去移动手机。

运行上述指令。

结束后会生成很多文件。kalibr很贴心的生成一个pdf，可以形象的查看标定的效果。

至此，标定部分完成！

五、配置启动vins的文件

将上述所有cam和imu相关内参和外参，写入vins-mono的config文件中，进行测试。

%YAML:1.0

#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
output_path: "/home/zzh/output/"

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0.03950348224657654
   k2: -0.06749775883363018
   p1: -0.00592411550206494
   p2: -0.0010599949132722051
projection_parameters:
   fx: 425.14538096051245
   fy: 425.2848488603618
   cx: 321.238440081968
   cy: 221.10305101769163

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [-0.00059191, -0.99966752, 0.02577778,
         -0.99999327, 0.00068502, 0.00360354, 
         -0.00362, -0.02577547, -0.9996612]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [-0.00003843, -0.00006284, 0.00002028]

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 25            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 16.0e-2          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 24.0e-3        # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 5.5e-4         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-4         # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80521281     # gravity magnitude


#loop closure parameters
loop_closure: 1                    # start loop closure
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0             # useful in real-time and large project
pose_graph_save_path: "/home/zzh/output/pose_graph/" # save and load path

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: -0.0199                            # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#rolling shutter parameters
rolling_shutter: 0                  # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0.033               # unit: s. rolling shutter read out time per frame (from data sheet). 

#visualization parameters
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4      # size of camera marker in RVIZ

注意：

1.在配置文件的时候，标定cam时采集的视频分辨率和后面跑自采数据的分辨率要相同。否则会发生错误。
2.在配置imu 参数这一部分，直接采用官方给到的公制参数，效果要远远好于自己标定的部分。 可以根据自己的经验进行微调。

3. 录制视频的方法：相机标定的正确姿势应该是手机固定、移动标定板；联合标定录制cam和IMU的时候是全方位移动手机，固定你的标定板。但是你甚至直接按照联合标定的方法来录制都没问题的，主要是我们的标定板是一张纸，拿来到处平移确实不方便。我的标定板是用A3打印的，建议彩色打印，对的，就是彩色打印黑白的标定板，因为特别大块的黑色容易打印不清楚，贴墙上的时候尽可能平。光线充足一点，但是也别反光太严重。
4. IMU参数调整：
5. 采集数据的时候，即使你标定的很好，转弯过程也要尽可能慢、尽可能平稳，手机里的惯导没那么精确，漂移是很正常的。
6. 在一些室内场景下，楼梯、地下车库等光线不充足的地方或者面对一大片白墙特征点较少的地方，会发现轨迹有漂移的现象，可以在墙壁上贴一些棋盘格等标志物，让相机尽可能抓到特征点。
7. 启动的时候，原地平稳的转几圈，激活你手机里的IMU，直接行进会导致一开始的一段没有轨迹。

学习过程中的参考链接：
手机上的SLAM（5）：rosbag打包image+imu
手机上的SLAM（6）：ubuntu+ros自采数据开源方案适配(vins-mono)
手机上的SLAM（7）：Kalibr相机+IMU离线标定
用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定
官方介绍