安卓手机采集数据（cam+imu）在vins-mono下测试_imu 手机_白白白白白kkk

大大的周 02-07 8042

上一个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的联合标定官方介绍