【室内导航通过视觉惯性数据融合】将用户携带的智能手机收集的惯性数据与手机相机获取的视觉信息进行融合研究（Matlab代码实现）

 💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

 ⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

 ⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

     或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

摘要：
本文旨在通过将用户携带的智能手机收集的惯性数据与手机相机获取的视觉信息进行融合，提升第一人称室内导航和场景理解体验。我们在一个期望最大化框架中采用消失方向概念以及人工环境的正交约束，以估计人在视频帧中相对于已知室内坐标的方向。该框架允许包含有关相机旋转轴的先验信息，以提高估计精度，并从单眼视频帧中选择候选边线，估计走廊的深度和宽度，并进行场景的三维建模。我们提出的算法同时使用Kalman滤波器将基于视觉的估计方向与惯性数据结合，以改进估计并消除惯性传感器产生的显著测量漂移。我们在一个由IMU增强视频上评估了我们的视觉惯性数据融合方法的性能，视频记录了参与者完成一整圈旋转走廊的过程。我们证明了这种融合提供了几乎无漂移的即时关于人的相对方向的信息。我们能够估计走廊的深度和宽度，并从约60米一圈的旋转走廊生成一个闭环路径地图。

人类自然地利用视觉感知和导航周围世界，视觉作为主要输入方式与自我本体感觉反馈相结合。然后，他们的大脑重建被可视化场景的三维模型，使他们能够在环境中导航或探索新位置。然而，要从A点到B点旅行，必须通过地图（物理或数字）以及定位系统或用户对位置/路径的先前知识来向用户提供路径信息。随时间收集的全球导航卫星系统（GNSS）数据提供了定位和路径信息。然而，在室内场所，如建筑物内部或隧道等地方，通常无法获得GNSS信号，或者信号非常微弱。为了在GNSS信号不可用的区域估计准确的定位，已经使用惯性测量单元（IMUs）的数据进行相对航向和方向检测。

然而，IMU的数据容易产生大量漂移和失真，通常是由于累积误差引起的。由于与时间积分，加速度计数据中的恒定误差（无论多么小）导致速度中的线性误差和位置估计中的二次误差增长。此外，由于加速度计数据是以本地设备坐标给出的，因此需要除去其中的重力分量以计算位移，而没有准确的姿态（角度）估计是无法实现的。陀螺仪测量可以被积分以产生短时间内可靠的角度估计，但对长时间会产生漂移。姿态速率中的恒定误差导致方位的线性误差，速度中的二次误差和位置中的三次误差增长。为了校正陀螺仪测量，可以利用从加速度计估计的重力分量计算的偏航和俯仰角度。然而，加速度计对短时间内的振动和其他非重力加速度敏感。

📚2 运行结果

部分代码：

%% Read Entire Video %%%%
mov = VideoReader('sample_video\aclab_video_TF.mp4');  %WIN_20170508_13_45_27_Pro
i=0;
while hasFrame(mov)
i=i+1;
vid = rgb2gray(readFrame(mov));
video(:,:,i)=vid;
end

%% Synchonize IMU and Video
video=video(:,:,1:size(video,3)/339:end); %obtain this number from csv file

%% Read IMU data %%%
showfig = 0;
l_start = 1;
l_end = 339; %size(video,3); obtain this number from csv file
frq=30;
filename='sample_video\aclab_data_TF.csv';
[data_IMU,data_lables,t] = iPhone_IMU_reading(filename,frq,l_start,l_end,showfig);
%%%Acceleration & Gravity data
x_g = data_IMU(:,5)*9.81; % X gravity * g
y_g = data_IMU(:,6)*9.81; % Y gravity * g
z_g = data_IMU(:,7)*9.81; % Z gravity * g
acc_x=data_IMU(:,2)*9.81; % X acceleration * g
acc_y=data_IMU(:,3)*9.81; % Y acceleration * g
acc_z=data_IMU(:,4)*9.81; % Z acceleration * g

%%%The 3 attitudes reported by iPhone
AttPitch = data_IMU(:,12);   %rotation over x axis
AttYaw = data_IMU(:,13);     %rotation over z axis
AttYaw = ThetaCorrect(AttYaw);
AttRoll = data_IMU(:,14);    %rotation over y axis

%%%Angle computed by integration of gyro measurement
x_gyro = data_IMU(:,15);
y_gyro = data_IMU(:,16);
z_gyro = data_IMU(:,17);
delta_t = [t(1) ; t(2:end) - t(1:end-1)];
pitch_gyro = cumsum(x_gyro.*delta_t,1); 
roll_gyro = cumsum(y_gyro.*delta_t,1); 
yaw_gyro = cumsum(z_gyro.*delta_t,1); 

%%%Angle computed by geometry of gravity
pitch_g = atan2(y_g , z_g) + pi;

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

A. Farnoosh, M. Nabian, P. Closas and S. Ostadabbas, "First-person indoor navigation via vision-inertial data fusion," 2018 IEEE/ION Position, Location and Navigation Symposium (PLANS), Monterey, CA, USA, 2018, pp. 1213-1222, doi: 10.1109/PLANS.2018.8373507. keywords: {Estimation;Cameras;Three-dimensional displays;Accelerometers;Indoor navigation;Simultaneous localization and mapping;Computer vision;Data fusion;Expectation maximization algorithm;In-door navigation;Simultaneous localization and mapping (SLAM)},

🌈4 Matlab代码实现