視覺和imu（慣性感測器）
阿新 • • 發佈：2018-12-12
由於轉載的地方沒有標明出處，這裡也沒提供出處。
目前單目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 propagation
- Eigen::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 small
- Eigen::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-Jacobians
- Eigen::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 delta
- double nexttime;
- if ((it + 1) == (measurements.size()-1)) {
- nexttime = end;
- }
- else
- nexttime =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();
- }
- else
- nexttime =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 integrity
- double 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));//w
- const 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 parts
- dalpha_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 propagation
- if (covariance) {
- Eigen::Matrix<double,15,15> F_delta = Eigen::Matrix<double,15,15>::Identity();
- // transform
- F_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 shift
- Delta_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_a
- F.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 states
- Eigen::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（權重）的公式了。
其中
到此為止已經把imu時間誤差項求完。
下一篇將是怎樣把時間誤差項融合到目標函式裡（主要是區域性地圖的優化）
--------------------- 本文來自 學之之博未若之之要-知之之要未若行之之實 的CSDN 部落格 ，全文地址請點選：https://blog.csdn.net/datase/article/details/78586880?utm_source=copy