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

一、采集数据

cam+imu传感器组合可以看作视觉惯导slam的一种组合方法，基于安卓平台大大简化用户的设备数量，在人人都有的安卓手机上也能进行实验。
本次实验采用的app：说明
该app集cam（视频流）+imu采集于一体，，离线采集，方便导入，适配华为手机效果较好。

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

发现本地文件：

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

二、打包

打包目前有两种方法，一种是直接在ROS下打包，另一种是通过kalibr_bagcreater.py脚本来进行打包
本次采用Python脚本打包的方式，打包脚本程序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下运行。

补充：
1.launch文件的作用是 启动多个节点，因为在ROS下许多命令都是一个个节点（node）组成的，这样可以简化流程启动。
2.yaml文件里包含了一些适应当前数据的参数配置。

三、简单测试

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

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

因为还不知道相机和imu的内外参和imu的噪声，所以暂且只修改相机和imu的topics以及相机的分辨率信息。即

imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
************
image_width: 1280
image_height: 720

注意：该文件就是最后的android参数配置文件，下面的各种参数是需要填写的，待到后面将cam和imu联合标定以后，会得到相关的参数！
2.修改launch文件
launch文件相当于一个启动器，roslaunch以后，启动了android配置文件yaml。
增加launch文件，直接将/vins_mono/vins_estimator/launch文件夹下的euroc.launch文件，拷贝一份，重命名为android.launch，修改config_path变量（启动后直接去找android_config.yaml）：
这里有一个玄学问题，待到后面roslaunch以后，经常会出现杀死进程的提示，不知道为啥，或许是虚拟机的问题。

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

roscore

roslaunch vins_estimator android.launch 

roslaunch vins_estimator vins_rviz.launch

rosbag play ~/catkin_ws/Dates/

因为没有配置手机的cam+imu的内外参，所以直接飘了。

说明还需要后续的imu和cam的内外参配置。

四、标定

标定部分包括相机的标定和imu的标定。

相机标定：

相机的标定用的是kalibr工具包。
源码安装工具箱：

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
sudo apt-get update
sudo apt-get install ros-kinetic-desktop python-rosinstall python-rosdep -y
rosdep init
rosdep update

其中的rosdep init 和rosdep update 出现问题，参考之前配置vins-mono时的解决方法即可，是可以解决的。
安装工具箱所需要的依赖库：

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

sudo apt-get update
sudo apt-get install python-igraph

sudo apt install python-pip (python2.7)
sudo pip install python-igraph

两种igraph的安装方式，推荐apt-get安装。pip因为版本问题太过于复杂。
igraph的两种安装方式
创立工作空间：

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

其中第二步创建好工作空间以后，source以后的catkin init可能出现catkin未找到命令这一错误提示，我们需要安装catkin tools工具

sudo apt-get update
sudo apt-get install python-catkin-tools

catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release后显示：

下载kalibr源码并编译：

cd ~/kalibr_workspace/src
git clone https://gitclone.com/github.com/ethz-asl/Kalibr.git
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4

安装完成！
接下来是对相机进行标定。准备一个打包好的包含标定板图像的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这个范围。

imu标定:

将手机静止1-2h，手机越新、越贵，标定的效果越好。时间越久越好，oled屏幕注意烧屏。
为了简单方便，我们用imu_utils来标定imu，在安装编译之前，首先需要明确的是，code_imu依赖全局ceres库，其次是不能对code_imu 和imu_utils同时编译，imu_utils依赖code_imu。
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

此时出现报错

/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的标定情况。
1.打包imu.bag:
将手机静置至少2h（>120分钟）。同上文第二步一样，将采集的数据保存到电脑中，进行打包。kalibr制包器也是可以将单一的imu数据打包成rosbag的，不过需要对代码进修小的改动。
在kalibr_bagcreater.py中，第569行的代码：

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

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

 else:
        pass

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

需要说明的是，max_time_min这一行，最后的数值表示的是记录最长时间，若是采集的数据时长超过这一时间，则会出现错误，需要注意！
3.标定imu：

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联合标定时的重要参数。

cam+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数据得到：

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 参数这一部分，直接采用官方给到的公制参数，效果要远远好于自己标定的部分。可以根据自己的经验进行微调。
学习过程中的参考链接：
手机上的SLAM（5）：rosbag打包image+imu
手机上的SLAM（6）：ubuntu+ros自采数据开源方案适配(vins-mono)
手机上的SLAM（7）：Kalibr相机+IMU离线标定
用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定
官方介绍