视觉和imu(惯性传感器)( 一)
转:https://blog.csdn.net/qq_18661939/article/details/53574981
目前单目slam存在初始化的尺度问题和追踪的尺度漂移问题,而双目也存在精度不高和鲁棒性不好的问题。针对这些问题,提出了融合imu的想法。
那么imu的作用是什么呢?
单目
(1)解决初始化尺度问题
(2)追踪中提供较好的初始位姿。
(3)提供重力方向
(4)提供一个时间误差项以供优化
双目
(1)追踪中提供较好的初始位姿。
(2)提供重力方向
(3)提供一个时间误差项以供优化
目前做这方面融合论文很多,但开源的比较少,这里给出几个比较好的开源code和论文
开源code:
(1)imu和单目的数据融合开源代码(EKF)
https://github.com/ethz-asl/rovio
(2)imu和单目的数据融合开源代码
https://github.com/ethz-asl/okvis_ros(非线性优化)
(3)orbslam+imu(立体相机)
https://github.com/JzHuai0108/ORB_SLAM
论文:
(1)Keyframe-based visual–inertial odometry(okvis的论文)
(2) IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation(预积分)
(3)Visual-Inertial Monocular SLAM with Map Reuse (orb+imu)
(4)Robust Visual Inertial Odometry Using a Direct EKF-Based Approach(eth的rovio)
(5)On-Manifold Preintegration for Real-Time Visual-Inertial Odometry(gtsam)
由于是初学比较详细看得就是以上5篇,而且自认为还不错的论文。
本人研究的是基于非线性优化的视觉和imu融合的算法研究,那么这里先引出融合的方式:
滤波方法:
(1)紧耦合
(2)松耦合
非线性优化:
(1)紧耦合(本人研究方向)
(2)松耦合
imu'和视觉是怎样融合的呢?
仅仅视觉的时候我们优化的只是重投影误差项:
以上的公式我就不解释了。
而imu+视觉优化的是重投影误差项+imu的时间误差项:
其中imu时间误差项:
其中为:
这里:imu时间误差项要求的主要有5个变量:eR,ev,ep,eb,W。即求(R ,v,p,b,W)
这里先给出一张非线性优化视觉+imu融合的图:
下面我们就开始用与积分的方式求以上的6个变量,下面给出预积分的code
Eigen::Matrix4d Tracking::propagate(const double time_frame)
{bool is_meas_good=getObservation(time_frame);assert(is_meas_good);time_pair[0]=time_pair[1];time_pair[1]=time_frame;Eigen::Vector3d tempVs0inw;Eigen::Matrix<double, 15,15>* holder=NULL;if(bPredictCov)holder= &P_;predictStates(T_s1_to_w, speed_bias_1, time_pair,measurement, imu_.gwomegaw, imu_.q_n_aw_babw,&pred_T_s2_to_w, &tempVs0inw);pred_speed_bias_2.head<3>()=tempVs0inw;//速度偏差pred_speed_bias_2.tail<6>()=speed_bias_1.tail<6>(); //biases do not change in propagationEigen::Matrix4d pred_Tr_delta=pred_T_s2_to_w*imu_.T_imu_from_cam;//camera-imu-world(矩阵的乘法从左开始)cam_to_w=pred_Tr_delta;pred_Tr_delta=pred_Tr_delta.inverse()*(T_s1_to_w*imu_.T_imu_from_cam);//由imu计算(预测)上一帧-》当前帧的变换关// T_s1_to_w=pred_T_s2_to_w;speed_bias_1=pred_speed_bias_2;return pred_Tr_delta;
}
void Tracking::predictStates(const Eigen::Matrix4d &T_sk_to_w, const Eigen::Matrix<double, 9,1>& speed_bias_k,const double * time_pair,std::vector<Eigen::Matrix<double, 7,1> >& measurements, const Eigen::Matrix<double, 6,1> & gwomegaw,const Eigen::Matrix<double, 12, 1>& q_n_aw_babw,Eigen::Matrix4d * pred_T_skp1_to_w, Eigen::Matrix<double, 3,1>* pred_speed_kp1,Eigen::Matrix<double, 15,15> *covariance,Eigen::Matrix<double, 15,15> *jacobian)
{double time=time_pair[0],end = time_pair[1];// the eventual covariance has little to do with this param as long as it remains smallEigen::Matrix<double, 3,1> r_0(T_sk_to_w.topRightCorner<3, 1>());Eigen::Matrix<double,3,3> C_WS_0(T_sk_to_w.topLeftCorner<3, 3>());Eigen::Quaternion<double> q_WS_0(C_WS_0);Eigen::Quaterniond Delta_q(1,0,0,0);Eigen::Matrix3d C_integral = Eigen::Matrix3d::Zero();Eigen::Matrix3d C_doubleintegral = Eigen::Matrix3d::Zero();Eigen::Vector3d acc_integral = Eigen::Vector3d::Zero();Eigen::Vector3d acc_doubleintegral = Eigen::Vector3d::Zero();Eigen::Matrix3d cross = Eigen::Matrix3d::Zero();// sub-JacobiansEigen::Matrix3d dalpha_db_g = Eigen::Matrix3d::Zero();Eigen::Matrix3d dv_db_g = Eigen::Matrix3d::Zero();Eigen::Matrix3d dp_db_g = Eigen::Matrix3d::Zero();// the Jacobian of the increment (w/o biases)Eigen::Matrix<double,15,15> P_delta = Eigen::Matrix<double,15,15>::Zero();double Delta_t = 0;bool hasStarted = false;int i = 0;for (int it=0;it<measurements.size();it++){Eigen::Vector3d omega_S_0 =measurements[it].block<3,1>(4,0);//角速度Eigen::Vector3d acc_S_0 = measurements[it].block<3,1>(1,0);//线加速度Eigen::Vector3d omega_S_1 = measurements[it+1].block<3,1>(4,0);Eigen::Vector3d acc_S_1 = measurements[it+1].block<3,1>(1,0);ave_omega_S=ave_omega_S+omega_S_0;ave_omega_S=ave_omega_S/(it+1);// time deltadouble nexttime;if ((it + 1) == (measurements.size()-1)) {nexttime = end;}elsenexttime =measurements [it + 1][0];double dt = (nexttime - time);if ( end < nexttime) {double interval = (nexttime - measurements[it][0]);nexttime = end;dt = (nexttime - time);const double r = dt / interval;omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval();acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval();}/* if ( it+1==measurements.size()) {double interval = last_dt;nexttime = end;double dt = (nexttime - time);const double r = dt / interval;omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval();acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval();}elsenexttime =measurements [it + 1][0];double dt = (nexttime - time);*/if (dt <= 0.0) {continue;}Delta_t += dt;if (!hasStarted) {hasStarted = true;const double r = dt / (nexttime -measurements[it][0]);omega_S_0 = (r * omega_S_0 + (1.0 - r) * omega_S_1).eval();//求开始是加权的角速度和线加速度acc_S_0 = (r * acc_S_0 + (1.0 - r) * acc_S_1).eval();}// ensure integritydouble sigma_g_c = q_n_aw_babw(3);//Gyroscope noise density.double sigma_a_c = q_n_aw_babw(1);// actual propagation// orientation:Eigen::Quaterniond dq;const Eigen::Vector3d omega_S_true = (0.5*(omega_S_0+omega_S_1) - speed_bias_k.segment<3>(3));//wconst double theta_half = omega_S_true.norm() * 0.5 * dt;const double sinc_theta_half = ode(theta_half);const double cos_theta_half = cos(theta_half);dq.vec() = sinc_theta_half * omega_S_true * 0.5 * dt;dq.w() = cos_theta_half;Eigen::Quaterniond Delta_q_1 = Delta_q * dq;// rotation matrix integral:const Eigen::Matrix3d C = Delta_q.toRotationMatrix();const Eigen::Matrix3d C_1 = Delta_q_1.toRotationMatrix();const Eigen::Vector3d acc_S_true = (0.5*(acc_S_0+acc_S_1) - speed_bias_k.segment<3>(6));const Eigen::Matrix3d C_integral_1 = C_integral + 0.5*(C + C_1)*dt;const Eigen::Vector3d acc_integral_1 = acc_integral + 0.5*(C + C_1)*acc_S_true*dt;// rotation matrix double integral:C_doubleintegral += C_integral*dt + 0.25*(C + C_1)*dt*dt;acc_doubleintegral += acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt;// Jacobian partsdalpha_db_g += dt*C_1;const Eigen::Matrix3d cross_1 = dq.inverse().toRotationMatrix()*cross +okvis::kinematics::rightJacobian(omega_S_true*dt)*dt;const Eigen::Matrix3d acc_S_x = crossMx(acc_S_true);Eigen::Matrix3d dv_db_g_1 = dv_db_g + 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);dp_db_g += dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);// covariance propagationif (covariance) {Eigen::Matrix<double,15,15> F_delta = Eigen::Matrix<double,15,15>::Identity();// transformF_delta.block<3,3>(0,3) = -crossMx(acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt);F_delta.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt;F_delta.block<3,3>(0,9) = dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);F_delta.block<3,3>(0,12) = -C_integral*dt + 0.25*(C + C_1)*dt*dt;F_delta.block<3,3>(3,9) = -dt*C_1;F_delta.block<3,3>(6,3) = -crossMx(0.5*(C + C_1)*acc_S_true*dt);F_delta.block<3,3>(6,9) = 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);F_delta.block<3,3>(6,12) = -0.5*(C + C_1)*dt;P_delta = F_delta*P_delta*F_delta.transpose();// add noise. Note that transformations with rotation matrices can be ignored, since the noise is isotropic.//F_tot = F_delta*F_tot;const double sigma2_dalpha = dt * sigma_g_c * sigma_g_c;P_delta(3,3) += sigma2_dalpha;P_delta(4,4) += sigma2_dalpha;P_delta(5,5) += sigma2_dalpha;const double sigma2_v = dt * sigma_a_c * q_n_aw_babw(1);P_delta(6,6) += sigma2_v;P_delta(7,7) += sigma2_v;P_delta(8,8) += sigma2_v;const double sigma2_p = 0.5*dt*dt*sigma2_v;P_delta(0,0) += sigma2_p;P_delta(1,1) += sigma2_p;P_delta(2,2) += sigma2_p;const double sigma2_b_g = dt * q_n_aw_babw(9) * q_n_aw_babw(9);P_delta(9,9) += sigma2_b_g;P_delta(10,10) += sigma2_b_g;P_delta(11,11) += sigma2_b_g;const double sigma2_b_a = dt * q_n_aw_babw(6) * q_n_aw_babw(6);P_delta(12,12) += sigma2_b_a;P_delta(13,13) += sigma2_b_a;P_delta(14,14) += sigma2_b_a;}// memory shiftDelta_q = Delta_q_1;C_integral = C_integral_1;acc_integral = acc_integral_1;cross = cross_1;dv_db_g = dv_db_g_1;time = nexttime;interval_time=Delta_t;last_dt=dt;++i;if (nexttime == end)break;}
// actual propagation output:
const Eigen::Vector3d g_W = gwomegaw.head<3>();
const Eigen::Vector3d t=r_0+speed_bias_k.head<3>()*Delta_t+ C_WS_0*(acc_doubleintegral)+0.5*g_W*Delta_t*Delta_t;
const Eigen::Quaterniond q=q_WS_0*Delta_q;
(*pred_T_skp1_to_w)=rt_to_T(t,q.toRotationMatrix());(*pred_speed_kp1)=speed_bias_k.head<3>() + C_WS_0*(acc_integral)+g_W*Delta_t;//???语法曾有错误
if (jacobian) {Eigen::Matrix<double,15,15> & F = *jacobian;F.setIdentity(); // holds for all states, including d/dalpha, d/db_g, d/db_aF.block<3,3>(0,3) = -okvis::kinematics::crossMx(C_WS_0*acc_doubleintegral);F.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*Delta_t;F.block<3,3>(0,9) = C_WS_0*dp_db_g;F.block<3,3>(0,12) = -C_WS_0*C_doubleintegral;F.block<3,3>(3,9) = -C_WS_0*dalpha_db_g;F.block<3,3>(6,3) = -okvis::kinematics::crossMx(C_WS_0*acc_integral);F.block<3,3>(6,9) = C_WS_0*dv_db_g;F.block<3,3>(6,12) = -C_WS_0*C_integral;
}// overall covariance, if requested
if (covariance) {Eigen::Matrix<double,15,15> & P = *covariance;// transform from local increments to actual statesEigen::Matrix<double,15,15> T = Eigen::Matrix<double,15,15>::Identity();T.topLeftCorner<3,3>() = C_WS_0;T.block<3,3>(3,3) = C_WS_0;T.block<3,3>(6,6) = C_WS_0;P = T * P_delta * T.transpose();
}
}
以上code来自以下公式:
其中认为角速度w和加速度a在连续两次imu测量之间是匀速的,因此上式可写成:
其中上式的角速度和加速度:
因此最终公式:
上面公式给出5个变量(R,V,P,b,W)中的3个最重要的变量:R,V,P。
而偏差变量b我们可以初始化的时候可以设为0(其实最好是要求出来的,这里就不给出推倒公式了)。
下面的们就是有关W(权重)的公式了。
其中
是有关R,P,V,b的协方差矩阵
到此为止已经把imu时间误差项求完。
下一篇将是怎样把时间误差项融合到目标函数里(主要是局部地图的优化)
--------------------- 本文来自 金木炎 的CSDN 博客 ,全文地址请点击:https://blog.csdn.net/qq_18661939/article/details/53574981?utm_source=copy
视觉和imu(惯性传感器)( 一)相关推荐
- 一、Arduino+MPU6050/MPU9250——IMU惯性传感器原理
由于最近的一个小项目,接触到了Arduino+MPU9250获取加速度.角速度和磁场数据,并计算Pitch角.Roll角.Yaw角.现对搜索到的资料和复现的代码进行一些整理. 目录 一. IMU原理 ...
- 「 机器人学 」“IMU惯性传感器”讲解
一.前言 惯性传感器(IMU)能够测量传感器本体的角速度和加速度,被认为与相机传感器具有明显的互补性,而且十分有潜力在融合之后得到更完善的SLAM系统. 二.IMU工作特点 IMU虽然可以测得角速度和 ...
- 2.1.6 IMU惯性传感器
更多内容,请关注: github:Autopilot-Updating-Notes gitee: Autopilot-Updating-Notes 2.1.6.1 原理 惯性测量单元IMU(Inert ...
- 基于视觉和惯性传感器的移动机器人手遥操作系统
论文:A Mobile Robot Hand-Arm Teleoperation System by Vision and IMU 相关视频.代码.论文地址:在公众号「3D视觉工坊」,后台回复「视觉和 ...
- VIO系列 | 视觉与惯性传感器如何融合?来研读VINS-Mono论文与代码
点击上方"AI算法修炼营",选择加星标或"置顶" 标题以下,全是干货 一.本系列概述 本系列旨在对前一阶段学习vins-mono开源框架的总结.结合暑假秋招之前 ...
- 视觉-惯导多传感器融合(1)IMU ROS
ROS从入门到精通系列(十六)-- IMU in ROS_Techblog of HaoWANG-CSDN博客1. IMU简介 请移步本人的另一篇博客文章:2. ROS IMU msgs官方文档:ht ...
- 惯性测量单元(IMU)传感器--MEMS微纳制造系列简报
随着新一轮科技革命和产业变革的加速演进,5G.人工智能.物联网等基础设施日趋完善,无人驾驶.无人机.VR/AR等终端应用技术商业化规模快速增长,而连接新一代信息技术的基础技术与终端应用的--以MEMS ...
- MEMS微纳制造系列简报——惯性测量单元(IMU)传感器
随着新一轮科技革命和产业变革的加速演进,5G.人工智能.物联网等基础设施日趋完善,无人驾驶.无人机.VR/AR等终端应用技术商业化规模快速增长,而连接新一代信息技术的基础技术与终端应用的--以MEMS ...
- 惯性传感器(IMU)简介【转】
近两年来,车联网.自动驾驶.无人驾驶.汽车智能化.网联化等成为了汽车行业的热点话题,未来汽车一定是朝着安全.可靠及舒适的方向发展.而这一切背后的发展都离不开传感器的作用,今天我们就来聊聊用途越来越广的 ...
最新文章
- copy()与deepcopy()
- 为什么在iOS7中,UITableView顶部的UITableViewStyleGrouped样式具有额外的填充
- NeurlPS 2021论文预讲会议题全公开,4大主题和25场报告等你来
- SpringBoot使用Websocket
- linux syslog 3
- 2019年三峡大学计算机考研名单,三峡大学2019硕士研究生复试录取方案
- Pure Pursuit纯跟踪算法的Matlab算法实现
- android 迅雷 好用版本,迅雷不限速版本安卓下载-迅雷不限速版 安卓版v6.6.6-PC6安卓网...
- 2021年荷兰经济发展研究报告
- SPSS 24安装教程详细步骤
- 布局改变时的过场动画
- HTML5作品展示摄影网站网页模板源码下载
- JS控制台报错Uncaught TypeError Cannot read properties of null (reading ‘appendChild‘);的解决方法
- 区分gym中的gym.make()函数与gym.vector.make()函数
- iText7高级教程之html2pdf——7.关于pdfHTML经常问的问题
- 机器学习从入门到创业手记-1.4 难以理解的数学知识
- html5 霸刀,基于Html5技术研发3D页游《霸刀》3月28日首测
- unity 游戏存档
- ANDROID SDK体系介绍
- 使用webflow的感觉