0 相關參考
《Quaternion kinematics for error-state kalman filter》、《Indirect Kalman Filter for 3D Attitude Estimation》、《SLAM十四講》
1 旋轉角度距離計算
2VINS中相關代碼
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
//solveRelativeR函數中可以根據corres中特徵點的對應關係計算出前後兩幀圖像之間的旋轉矩陣,加到Rc中,Rc表示相機位姿的旋轉矩陣
Rc.push_back(solveRelativeR(corres));
//delta_q_imu爲IMU預積分得到的旋轉四元數,轉換成矩陣形式存入Rimu當中。則Rimu中存放的是imu預積分得到的旋轉矩陣
Rimu.push_back(delta_q_imu.toRotationMatrix());
//每幀IMU相對於起始幀IMU的R,ric初始化值爲單位矩陣,則Rc_g中存入的第一個旋轉向量爲IMU的旋轉矩陣
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
Eigen::MatrixXd A(frame_count * 4, 4);
A.setZero();
int sum_ok = 0;
//遍歷滑動窗口中的每一幀
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]); //相機位姿的旋轉矩陣
Quaterniond r2(Rc_g[i]); //IMU的旋轉矩陣
//旋轉角度距離計算,這裏計算的是圖像幀和IMU數據之間的旋轉角度的距離
//TODO 四元數函數angularDistance()操作參考鏈接:[https://blog.csdn.net/hzwwpgmwy/article/details/84846016#1__116]
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
ROS_DEBUG(
"%d %f", i, angular_distance);
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
++sum_ok;
Matrix4d L, R;
double w = Quaterniond(Rc[i]).w();
Vector3d q = Quaterniond(Rc[i]).vec();
L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
L.block<3, 1>(0, 3) = q;
L.block<1, 3>(3, 0) = -q.transpose();
L(3, 3) = w;
Quaterniond R_ij(Rimu[i]);
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w;
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
}
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
ric = estimated_R.toRotationMatrix().inverse();
//cout << svd.singularValues().transpose() << endl;
//cout << ric << endl;
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}