昆明网站建设时间,有cms布局的wordpress主题,哪个网站可以做前端项目,网络工程师有前途吗MSCKF5讲#xff1a;后端代码分析 文章目录 MSCKF5讲#xff1a;后端代码分析1 初始化initialize()1.1 加载参数1.2 初始化IMU连续噪声协方差矩阵1.3 卡方检验1.4 接收与订阅话题createRosIO() 2 IMU静止初始化3 重置resetCallback()4 featureCallback4.1 IMU初始化判断4.2 I…MSCKF5讲后端代码分析 文章目录 MSCKF5讲后端代码分析1 初始化initialize()1.1 加载参数1.2 初始化IMU连续噪声协方差矩阵1.3 卡方检验1.4 接收与订阅话题createRosIO() 2 IMU静止初始化3 重置resetCallback()4 featureCallback4.1 IMU初始化判断4.2 IMU积分4.2.1 batchImuProcessing4.2.2 processModel① 移除imu测量偏置② 计算系数矩阵F和噪声矩阵G连续误差状态③ 计算状态转移矩阵Phi离散化的F误差状态④ IMU状态预测4.1 Omega矩阵4.2 获取k时刻的imu状态4.3 预测姿态 G I q T _G^Iq^T GIqT4.4 预测速度v和位置p ⑤ 可观性约束⑥ 更新0空间约束后的状态转移矩阵phi和误差状态协方差矩阵⑦ 更新imu状态量与相机状态量交叉的部分⑧ 强制对称协方差矩阵⑨ 更新零空间 4.3 状态增广 stateAugmentation4.3.1 利用最新的imu状态计算cam状态4.3.2 记录相机状态- imu_state.id4.3.3 更新协方差矩阵① 计算雅可比矩阵② 计算增广协方差矩阵 4.4 添加特征点观测4.5 利用视觉观测更新状态4.5.1 特征点管理4.5.2 计算误差量雅可比与重投影误差① featureJacobian()② measurementJacobian()1 取出观测z和位姿Rwc、路标点p_w2 计算雅可比H3 对H矩阵的可观测性约束4 计算残差r ③ 左零空间投影4.5.3 状态更新① 降维② 卡尔曼滤波更新 4.6 pruneCamStateBuffer4.7 发布位姿publish4.8 是否重置系统onlineReset 这里程序实际上是两个进程 msckf_vio_nodelet.h namespace msckf_vio {
class MsckfVioNodelet : public nodelet::Nodelet {
public:MsckfVioNodelet() { return; }~MsckfVioNodelet() { return; }private:virtual void onInit(); // 虚函数MsckfVioPtr msckf_vio_ptr; // boost::shared_ptrMsckfVio类指针
};
} // end namespace msckf_vionodelet/nodelet.h 提供了 Nodelet 类的定义是用于实现节点的基类。Nodelet 是一种轻量级的ROS节点它可以在同一个进程中共享节点管理和ROS通信以提高系统的效率。Nodelet 允许将不同的ROS节点合并到一个进程中从而减少通信开销。 pluginlib/class_list_macros.h 提供了一组宏用于导出和加载ROS插件包括节点类nodelets。PLUGINLIB_EXPORT_CLASS 是该库中的一个重要宏用于将一个类导出为ROS插件以便它可以在运行时被动态加载。通过这些宏ROS能够在运行时根据类名查找和加载相应的插件。 msckf_vio_nodelet.cpp 在这里getPrivateNodeHandle() 用于获取一个私有节点句柄然后将这个句柄传递给 MsckfVio 类的构造函数。这样做的目的可能是为了让 MsckfVio 类能够在私有命名空间下访问参数主题等ROS资源。 在ROS中私有节点句柄的名称通常是在节点命名空间后添加 “~” 符号来表示的。例如如果节点的名称是 /my_node那么私有节点句柄的名称将是 /my_node/。这有助于确保节点的参数和主题不会与其他节点的冲突。
#include msckf_vio/msckf_vio_nodelet.hnamespace msckf_vio {
void MsckfVioNodelet::onInit() {// 通过 reset 方法初始化为一个新的 MsckfVio 对象// 该对象使用 getPrivateNodeHandle() 进行初始化msckf_vio_ptr.reset(new MsckfVio(getPrivateNodeHandle()));if (!msckf_vio_ptr-initialize()) {ROS_ERROR(Cannot initialize MSCKF VIO...);return;}return;
}// MsckfVioNodelet 类被导出以便 ROS 可以动态加载它
PLUGINLIB_EXPORT_CLASS(msckf_vio::MsckfVioNodelet,nodelet::Nodelet);}struct StateServer {IMUState imu_state;Cam cam_states;// State covariance matrix Eigen::MatrixXd state_cov; // 误差状态(包括IMU和Cam)协方差矩阵Eigen::Matrixdouble, 12, 12 continuous_noise_cov; // IMU连续噪声协方差
};1 初始化initialize() 我们要搞清楚在帧Frame的构造函数中做了那些工作与哪些函数相关联。
后端初始化函数内容1 加载参数loadParameters()2 初始化IMU噪声协方差矩阵初始化state_server.continuous_noise_cov中 n g , n a , n b g , n b a n_{g},n_{a},n_{bg},n_{ba} ng,na,nbg,nba3 卡方检验chi_squared_test_table4 订阅与发布ROS话题调用createRosIO()
1.1 加载参数
bool MsckfVio::initialize()
{// 1. 加载参数if (!loadParameters()) return false; ...return true;
}bool MsckfVio::loadParameters() {// 世界坐标系、里程计坐标系nh.paramstring(fixed_frame_id, fixed_frame_id, world);nh.paramstring(child_frame_id, child_frame_id, robot);// 发布坐标nh.parambool(publish_tf, publish_tf, true);// 帧率nh.paramdouble(frame_rate, frame_rate, 40.0);// 没有”跟丢“概念 判断是否发散 状态的协方差应该在一个范围内超过阈值即resizenh.paramdouble(position_std_threshold, position_std_threshold, 8.0);// 判断是否删除状态nh.paramdouble(rotation_threshold, rotation_threshold, 0.2618);nh.paramdouble(translation_threshold, translation_threshold, 0.4);nh.paramdouble(tracking_rate_threshold, tracking_rate_threshold, 0.5);// Feature optimization parameters nh.paramdouble(feature/config/translation_threshold,Feature::optimization_config.translation_threshold, 0.2);// IMU参数(标准差) 预测 默认值是一些经验值便宜的IMU这方面不好说影响nh.paramdouble(noise/gyro, IMUState::gyro_noise, 0.001); // ngnh.paramdouble(noise/acc, IMUState::acc_noise, 0.01); // nanh.paramdouble(noise/gyro_bias, IMUState::gyro_bias_noise, 0.001); // nbgnh.paramdouble(noise/acc_bias, IMUState::acc_bias_noise, 0.01); // nba// 观测---特征噪声---经验值---nh.paramdouble(noise/feature, Feature::observation_noise, 0.01);// 标定得到是标准差协方差矩阵应该是方差使用方差代替上面标准差IMUState::gyro_noise * IMUState::gyro_noise;IMUState::acc_noise * IMUState::acc_noise;IMUState::gyro_bias_noise * IMUState::gyro_bias_noise;IMUState::acc_bias_noise * IMUState::acc_bias_noise;Feature::observation_noise * Feature::observation_noise;/*设置IMU状态*/// 对于旋转R和偏移t初始化大都默认设置为0.但是对于初始的速度v和偏差b设置为0是否合理// 设置v 0nh.paramdouble(initial_state/velocity/x,state_server.imu_state.velocity(0), 0.0);nh.paramdouble(initial_state/velocity/y,state_server.imu_state.velocity(1), 0.0);nh.paramdouble(initial_state/velocity/z,state_server.imu_state.velocity(2), 0.0);// The initial covariance of orientation and position can be set to 0. // But for velocity, bias and extrinsic parameters,// there should be nontrivial uncertainty.// 初始化误差状态的协方差。 (这里和上面标准差不同之处在于上面是IMU模型这里是指状态量)// 误差状态量:速度 、 g 、加速度double gyro_bias_cov, acc_bias_cov, velocity_cov;nh.paramdouble(initial_covariance/velocity,velocity_cov, 0.25);nh.paramdouble(initial_covariance/gyro_bias,gyro_bias_cov, 1e-4);nh.paramdouble(initial_covariance/acc_bias,acc_bias_cov, 1e-2);// 旋转、平移double extrinsic_rotation_cov, extrinsic_translation_cov;nh.paramdouble(initial_covariance/extrinsic_rotation_cov,extrinsic_rotation_cov, 3.0462e-4);nh.paramdouble(initial_covariance/extrinsic_translation_cov,extrinsic_translation_cov, 1e-4);// 对应论文,不过那个讲师说后面那个外参协方差在其他里面优化效果不好。为什么旋转平移就可以是0因为在正式开始之前我们通过初始化找好了重力方向确定了第一帧的位姿// 0~3 角度// 3~6 陀螺仪偏置// 6~9 速度 // 9~12 加速度计偏置// 12~15 位移// 15~18 外参旋转// 18~21 外参平移state_server.state_cov MatrixXd::Zero(21, 21);for (int i 3; i 6; i)state_server.state_cov(i, i) gyro_bias_cov;for (int i 6; i 9; i)state_server.state_cov(i, i) velocity_cov;for (int i 9; i 12; i)state_server.state_cov(i, i) acc_bias_cov;for (int i 15; i 18; i)state_server.state_cov(i, i) extrinsic_rotation_cov;for (int i 18; i 21; i)state_server.state_cov(i, i) extrinsic_translation_cov;// Transformation offsets between the frames involved.Isometry3d T_imu_cam0 utils::getTransformEigen(nh, cam0/T_cam_imu);Isometry3d T_cam0_imu T_imu_cam0.inverse();state_server.imu_state.R_imu_cam0 T_cam0_imu.linear().transpose();state_server.imu_state.t_cam0_imu T_cam0_imu.translation();CAMState::T_cam0_cam1 utils::getTransformEigen(nh, cam1/T_cn_cnm1);IMUState::T_imu_body utils::getTransformEigen(nh, T_imu_body).inverse();// Maximum number of camera states to be storednh.paramint(max_cam_state_size, max_cam_state_size, 30);return true;
}1.2 初始化IMU连续噪声协方差矩阵 为什么都是三维呢因为角速度和角速度都是三个轴xyz所以就是3*3的协方差矩阵为什么初始只有对角线有值(自身方差)即认为一开始的时候任意两个轴之间应该是没有关系的。协方差越小联系越小。 实际上应该可以表示为3*12的矩阵但是估计写成方阵比较好计算吧。
n g , n a , n b g , n b a n_{g},n_{a},n_{bg},n_{ba} ng,na,nbg,nba
state_server.continuous_noise_cov Matrixdouble, 12, 12::Zero();
state_server.continuous_noise_cov.block3, 3(0, 0) Matrix3d::Identity() * IMUState::gyro_noise;
state_server.continuous_noise_cov.block3, 3(3, 3) Matrix3d::Identity() * IMUState::gyro_bias_noise;
state_server.continuous_noise_cov.block3, 3(6, 6) Matrix3d::Identity() * IMUState::acc_noise;
state_server.continuous_noise_cov.block3, 3(9, 9) Matrix3d::Identity() * IMUState::acc_bias_noise;1.3 卡方检验 在统计学中卡方分布的分位数表示了在给定自由度下该分布中随机变量落在分位数值以下的概率。具体来说3.841 是指在 1 自由度的卡方分布中有 5% 的概率随机变量取值小于或等于 3.841。 这段代码的目的是计算不同自由度下卡方分布的 5% 分位数并将结果保存在 chi_squared_test_table 中
bool MsckfVio::initialize()
{// 卡方检验表// Initialize the chi squared test table with confidence// level 0.95.for (int i 1; i 100; i){boost::math::chi_squared chi_squared_dist(i);chi_squared_test_table[i] boost::math::quantile(chi_squared_dist, 0.05);}return true;
}1.4 接收与订阅话题createRosIO()
bool MsckfVio::initialize()
{if (!createRosIO())return false;
}bool MsckfVio::createRosIO()
{// 发送位姿信息与三维点odom_pub nh.advertisenav_msgs::Odometry(odom, 10);feature_pub nh.advertisesensor_msgs::PointCloud2(feature_point_cloud, 10);// 重置 ----服务通信reset_srv nh.advertiseService(reset, MsckfVio::resetCallback, this);// 接收imu数据与前端跟踪的特征imu_sub nh.subscribe(imu, 100, MsckfVio::imuCallback, this);feature_sub nh.subscribe(features, 40, MsckfVio::featureCallback, this);// 接受真值动作捕捉发来的然后再发送出去为了再rviz可视化mocap_odom_sub nh.subscribe(mocap_odom, 10, MsckfVio::mocapOdomCallback, this);mocap_odom_pub nh.advertisenav_msgs::Odometry(gt_odom, 1);return true;
}2 IMU静止初始化 imuCallback说白了IMU初始化之后就是不停的订阅IMU数据放到buffer直到处理下一帧图像再使用。
void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr msg)
{ // IMU数据没有立即被处理等待下一帧图像// 1. 存放imu数据imu_msg_buffer.push_back(*msg); // std::vectorsensor_msgs::Imu imu_msg_buffer;// 2. 用200个imu数据做静止初始化不够则不做if (!is_gravity_set){if (imu_msg_buffer.size() 200)return;initializeGravityAndBias(); // 静态初始化is_gravity_set true;}
}initializeGravityAndBias imu初始化200个数据必须都是静止时采集的。这里面没有判断是否成功也就是一开始如果运动会导致轨迹飘。 现在分析一下为什么要做IMU初始化从IMU噪声模型分析。 对于角速度静止时理论时角速度应该为0但实际不为0也就是存在偏置 b g b_g bg所以sum_angular_vel/n求取陀螺仪均值作为偏置对于加速度我们得到的时IMU坐标系下的加速度但实际要的时世界系下的加速度目的就是为了求解模型中 R b w {R}_{bw} Rbw.静止时 a w a^w aw是0忽略了加速度偏置既可以得到最简单的计算公式 a b R b w g w {a}^b {R}_{bw}{g}^w abRbwgw。 ω ~ b ω b b g η g a ~ b R b w ( a w − g w ) b a η a \begin{aligned}\tilde{\boldsymbol{\omega}}^b\mathbf{\omega}^b\mathbf{b}_g\mathbf{\eta}_g\\\tilde{\mathbf{a}}^b\mathbf{R}_{bw}(\mathbf{a}^w{-}\mathbf{g}^w)\mathbf{b}_a\mathbf{\eta}_a\end{aligned} ω~ba~bωbbgηgRbw(aw−gw)baηa 实际上如果我们认为g的模长是定值如9.8那么也是可以估计加速度偏置的 补充IMU初始化是为了惯性变量获得良好的初始值这些惯性变量包括重力方向和IMU零偏。先说零偏MU的零偏不是固定的是随时间变化的量。由于零偏对MU的影响较大因此通常作为一个独立的状态来优化。再说重力方向在视觉惯性模式下系统以视觉地图初始化成功的第一帧作为世界坐标系原点此时我们是不知道坐标系中重力的方向的如果不进行MU初始化则无法消除重力对IMU积分的影响。IMU初始化的目的就是把图像建立的世界坐标系的之轴拉到和重力方向平行的状态。
/*** brief imu初始化计算陀螺仪偏置重力方向以及初始姿态必须都是静止且不做加速度计的偏置估计*/
void MsckfVio::initializeGravityAndBias()
{// Initialize gravity and gyro bias.// 1. 角速度与加速度的和Vector3d sum_angular_vel Vector3d::Zero();Vector3d sum_linear_acc Vector3d::Zero();for (const auto imu_msg : imu_msg_buffer){Vector3d angular_vel Vector3d::Zero();Vector3d linear_acc Vector3d::Zero();// tf::vectorMsgToEigen 函数将ROS消息对象 imu_msg 中的角速度信息提取并转换为Vector3d 类型tf::vectorMsgToEigen(imu_msg.angular_velocity, angular_vel);tf::vectorMsgToEigen(imu_msg.linear_acceleration, linear_acc);sum_angular_vel angular_vel;sum_linear_acc linear_acc;}// 2. 因为假设静止的因此陀螺仪理论应该都是0额外读数包括偏置噪声但是噪声属于高斯分布// 因此这一段相加噪声被认为互相抵消了所以剩下的均值被认为是陀螺仪的初始偏置state_server.imu_state.gyro_bias sum_angular_vel / imu_msg_buffer.size();// 3. 计算重力忽略加速度计的偏置那么剩下的就只有重力了Vector3d gravity_imu sum_linear_acc / imu_msg_buffer.size();// 初始化重力本来的方向使估计与惯性系一致因为一开始测得不一定是(0,0,-g)所以需要转换// 注意一个向量不会因为坐标系变化而发生变化所以其模长固定double gravity_norm gravity_imu.norm();IMUState::gravity Vector3d(0.0, 0.0, -gravity_norm); // 得到// 求出当前imu状态的重力方向与实际重力方向的旋转---说白了就是IMU坐标系到世界系的旋转RwiQuaterniond q0_i_w Quaterniond::FromTwoVectors(gravity_imu, -IMUState::gravity);// 得出姿态state_server.imu_state.orientation rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());return;
}3 重置resetCallback()
/*** brief 重置*/
bool MsckfVio::resetCallback(std_srvs::Trigger::Request req,std_srvs::Trigger::Response res)
{// 暂时不订阅相关的数据feature_sub.shutdown();imu_sub.shutdown();// 重置IMU状态.IMUState imu_state state_server.imu_state;imu_state.time 0.0;imu_state.orientation Vector4d(0.0, 0.0, 0.0, 1.0);imu_state.position Vector3d::Zero();imu_state.velocity Vector3d::Zero();imu_state.gyro_bias Vector3d::Zero();imu_state.acc_bias Vector3d::Zero();imu_state.orientation_null Vector4d(0.0, 0.0, 0.0, 1.0);imu_state.position_null Vector3d::Zero();imu_state.velocity_null Vector3d::Zero();// Remove all existing camera states.state_server.cam_states.clear();// Reset the state covariance.double gyro_bias_cov, acc_bias_cov, velocity_cov;nh.paramdouble(initial_covariance/velocity, velocity_cov, 0.25);nh.paramdouble(initial_covariance/gyro_bias, gyro_bias_cov, 1e-4);nh.paramdouble(initial_covariance/acc_bias, acc_bias_cov, 1e-2);double extrinsic_rotation_cov, extrinsic_translation_cov;nh.paramdouble(initial_covariance/extrinsic_rotation_cov, extrinsic_rotation_cov, 3.0462e-4);nh.paramdouble(initial_covariance/extrinsic_translation_cov, extrinsic_translation_cov, 1e-4);state_server.state_cov MatrixXd::Zero(21, 21);for (int i 3; i 6; i)state_server.state_cov(i, i) gyro_bias_cov;for (int i 6; i 9; i)state_server.state_cov(i, i) velocity_cov;for (int i 9; i 12; i)state_server.state_cov(i, i) acc_bias_cov;for (int i 15; i 18; i)state_server.state_cov(i, i) extrinsic_rotation_cov;for (int i 18; i 21; i)state_server.state_cov(i, i) extrinsic_translation_cov;// Clear all exsiting features in the map.map_server.clear();// Clear the IMU msg buffer.imu_msg_buffer.clear();// Reset the starting flags.is_gravity_set false;is_first_img true;// Restart the subscribers.imu_sub nh.subscribe(imu, 100, MsckfVio::imuCallback, this);feature_sub nh.subscribe(features, 40, MsckfVio::featureCallback, this);// TODO: When can the reset fail?res.success true;ROS_WARN(Resetting msckf vio completed...);return true;
}4 featureCallback
/*** brief 后端主要函数处理新来的数据*/
void MsckfVio::featureCallback(const CameraMeasurementConstPtr msg) // msg图像消息
{...// 1 IMU初始化判断// 2 IMU积分batchImuProcessing(msg-header.stamp.toSec());}4.1 IMU初始化判断 // 1. 必须经过imu初始化if (!is_gravity_set)return;// Start the system if the first image is received. The frame where the first image is received will be the origin.// 开始递推状态的第一个时刻为初始化后的第一帧if (is_first_img){is_first_img false;state_server.imu_state.time msg-header.stamp.toSec();}4.2 IMU积分 遍历imu_msg_buffer容器中合法的IMU数据对每一个数据进行processModel积分处理
4.2.1 batchImuProcessing 这个函数的主要功能就是筛选出两帧图像之间的imu消息去进行processModel处理。 /*** brief imu积分批量处理imu数据* param time_bound 处理到这个时间---就是当前新图像msg这个时间*/
void MsckfVio::batchImuProcessing(const double time_bound)
{// Counter how many IMU msgs in the buffer are used.int used_imu_msg_cntr 0;// 取出两帧之间的imu数据去递推位姿// 这里有个细节问题time_bound表示新图片的时间戳// 但是IMU就积分到了距time_bound最近的一个导致时间会差一点点for (const auto imu_msg : imu_msg_buffer){double imu_time imu_msg.header.stamp.toSec();// 小于说明这个数据比较旧因为state_server.imu_state.time代表已经处理过的imu数据的时间if (imu_time state_server.imu_state.time){used_imu_msg_cntr;continue;}// 超过的供下次使用if (imu_time time_bound)break;// Convert the msgs.Vector3d m_gyro, m_acc;tf::vectorMsgToEigen(imu_msg.angular_velocity, m_gyro);tf::vectorMsgToEigen(imu_msg.linear_acceleration, m_acc);// Execute process model.// 递推位姿核心函数processModel(imu_time, m_gyro, m_acc);used_imu_msg_cntr;}// Set the state ID for the new IMU state.// 新的状态更新id相机状态的id也根据这个赋值state_server.imu_state.id IMUState::next_id;// 删掉已经用过// Remove all used IMU msgs.imu_msg_buffer.erase(imu_msg_buffer.begin(),imu_msg_buffer.begin() used_imu_msg_cntr);return;
}4.2.2 processModel void MsckfVio::processModel(const double time, const Vector3d m_gyro, const Vector3d m_acc)
{// 以引用的方式取出IMUState imu_state state_server.imu_state;// 1. imu读数减掉偏置Vector3d gyro m_gyro - imu_state.gyro_bias;Vector3d acc m_acc - imu_state.acc_bias; // acc_bias 初始值是0double dtime time - imu_state.time;// state_server.imu_state应该就是左边界...// 更新时间time即当前处理的那个imu数据的时间戳state_server.imu_state.time time;return;
} 所以每一次调用这个函数都是指两个imu数据之间的一个数据处理预测等。比如dt时间内的状态预测对于姿态认为角速度dt时间内保持不变即欧拉积分直接利用time时刻即imu数据时刻得到的角速度w去预测这个时刻的位姿q。每计算完一个dt就会更新时间state_server.imu_state.time time。所以在观测来到前所有IMU数据依次在上一个数据上预测。这也表明如果没有引入外部的观测这个预测的值一定会发散的 ① 移除imu测量偏置
/*** brief 来一个新的imu数据更新协方差矩阵与状态积分* param time 新数据的时间戳* param m_gyro 角速度* param m_acc 加速度*/
void MsckfVio::processModel(const double time, const Vector3d m_gyro, const Vector3d m_acc)
{// 以引用的方式取出IMUState imu_state state_server.imu_state;// 1. imu读数减掉偏置Vector3d gyro m_gyro - imu_state.gyro_bias;Vector3d acc m_acc - imu_state.acc_bias; // acc_bias 初始值是0double dtime time - imu_state.time;// state_server.imu_state应该就是左边界...// 更新时间time即当前处理的那个imu数据的时间戳state_server.imu_state.time time;return;
} ② 计算系数矩阵F和噪声矩阵G连续误差状态 写成矩阵形式 δ x ˙ F c ⋅ δ x G c ⋅ n \dot{\delta\mathbf{x}}\mathbf{F}_c\cdot\delta\mathbf{x}\mathbf{G}_c\cdot\mathbf{n} δx˙Fc⋅δxGc⋅n 变量 δ x ( G I δ θ ⊤ δ b g ⊤ G δ v I ⊤ δ b a ⊤ G δ p I ⊤ C I δ θ ⊤ I δ p C ⊤ ) ⊤ \left.\delta\mathbf{x}\left(\begin{array}{cccccc}^I_G\delta\boldsymbol{\theta}^\top\delta\mathbf{b}_g^\top^G\delta\mathbf{v}_I^\top\delta\mathbf{b}_a^\top^G\delta\mathbf{p}_I^\top{}^I_C\delta\boldsymbol{\theta}^\top{}^I\delta\mathbf{p}_C^\top\end{array}\right.\right)^\top δx(GIδθ⊤δbg⊤GδvI⊤δba⊤GδpI⊤CIδθ⊤IδpC⊤)⊤ n ( n g n w g n a n w a ) ⊤ \left.\mathbf{n}\left(\begin{array}{ccc}\mathbf{n_g}\mathbf{n_{wg}}\mathbf{n_a}\mathbf{n_{wa}}\end{array}\right.\right)^\top n(ngnwgnanwa)⊤ 矩阵F注意点 ① 速度即第三行忽略了相关因素影响 ② 外参相机与IMU的转换这个是不变的所以第6、7行都是06、7列也是0这里没写出来。 ③ 这里只是5个变量S-MSCKF论文以及代码中都还有外参两个自变量所以F实际是21*21矩阵。7个变量每个都是3×3 F ( − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C ( I C q ^ ) ⊤ ⌊ a ^ × ⌋ 0 3 × 3 0 3 × 3 − C ( I G q ^ ) ⊤ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ) \mathbf{F}\begin{pmatrix}-\lfloor\hat{\boldsymbol{\omega}}_{\times}\rfloor-\mathbf{I}_{3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\-C\left(\frac IC\hat{\mathbf{q}}\right)^\top\left\lfloor\hat{\mathbf{a}}_{\times}\right\rfloor\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}-C\left(\frac IG\hat{\mathbf{q}}\right)^\top\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{I}_{3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3} \end{pmatrix} F −⌊ω^×⌋03×3−C(CIq^)⊤⌊a^×⌋03×303×303×303×3−I303×303×303×303×303×303×303×303×303×303×3I303×303×303×303×3−C(GIq^)⊤03×303×303×303×303×303×303×303×303×303×303×3 矩阵G注意原论文可能给出是下面式子代码中有一个地方对不上应该公式是写错了。只有位姿q、速度v、以及两个偏置b有噪声项。 G.block3, 3(9, 9) Matrix3d::Identity(); G ( − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C ( I G q ^ ) ⊤ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ) \left.\mathbf{G}\left(\begin{array}{cccc}-\mathbf{I}_3\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{I}_3\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}-C\left(\frac IG\hat{\mathbf{q}}\right)^\top\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{I}_3\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{array}\right.\right) G −I303×303×303×303×303×303×303×3I303×303×303×303×303×303×303×3−C(GIq^)⊤03×303×303×303×303×303×303×303×3I303×303×3
// 2. 计算F G矩阵// Compute discrete transition and noise covariance matrixMatrixdouble, 21, 21 F Matrixdouble, 21, 21::Zero();Matrixdouble, 21, 12 G Matrixdouble, 21, 12::Zero();// 误差为真值观测 - 预测// F矩阵表示的是误差的导数的微分方程其实就是想求δ的递推公式// δ F · δ G δ表示误差// δn1 (I F·dt)·δn G·dt·Q Q表示imu噪声// 两种推法一种是通过论文中四元数的推这个过程不再重复// 下面给出李代数推法我们知道msckf的四元数使用的是反人类的jpl// 也就是同一个数值的旋转四元数经过两种不同定义得到的旋转是互为转置的// 这里面的四元数转成的旋转表示的是Riw所以要以李代数推的话也要按照Riw推// 按照下面的旋转更新方式为左乘因此李代数也要用左乘且jpl模式下左乘一个δq Exp(-δθ)// δQj * Qjw Exp(-(w - b) * t) * δQi * Qiw// Exp(-δθj) * Qjw Exp(-(w - b) * t) * Exp(-δθi) * Qiw// 其中Qjw Exp(-(w - b) * t) * Qiw// 两边除得到 Exp(-δθj) Exp(-(w - b) * t) * Exp(-δθi) * Exp(-(w - b) * t).t()// -δθj - Exp(-(w - b) * t) * δθi// δθj (I - (w - b)^ * t) * δθi 得证// 关于偏置一样可以这么算只不过式子变成了// δQj * Qjw Exp(-(w - b - δb) * t) * Qiw// 上式使用bch近似公式可以得到 δθj -t * δb// 其他也可以通过这个方法推出是正确的F.block3, 3(0, 0) -skewSymmetric(gyro);F.block3, 3(0, 3) -Matrix3d::Identity();// imu_state.orientation---qbw 转置即qwbF.block3, 3(6, 0) // acc是imu坐标系下的已经在①中减去了偏置-quaternionToRotation(imu_state.orientation).transpose() * skewSymmetric(acc);F.block3, 3(6, 9) -quaternionToRotation(imu_state.orientation).transpose();F.block3, 3(12, 6) Matrix3d::Identity();G.block3, 3(0, 0) -Matrix3d::Identity();G.block3, 3(3, 3) Matrix3d::Identity();G.block3, 3(6, 6) -quaternionToRotation(imu_state.orientation).transpose();G.block3, 3(9, 9) Matrix3d::Identity();// Approximate matrix exponential to the 3rd order,// which can be considered to be accurate enough assuming// dtime is within 0.01s. 这里主要是为了把连续时间矩阵F离散为状态转移矩阵指数的泰勒3阶展开再乘以时间dtMatrixdouble, 21, 21 Fdt F * dtime; // 1阶近似Matrixdouble, 21, 21 Fdt_square Fdt * Fdt; // 2阶近似Matrixdouble, 21, 21 Fdt_cube Fdt_square * Fdt; // 3阶近似把一个连续系统离散化在线性系统理论中讲过系统矩阵 F F F变为了 状态转移矩阵 Φ ( t Δ t , t ) e x p ( F t ) \Phi(t\Delta t,t) exp(Ft) Φ(tΔt,t)exp(Ft) Φ ( t Δ t , t ) exp ( F c Δ t ) I 21 × 21 F c Δ t 1 2 ! F c 2 Δ t 2 1 3 ! F c 3 Δ t 3 . . . \begin{aligned} \Phi(t\Delta t,t) \exp\left(\mathbf{F}_{c}\Delta t\right) \\ \mathbf{I}_{21\times21}\mathbf{F}_{c}\Delta t\frac{1}{2!}\mathbf{F}_{c}^{2}\Delta t^{2}\frac{1}{3!}\mathbf{F}_{c}^{3}\Delta t^{3} ... \end{aligned} Φ(tΔt,t)exp(FcΔt)I21×21FcΔt2!1Fc2Δt23!1Fc3Δt3... 其中幂指数注意这里F是简化版道理是一样的 F c [ − ⌊ ω ^ × ⌋ − I 3 × 3 0 3 × 3 0 3 × 3 ] , F c 2 [ ⌊ ω ^ × ⌋ 2 ⌊ ω ^ × ⌋ 0 3 × 3 0 3 × 3 ] F c 3 [ − ⌊ ω ^ × ⌋ 3 − ⌊ ω ^ × ⌋ 2 0 3 × 3 0 3 × 3 ] , F c 4 [ ⌊ ω ^ × ⌋ 4 ⌊ ω ^ × ⌋ 3 0 3 × 3 0 3 × 3 ] \begin{aligned}\mathbf{F}_c\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor-\mathbf{I}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\quad,\quad\mathbf{F}_c^2\begin{bmatrix}\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^2\lfloor\hat{\boldsymbol{\omega}}\times\rfloor\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\\\mathbf{F}_c^3\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^3-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^2\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\quad,\quad\mathbf{F}_c^4\begin{bmatrix}\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^4\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^3\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\end{aligned} Fc[−⌊ω^×⌋03×3−I3×303×3],Fc2[⌊ω^×⌋203×3⌊ω^×⌋03×3]Fc3[−⌊ω^×⌋303×3−⌊ω^×⌋203×3],Fc4[⌊ω^×⌋403×3⌊ω^×⌋303×3]
③ 计算状态转移矩阵Phi离散化的F误差状态 // 3. 计算转移矩阵Phi矩阵Matrixdouble, 21, 21 Phi Matrixdouble, 21, 21::Identity() Fdt 0.5 * Fdt_square (1.0 / 6.0) * Fdt_cube;④ IMU状态预测 注意这里是状态而不是误差状态同样我们要把连续时间下IMU状态运动学方程转换为离散时间下IMU运动学方程。为了更加精确的计算对于速度v和位置p进行了四阶龙格库塔法积分。 下面是连续时间IMU运动学方程微分方程后面要离散化差分方程 G I q ˉ ˙ ( t ) 1 2 Ω ( ω ( t ) ) G I q ˉ ( t ) , b ˙ g ( t ) n w g ( t ) v ˙ I ( t ) G a ( t ) , b ˙ a ( t ) n w a ( t ) , p ˙ I ( t ) G v I ( t ) \begin{aligned} {^{I}_{G}\dot{\bar{q}}(t)}\frac{1}{2}\boldsymbol{\Omega}\big(\boldsymbol{\omega}(t)\big)_{G}^{I}\bar{q}(t),\dot{\mathbf{b}}_{g}(t)\mathbf{n}_{wg}(t)\\\dot{\mathbf{v}}_{I}(t){}^{G}\mathbf{a}(t),\dot{\mathbf{b}}_{a}(t)\mathbf{n}_{wa}(t),\dot{\mathbf{p}}_{I}(t){}^{G}\mathbf{v}_{I}(t)\end{aligned} GIqˉ˙(t)v˙I(t)21Ω(ω(t))GIqˉ(t),Ga(t),b˙g(t)b˙a(t)nwg(t)nwa(t),p˙I(t)GvI(t) // Propogate the state using 4th order Runge-Kutta// 4. 四阶龙格库塔积分预测状态predictNewState(dtime, gyro, acc);4.1 Omega矩阵 4.1 Omega矩阵用在向量和四元数的乘积中例如用于四元数导数中 Ω ( ω ) [ 0 ω z − ω y ω x − ω z 0 ω x ω y ω y − ω x 0 ω z − ω x − ω y − ω z 0 ] [ − ⌊ ω × ⌋ ω − ω T 0 ] \left.\boldsymbol{\Omega}(\boldsymbol{\omega})\left[\begin{array}{cccc}0\omega_z-\omega_y\omega_x\\-\omega_z0\omega_x\omega_y\\\omega_y-\omega_x0\omega_z\\-\omega_x-\omega_y-\omega_z0\end{array}\right.\right] \left.\left[\begin{array}{cc}-\lfloor\boldsymbol{\omega}\times\rfloor\boldsymbol{\omega}\\-\boldsymbol{\omega}^\mathrm{T}0\end{array}\right.\right] Ω(ω) 0−ωzωy−ωxωz0−ωx−ωy−ωyωx0−ωzωxωyωz0 [−⌊ω×⌋−ωTω0]
/*** brief 来一个新的imu数据做积分应用四阶龙哥库塔法* param dt 相对上一个数据的间隔时间* param gyro 角速度减去偏置后的* param acc 加速度减去偏置后的*/
void MsckfVio::predictNewState(const double dt, const Vector3d gyro, const Vector3d acc)
{// TODO: Will performing the forward integration using// the inverse of the quaternion give better accuracy?// 角速度标量double gyro_norm gyro.norm(); // 用于选择离散公式// 4.1 计算Omega矩阵Matrix4d Omega Matrix4d::Zero();Omega.block3, 3(0, 0) -skewSymmetric(gyro);Omega.block3, 1(0, 3) gyro;Omega.block1, 3(3, 0) -gyro;...}4.2 获取k时刻的imu状态
void MsckfVio::predictNewState(const double dt, const Vector3d gyro, const Vector3d acc)
{...// 4.2 获取k时刻的imu状态----注意这里是引用所以后面预测会直接更改这里的变量。predictNewState处理之后imu_state就是k1时刻的预测值了。Vector4d q state_server.imu_state.orientation;Vector3d v state_server.imu_state.velocity;Vector3d p state_server.imu_state.position;...
}4.3 预测姿态 G I q T _G^Iq^T GIqT 采用论文Indirect Kalman Filter for 3D Attitude Estimation1.6节0阶离散时间差分方程微分方程差分化 ∣ ω ^ ∣ 1 0 − 5 时: G I q ^ ( t Δ t ) ( cos ( ∣ ω ^ ∣ 2 Δ t ) ⋅ I 4 × 4 1 ∣ ω ^ ∣ sin ( ∣ ω ^ ∣ 2 Δ t ) ⋅ Ω ( ω ^ ) ) G I q ^ ( t ) ∣ ω ^ ∣ ≤ 1 0 − 5 时: G I q ^ ( t Δ t ) ( I 4 × 4 − Δ t 2 Ω ( ω ^ ) ) G I q ^ ( t ) \begin{aligned}|\hat{\omega}|10^{-5}\text{ 时: }^I_G\hat{q}(t\Delta t)\left(\cos\left(\frac{|\hat{\omega}|}2\Delta t\right)\cdot I_{4\times4}\frac1{|\hat{\omega}|}\sin\left(\frac{|\hat{\omega}|}2\Delta t\right)\cdot\Omega(\hat{\omega})\right) {^I_G\hat{q}}(t)\\|\hat{\omega}|\leq10^{-5}\text{ 时: }^I_G\hat{q}(t\Delta t)\left(I_{4\times4}-\frac{\Delta t}2\Omega(\hat{\omega})\right){^I_G\hat{q}(t)}\end{aligned} ∣ω^∣10−5 时: GIq^(tΔt)(cos(2∣ω^∣Δt)⋅I4×4∣ω^∣1sin(2∣ω^∣Δt)⋅Ω(ω^))GIq^(t)∣ω^∣≤10−5 时: GIq^(tΔt)(I4×4−2ΔtΩ(ω^))GIq^(t) // Some pre-calculation// dq_dt表示积分n到n1// dq_dt2表示积分n到n0.5 算龙哥库塔用的Vector4d dq_dt, dq_dt2;if (gyro_norm 1e-5) // 角速度模长{dq_dt (cos(gyro_norm * dt * 0.5) * Matrix4d::Identity() 1 / gyro_norm * sin(gyro_norm * dt * 0.5) * Omega) * q;dq_dt2 (cos(gyro_norm * dt * 0.25) * Matrix4d::Identity() 1 / gyro_norm * sin(gyro_norm * dt * 0.25) * Omega) * q;}else{// 当角增量很小时的近似dq_dt (Matrix4d::Identity() 0.5 * dt * Omega) * cos(gyro_norm * dt * 0.5) * q;dq_dt2 (Matrix4d::Identity() 0.25 * dt * Omega) * cos(gyro_norm * dt * 0.25) * q;}// Rwi 转换为旋转矩阵Matrix3d dR_dt_transpose quaternionToRotation(dq_dt).transpose();Matrix3d dR_dt2_transpose quaternionToRotation(dq_dt2).transpose();q dq_dt;quaternionNormalize(q); // 一定要记得归一化4.4 预测速度v和位置p 四阶龙格库塔法 { y n 1 y n h 6 ( k 1 2 k 2 2 k 3 k 4 ) k 1 f ( x n , y n ) k 2 f ( x n h 2 , y n h 2 k 1 ) k 3 f ( x n ℏ 2 , y n ℏ 2 k 2 ) k 4 f ( x n h , y n h k 3 ) \left.\left\{\begin{array}{l}y_{n1}y_n\frac h6\left(k_12\boldsymbol{k}_22k_3k_4\right)\\k_1f\left(x_n,y_n\right)\\k_2f\left(x_n\frac h2,y_n\frac h2k_1\right)\\k_3f\left(x_n\frac\hbar2,y_n\frac\hbar2\boldsymbol{k}_2\right)\\\boldsymbol{k}_4\boldsymbol{f}\left(\boldsymbol{x}_n\boldsymbol{h},\boldsymbol{y}_n\boldsymbol{h}\boldsymbol{k}_3\right)\end{array}\right.\right. ⎩ ⎨ ⎧yn1yn6h(k12k22k3k4)k1f(xn,yn)k2f(xn2h,yn2hk1)k3f(xn2ℏ,yn2ℏk2)k4f(xnh,ynhk3) 利用龙格库塔法预测速度v和位移p v ^ ( t Δ t ) v ^ ( t ) Δ t 6 ( k v 1 2 k v 2 2 k v 3 k v 4 ) k v 1 R ^ ( t ) a ^ g k v 2 R ^ ( t Δ t / 2 ) a ^ g k v 3 R ^ ( t Δ t / 2 ) a ^ g k v 4 R ^ ( t Δ t ) a ^ g p ^ ( t Δ t ) p ^ ( t ) Δ t 6 ( k p 1 2 k p 2 2 k p 3 k p 4 ) k p 1 v ^ ( t ) k p 2 v ^ ( t ) k v 1 Δ t / 2 k p 3 v ^ ( t ) k v 2 Δ t / 2 k p 4 v ^ ( t ) k v 3 Δ t \begin{aligned} \hat{v}(t\Delta t)\hat{v}(t)\frac{\Delta t}6\left(k_{v_1}2k_{v_2}2k_{v_3}k_{v_4}\right) \\ k_{v_1}\hat{R}(t)\hat{a}g \\ k_{v_2}\hat{R}(t\Delta t/2)\hat{a}g \\ k_{v_3}\hat{R}(t\Delta t/2)\hat{a}g \\ k_{v_4}\hat{R}(t\Delta t)\hat{a}g \\ \hat{p}(t\Delta t)\hat{p}(t)\frac{\Delta t}6\left(k_{p_1}2k_{p_2}2k_{p_3}k_{p_4}\right) \\ k_{p_1}\hat{v}(t) \\ k_{p_2}\hat{v}(t)k_{v_1}\Delta t/2 \\ k_{p_3}\hat{v}(t)k_{v_2}\Delta t/2 \\ k_{p_4}\hat{v}(t)k_{v_3}\Delta t \end{aligned} v^(tΔt)v^(t)6Δt(kv12kv22kv3kv4)kv1R^(t)a^gkv2R^(tΔt/2)a^gkv3R^(tΔt/2)a^gkv4R^(tΔt)a^gp^(tΔt)p^(t)6Δt(kp12kp22kp3kp4)kp1v^(t)kp2v^(t)kv1Δt/2kp3v^(t)kv2Δt/2kp4v^(t)kv3Δt // k1 f(tn, yn) ----kv1Vector3d k1_v_dot quaternionToRotation(q).transpose() * acc IMUState::gravity;Vector3d k1_p_dot v; // kp1// k2 f(tndt/2, ynk1*dt/2)// 这里的4阶LK法用了匀加速度假设即认为前一时刻的加速度和当前时刻相等Vector3d k1_v v k1_v_dot * dt / 2;Vector3d k2_v_dot dR_dt2_transpose * acc IMUState::gravity;Vector3d k2_p_dot k1_v;// k3 f(tndt/2, ynk2*dt/2)Vector3d k2_v v k2_v_dot * dt / 2;Vector3d k3_v_dot dR_dt2_transpose * acc IMUState::gravity;Vector3d k3_p_dot k2_v;// k4 f(tndt, ynk3*dt)Vector3d k3_v v k3_v_dot * dt;Vector3d k4_v_dot dR_dt_transpose * acc IMUState::gravity;Vector3d k4_p_dot k3_v;// yn1 yn dt/6*(k12*k22*k3k4)v v dt / 6 * (k1_v_dot 2 * k2_v_dot 2 * k3_v_dot k4_v_dot);p p dt / 6 * (k1_p_dot 2 * k2_p_dot 2 * k3_p_dot k4_p_dot);return;
}⑤ 可观性约束 这部分不好理解但公式可以对上。 N r , k 1 Φ k N r , k → [ C ( q ^ c , k 1 ∣ k ) c g 0 3 − ⌊ c v ^ t , k 1 ∣ k × ⌋ c g 0 3 − ⌊ c p ^ t , k 1 ∣ k × ⌋ c g ] [ Φ 11 Φ 12 0 3 0 3 0 3 0 3 0 3 I 3 0 3 0 3 0 3 Φ 31 Φ 32 I 3 0 34 0 3 0 3 0 3 0 3 I 3 0 3 0 51 Φ 52 δ t I 3 Φ 54 I 3 ] [ C ( q ^ c , k ∣ k − 1 ) c g 0 3 − ⌊ q ^ t , k ∣ k − 1 × ⌋ c g 0 3 0 3 ] . \mathbf{N}_{r,k1}\mathbf{\Phi}_k\mathbf{N}_{r,k}\quad\to\quad\begin{bmatrix}\mathbf{C}\left(\hat{q}_{c,k1|k}\right)^{c}\mathbf{g}\\\mathbf{0}_{3}\\-\lfloor c\hat{\mathbf{v}}_{t,k1|k}\times\rfloor^{c}\mathbf{g}\\\mathbf{0}_{3}\\-\lfloor{}^{c}\hat{\mathbf{p}}_{t,k1|k}\times\rfloor^{c}\mathbf{g}\end{bmatrix}\begin{bmatrix}\Phi_{11}\Phi_{12}\mathbf{0}_{3}\mathbf{0}_{3}\mathbf{0}_{3}\mathbf{0}_{3}\\\mathbf{0}_{3}\mathbf{I}_{3}\mathbf{0}_{3}\mathbf{0}_{3}\mathbf{0}_{3}\\\Phi_{31}\Phi_{32}\mathbf{I}_{3}\mathbf{0}_{34}\mathbf{0}_{3}\\\mathbf{0}_{3}\mathbf{0}_{3}\mathbf{0}_{3}\mathbf{I}_{3}\mathbf{0}_{3}\\\mathbf{0}_{51}\Phi_{52}\delta t\mathbf{I}_{3}\Phi_{54}\mathbf{I}_{3}\end{bmatrix}\begin{bmatrix}\mathbf{C}\left(\hat{q}_{c,k|k-1}\right)^{c}\mathbf{g}\\\mathbf{0}_{3}\\-\lfloor\hat{q}_{t,k|k-1}\times\rfloor^{c}\mathbf{g}\\\mathbf{0}_{3}\\\mathbf{0}_{3}\end{bmatrix}. Nr,k1ΦkNr,k→ C(q^c,k1∣k)cg03−⌊cv^t,k1∣k×⌋cg03−⌊cp^t,k1∣k×⌋cg Φ1103Φ3103051Φ12I3Φ3203Φ520303I303δtI30303034I3Φ5403030303I303 C(q^c,k∣k−1)cg03−⌊q^t,k∣k−1×⌋cg0303 . 我们把这个矩阵乘出来能得到3个等式主要是修改Φ113151 第1行可以直接接出来 C ( ′ q ˉ ^ G , k 1 ∣ k ) c g Φ 11 C ( ′ q ˉ ^ G , k ∣ k − 1 ) c g → Φ 11 C ( ι , k 1 ∣ k q ~ ^ ι , k ∣ k − 1 ) . \mathbf{C}\left({}^{\prime}\hat{\bar{q}}_{G,k1|k}\right)^{c}\mathbf{g}\Phi_{11}\mathbf{C}\left({}^{\prime}\hat{\bar{q}}_{G,k|k-1}\right)^{c}\mathbf{g}\quad\to\Phi_{11}\mathbf{C}\left({}^{\iota,k1|k}\hat{\tilde{q}}_{\iota,k|k-1}\right). C(′qˉ^G,k1∣k)cgΦ11C(′qˉ^G,k∣k−1)cg→Φ11C(ι,k1∣kq~^ι,k∣k−1). 第3、5行论文中提到线性相关无法直接求或者说由多个解等式中包含了反对 称矩阵导致实际右侧如果按照第⼀个式子那么算就会导致得到矩阵是线性相关的 Φ 31 C ( q ^ c , k ∣ k − 1 ) G g ⌊ G v ^ l , k ∣ k − 1 × ⌋ G g − ⌊ G v ^ l , k 1 ∣ k × ⌋ G g Φ 51 C ( I q ^ G , k ∣ k − 1 ) G g δ t ⌊ G v ^ l , k ∣ k − 1 × ⌋ G g − ⌊ G p ^ l , k 1 ∣ k × ⌋ G g \begin{aligned}\Phi_{31}\mathbf{C}\left(\hat{q}_{c,k|k-1}\right)^G\mathbf{g}\lfloor{}^G\hat{\mathbf{v}}_{l,k|k-1}\times\rfloor^G\mathbf{g}-\lfloor{}^G\hat{\mathbf{v}}_{l,k1|k}\times\rfloor^G\mathbf{g}\\\Phi_{51}\mathbf{C}\left({}^I\hat{q}_{G,k|k-1}\right)^G\mathbf{g}\delta t\lfloor{}^G\hat{\mathbf{v}}_{l,k|k-1}\times\rfloor^G\mathbf{g}-\lfloor{}^G\hat{\mathbf{p}}_{l,k1|k}\times\rfloor^G\mathbf{g}\end{aligned} Φ31C(q^c,k∣k−1)GgΦ51C(Iq^G,k∣k−1)Gg⌊Gv^l,k∣k−1×⌋Gg−⌊Gv^l,k1∣k×⌋Ggδt⌊Gv^l,k∣k−1×⌋Gg−⌊Gp^l,k1∣k×⌋Gg 然后论文里面引入了一个最小二乘类似的约束其中 AΦ31 或 Φ51 min A ∗ ∥ A ∗ − A ∥ F 2 , s.t. A ∗ u w \min_{\mathbf{A}^{*}}\left\|\mathbf{A}^{*}-\mathbf{A}\right\|_{\mathcal{F}}^{2},\quad\text{s.t. }\mathbf{A}^{*}\mathbf{u}\mathbf{w} A∗min∥A∗−A∥F2,s.t. A∗uw A ∗ A − ( A u − w ) ( u T u ) − 1 u T \mathbf{A}^*\mathbf{A}-(\mathbf{A}\mathbf{u}-\mathbf{w})\left(\mathbf{u}^T\mathbf{u}\right)^{-1}\mathbf{u}^T A∗A−(Au−w)(uTu)−1uT // 5. Observability-constrained VINS 可观性约束// Modify the transition matrix// 5.1 修改phi_11// imu_state.orientation_null为上一个imu数据递推后保存的// 这块可能会有疑问因为当上一个imu假如被观测更新了// 导致当前的imu状态是由更新后的上一个imu状态递推而来但是这里的值是没有更新的这个有影响吗// 答案是没有的因为我们更改了phi矩阵保证了零空间// 并且这里必须这么处理因为如果使用更新后的上一个imu状态构建上一时刻的零空间// 就破坏了上上一个跟上一个imu状态之间的0空间// Ni-1 phi_[i-2] * Ni-2// Ni phi_[i-1] * Ni-1^// 如果像上面这样约束那么中间的0空间就“崩了”Matrix3d R_kk_1 quaternionToRotation(imu_state.orientation_null);Phi.block3, 3(0, 0) quaternionToRotation(imu_state.orientation) * R_kk_1.transpose();// 5.2 修改phi_31Vector3d u R_kk_1 * IMUState::gravity;RowVector3d s (u.transpose() * u).inverse() * u.transpose();Matrix3d A1 Phi.block3, 3(6, 0);Vector3d w1 skewSymmetric(imu_state.velocity_null - imu_state.velocity) * IMUState::gravity;Phi.block3, 3(6, 0) A1 - (A1 * u - w1) * s;// 5.3 修改phi_51Matrix3d A2 Phi.block3, 3(12, 0);Vector3d w2 skewSymmetric(dtime * imu_state.velocity_null imu_state.position_null -imu_state.position) *IMUState::gravity;Phi.block3, 3(12, 0) A2 - (A2 * u - w2) * s;⑥ 更新0空间约束后的状态转移矩阵phi和误差状态协方差矩阵 P k 1 ∣ k Φ k P k ∣ k Φ k T Q d , k Q d , k ∫ t k t k 1 Φ ( t k 1 , τ ) G c ( τ ) Q c G c T ( τ ) Φ T ( t k 1 , τ ) d τ \begin{gathered}\mathbf{P}_{k1|k}\Phi_k\mathbf{P}_{k|k}\Phi_k^T\mathbf{Q}_{d,k} \\ \mathbf{Q}_{d,k}\int_{t_k}^{t_{k1}}\boldsymbol{\Phi}\left(t_{k1},\tau\right)\mathbf{G}_c(\tau)\mathbf{Q}_c\mathbf{G}_c^\mathrm{T}(\tau)\boldsymbol{\Phi}^\mathrm{T}\left(t_{k1},\tau\right)\mathrm{d}\tau\end{gathered}\\ Pk1∣kΦkPk∣kΦkTQd,kQd,k∫tktk1Φ(tk1,τ)Gc(τ)QcGcT(τ)ΦT(tk1,τ)dτ // Propogate the state covariance matrix.// 6. 使用0空间约束后的phi计算积分后的新的协方差矩阵// Q是高斯白噪声带来的协方差矩阵代码中简单计算了Matrixdouble, 21, 21 Q Phi * G * state_server.continuous_noise_cov * G.transpose() * Phi.transpose() * dtime; // G就是上面求得F、G// 误差状态协方差矩阵state_server.state_cov.block21, 21(0, 0) Phi * state_server.state_cov.block21, 21(0, 0) * Phi.transpose() Q;⑦ 更新imu状态量与相机状态量交叉的部分 P k 1 ∣ k ( P I I k 1 ∣ k Φ k P I C k ∣ k P I C k ∣ k ⊤ Φ k ⊤ P C C k ∣ k ) \left.\mathbf{P}_{k1|k}\left(\begin{matrix}\mathbf{P}_{II_{k1|k}}\mathbf{\Phi}_{k}\mathbf{P}_{IC_{k|k}}\\\mathbf{P}_{IC_{k|k}}^{\top}\mathbf{\Phi}_{k}^{\top}\mathbf{P}_{CC_{k|k}}\end{matrix}\right.\right) Pk1∣k(PIIk1∣kPICk∣k⊤Φk⊤ΦkPICk∣kPCCk∣k) // 7. 如果有相机状态量那么更新imu状态量与相机状态量交叉的部分if (state_server.cam_states.size() 0){// 起点是0 21 然后是21行 state_server.state_cov.cols() - 21 列的矩阵// 也就是整个协方差矩阵的右上角这部分说白了就是imu状态量与相机状态量的协方差imu更新了这部分也需要更新state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21) Phi * state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21);// 同理这个是左下角state_server.state_cov.block(21, 0, state_server.state_cov.rows() - 21, 21) state_server.state_cov.block(21, 0, state_server.state_cov.rows() - 21, 21) *Phi.transpose();}⑧ 强制对称协方差矩阵 // 8. 强制对称因为协方差矩阵就是对称的MatrixXd state_cov_fixed (state_server.state_cov state_server.state_cov.transpose()) / 2.0;state_server.state_cov state_cov_fixed;⑨ 更新零空间
// Update the state correspondes to null space.// 9. 更新零空间供下个IMU来了使用---// imu_state.orientation这种目前就是上一个时刻状态预测值imu_state.orientation_null imu_state.orientation;imu_state.position_null imu_state.position;imu_state.velocity_null imu_state.velocity;// 更新时间state_server.imu_state.time time;return;
}4.3 状态增广 stateAugmentation 在没有图像进来时对IMU状态进行预测并计算系统误差状态协方差矩阵在有图像进来时根据相机与IMU的相对外参计算当前相机的位姿。然后将最新的相机状态加入到系统状态向量中去然后扩增误差状态协方差矩阵。
4.3.1 利用最新的imu状态计算cam状态 const CameraMeasurementConstPtr msg msg-header.stamp.toSec()前最近的1次IMU预测的状态。理论上和相机时间戳是差了一点的但无伤大雅近似为同一时间。 When new images are received, the state should be augmented with the new camera state. The pose of the new camera state can be computed from the latest IMU state as G C q ^ I C q ^ ⊗ G I q ^ , G p ^ C G p ^ C C ( G I q ^ ) ⊤ I p ^ C \left._G^C\hat{\mathbf{q}}{}_I^C\hat{\mathbf{q}}\otimes{}_G^I\hat{\mathbf{q}},\quad{}^G\hat{\mathbf{p}}_C{}^G\hat{\mathbf{p}}_CC\left(\begin{matrix}^I_G\hat{\mathbf{q}}\end{matrix}\right.\right)^\top{}^I\hat{\mathbf{p}}_C GCq^ICq^⊗GIq^,Gp^CGp^CC(GIq^)⊤Ip^C
/*** brief 根据时间分裂出相机状态* param time 图片的时间戳*/
void MsckfVio::stateAugmentation(const double time)
{// 1. 取出当前更新好的imu状态量// 1.1 取出状态量中的外参imu到cam的外参const Matrix3d R_i_c state_server.imu_state.R_imu_cam0;const Vector3d t_c_i state_server.imu_state.t_cam0_imu;// Add a new camera state to the state server.// 1.2 取出imu旋转平移按照外参将这个时刻cam0的位姿算出来Matrix3d R_w_i quaternionToRotation(state_server.imu_state.orientation);Matrix3d R_w_c R_i_c * R_w_i; //RcwVector3d t_c_w state_server.imu_state.position // twcR_w_i.transpose() * t_c_i;}4.3.2 记录相机状态- imu_state.id // 2. 注册新的相机状态到状态库中// 嗯。。。说人话就是找个记录的不然咋更新state_server.cam_states[state_server.imu_state.id] CAMState(state_server.imu_state.id);CAMState cam_state state_server.cam_states[state_server.imu_state.id];// 严格上讲这个时间不对但是几乎没影响 ---cam时间略大于imu时间cam_state.time time;cam_state.orientation rotationToQuaternion(R_w_c);cam_state.position t_c_w;// 记录第一次被估计的数据不能被改变因为改变了就破坏了之前的0空间cam_state.orientation_null cam_state.orientation;cam_state.position_null cam_state.position;4.3.3 更新协方差矩阵
① 计算雅可比矩阵 6*(216N) J ( J I 0 6 × 6 N ) \mathbf{J}\begin{pmatrix}\mathbf{J}_I\mathbf{0}_{6\times6N}\end{pmatrix} J(JI06×6N) 6*21 J I [ R c b 0 0 0 0 I 0 ( R w b t b c ) ∧ 0 0 0 I 0 R w b ] \left.J_I\left[\begin{array}{cccccccc}R_{cb}\mathbf{0}\mathbf{0}\mathbf{0}\mathbf{0}\mathbf{I}\mathbf{0}\\(R_{wb}t_{bc})^{\wedge}\mathbf{0}\mathbf{0}\mathbf{0}\mathbf{I}\mathbf{0}R_{wb}\end{array}\right.\right] JI[Rcb(Rwbtbc)∧0000000II00Rwb] // 3. 这个雅可比可以认为是cam0位姿相对于imu的状态量的求偏导// 此时我们首先要知道相机位姿是 Rcw twc// Rcw Rci * Riw twc twi Rwi * ticMatrixdouble, 6, 21 J Matrixdouble, 6, 21::Zero();// Rcw对Riw的左扰动导数J.block3, 3(0, 0) R_i_c;// Rcw对Rci的左扰动导数J.block3, 3(0, 15) Matrix3d::Identity();// twc对Riw的左扰动导数// twc twi Rwi * Exp(φ) * tic// twi Rwi * (I φ^) * tic// twi Rwi * tic Rwi * φ^ * tic// twi Rwi * tic - Rwi * tic^ * φ// 这部分的偏导为 -Rwi * tic^ 与论文一致// TODO 试一下 -R_w_i.transpose() * skewSymmetric(t_c_i)// 其实这里可以反过来推一下当给的扰动是// twc twi Exp(-φ) * Rwi * tic// twi (I - φ^) * Rwi * tic// twi Rwi * tic - φ^ * Rwi * tic// twi Rwi * tic (Rwi * tic)^ * φ// 这样跟代码就一样了但是上下定义的扰动方式就不同了J.block3, 3(3, 0) skewSymmetric(R_w_i.transpose() * t_c_i);// 下面是代码里自带的论文中给出的也是下面的结果// J.block3, 3(3, 0) -R_w_i.transpose()*skewSymmetric(t_c_i);// twc对twi的左扰动导数J.block3, 3(3, 12) Matrix3d::Identity();// twc对tic的左扰动导数J.block3, 3(3, 18) R_w_i.transpose();② 计算增广协方差矩阵 就是说原来只有左上角的P现在多了其余三个部分整体来看就是整个矩阵的行维和列维都多了6(old_rows 6, old_cols 6)代码也能看出来。 P ( 21 6 ( N 1 ) ) × ( 21 6 ( N 1 ) ) [ I 21 6 N J 6 × ( 21 6 N ) ] P ( 21 6 N ) × ( 21 6 N ) [ I 21 6 N J 6 × ( 21 6 N ) ] T [ P P J T J P J P J T ] \begin{gathered} P^{(216(N1))\times(216(N1))} \left.\left[\begin{array}{c}I_{216N}\\J_{6\times(216N)}\end{array}\right.\right]P^{(216N)\times(216N)}\left[\begin{array}{c}I_{216N}\\J_{6\times(216N)}\end{array}\right]^T \\ \left.\left[\begin{array}{cc}PPJ^T\\JPJPJ^T\end{array}\right.\right] \end{gathered} P(216(N1))×(216(N1))[I216NJ6×(216N)]P(216N)×(216N)[I216NJ6×(216N)]T[PJPPJTJPJT] // 4. 增广协方差矩阵// 简单地说就是原来的协方差是 21 6n 维的现在新来了一个伙计维度要扩了// 并且对应位置的值要根据雅可比跟这个时刻也就是最新时刻的imu协方差计算// 4.1 扩展矩阵大小 conservativeResize函数不改变原矩阵对应位置的数值-----左上角不需要改变// Resize the state covariance matrix.size_t old_rows state_server.state_cov.rows();size_t old_cols state_server.state_cov.cols();state_server.state_cov.conservativeResize(old_rows 6, old_cols 6);// Rename some matrix blocks for convenience.// imu的协方差矩阵const Matrixdouble, 21, 21 P11 state_server.state_cov.block21, 21(0, 0);// imu相对于各个相机状态量的协方差矩阵不包括最新的--对过去相机状态量的雅可比。就是上一次协方差的右上角const MatrixXd P12 state_server.state_cov.block(0, 21, 21, old_cols - 21);// 4.2 计算协方差矩阵// 左下角state_server.state_cov.block(old_rows, 0, 6, old_cols) J * P11, J * P12;// 右上角-----216N*6---与左下角互为转置state_server.state_cov.block(0, old_cols, old_rows, 6) state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();// 右下角关于相机部分的J都是0所以省略了----6*6矩阵state_server.state_cov.block6, 6(old_rows, old_cols) J * P11 * J.transpose();// 强制对称MatrixXd state_cov_fixed (state_server.state_cov state_server.state_cov.transpose()) /2.0;state_server.state_cov state_cov_fixed;return;
}4.4 添加特征点观测 现在来了一帧新的图像那么就会有新的特征点所以把要添加新的特征信息。
/*** brief 添加特征点观测* param msg 前端发来的特征点信息里面包含了时间左右目上的角点及其id严格意义上不能说是特征点*/
void MsckfVio::addFeatureObservations(const CameraMeasurementConstPtr msg)
{// 这是个long long int 嗯。。。。直接当作int理解吧// 这个id会在 batchImuProcessing 更新StateIDType state_id state_server.imu_state.id;// 1. 获取当前窗口内特征点数量int curr_feature_num map_server.size();int tracked_feature_num 0;// Add new observations for existing features or new// features in the map server.// 2. 添加新来的点做的花里胡哨其实就是在现有的特征管理里面找// id已存在说明是跟踪的点在已有的上面更新// id不存在说明新来的点那么就新添加一个for (const auto feature : msg-features){if (map_server.find(feature.id) map_server.end()){// This is a new feature.map_server[feature.id] Feature(feature.id);map_server[feature.id].observations[state_id] Vector4d(feature.u0, feature.v0,feature.u1, feature.v1);}else{// This is an old feature.map_server[feature.id].observations[state_id] Vector4d(feature.u0, feature.v0,feature.u1, feature.v1);tracked_feature_num;}}// 这个东西计算了当前进来的跟踪的点中在总数里面的占比进来的点有可能是新提的tracking_rate static_castdouble(tracked_feature_num) /static_castdouble(curr_feature_num);return;
}4.5 利用视觉观测更新状态
removeLostFeatures()
4.5.1 特征点管理 这部分与cam有了较大的联系看代码之前要搞清楚下面几个变量。
状态量含义map_serverFeatureIDType, Feature存储满足要求的特征点的map容器键-特征点id值-对应特征点FeatureIDType长整型long long intfeature.observationsStateIDType, Eigen::Vector4d哪些帧观测到了这个特征点相应的归一化坐标是什么 这里主要做了两个工作一个是删除那么无效点一个是找到哪些当前已经观测不到的有效点去进行后续优化。
/*** brief 使用不再跟踪上的点来更新*/
void MsckfVio::removeLostFeatures()
{// 移除哪些追踪丢失的点// BTW, find the size the final Jacobian matrix and residual vector.int jacobian_row_size 0;vectorFeatureIDType invalid_feature_ids(0); // 无效点最后要删的vectorFeatureIDType processed_feature_ids(0); // 待参与更新的点用完也被无情的删掉// 遍历所有特征管理里面的点包括新进来的for (auto iter map_server.begin();iter ! map_server.end(); iter){// Rename the feature to be checked.// 引用改变feature相当于改变iter-second类似于指针的效果auto feature iter-second;// 1. 这个点被当前状态观测到说明这个点后面还有可能被跟踪// 跳过这些点if (feature.observations.find(state_server.imu_state.id) !feature.observations.end())continue;// 2. 跟踪小于3帧的点认为是质量不高的点// 也好理解三角化起码要两个观测但是只有两个没有其他观测来验证if (feature.observations.size() 3){invalid_feature_ids.push_back(feature.id);continue;}// 3. 如果这个特征没有被初始化尝试去初始化// 初始化就是三角化if (!feature.is_initialized){// 3.1 看看运动是否足够没有足够视差或者平移小旋转多这种不符合三角化// 所以就不要这些点了if (!feature.checkMotion(state_server.cam_states)){invalid_feature_ids.push_back(feature.id);continue;}else{// 3.3 尝试三角化失败也不要了if (!feature.initializePosition(state_server.cam_states)){invalid_feature_ids.push_back(feature.id);continue;}}}// 4. 到这里表示这个点能用于更新所以准备下一步计算// 一个观测代表一帧一帧有左右两个观测// 也就是算重投影误差时维度将会是4 * feature.observations.size()// 这里为什么减3下面会提到jacobian_row_size 4 * feature.observations.size() - 3;// 接下来要参与优化的点加入到这个变量中processed_feature_ids.push_back(feature.id);}// Remove the features that do not have enough measurements.// 5. 删掉非法点for (const auto feature_id : invalid_feature_ids)map_server.erase(feature_id);// Return if there is no lost feature to be processed.if (processed_feature_ids.size() 0)return;4.5.2 计算误差量雅可比与重投影误差
// 准备好误差相对于状态量的雅可比MatrixXd H_x MatrixXd::Zero(jacobian_row_size,21 6 * state_server.cam_states.size());VectorXd r VectorXd::Zero(jacobian_row_size);int stack_cntr 0;// 6. 处理特征点for (const auto feature_id : processed_feature_ids){auto feature map_server[feature_id];vectorStateIDType cam_state_ids(0);for (const auto measurement : feature.observations)cam_state_ids.push_back(measurement.first);MatrixXd H_xj;VectorXd r_j;// 6.1 计算雅可比计算重投影误差featureJacobian(feature.id, cam_state_ids, H_xj, r_j);// 6.2 卡方检验剔除错误点并不是所有点都用if (gatingTest(H_xj, r_j, cam_state_ids.size() - 1)){H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) H_xj;r.segment(stack_cntr, r_j.rows()) r_j;stack_cntr H_xj.rows();}// Put an upper bound on the row size of measurement Jacobian,// which helps guarantee the executation time.// 限制最大更新量if (stack_cntr 1500)break;}① featureJacobian() 一个特征观测误差对于位姿的雅可比是4*6对于地图点的雅可比是4*3.因为是双目所以2*2共4维行向量每一个误差都是归一化后的坐标x、y组成 H C i j ∂ z i j ∂ C i , 1 p j ⋅ ∂ C i , 1 p j ∂ x C i , 1 ∂ z i j ∂ C i , 2 p j ⋅ ∂ C i , 2 p j ∂ x C i , 1 H f i j ∂ z i j ∂ C i , 1 p j ⋅ ∂ C i , 1 p j ∂ G p j ∂ z i j ∂ C i , 2 p j ⋅ ∂ C i , 2 p j ∂ G p j \begin{gathered} \mathrm{H}_{C_{i}}^{j} \frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,1}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,1}}\mathbf{p}_{j}}{\partial\mathbf{x}_{C_{i,1}}}\frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,2}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,2}}\mathbf{p}_{j}}{\partial\mathbf{x}_{C_{i,1}}} \\ \mathbf{H}_{f_{i}}^{j} \frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,1}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,1}}\mathbf{p}_{j}}{\partial^{G}\mathbf{p}_{j}}\frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,2}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,2}}\mathbf{p}_{j}}{\partial^{G}\mathbf{p}_{j}} \end{gathered} HCij∂Ci,1pj∂zij⋅∂xCi,1∂Ci,1pj∂Ci,2pj∂zij⋅∂xCi,1∂Ci,2pjHfij∂Ci,1pj∂zij⋅∂Gpj∂Ci,1pj∂Ci,2pj∂zij⋅∂Gpj∂Ci,2pj 整体的求雅可比过程和十四讲一致都是利用链式法则误差量先对相机系下点 C P ( x , y , z ) ^CP(x,y,z) CP(x,y,z)求导然后点 C P ( x , y , z ) ^CP(x,y,z) CP(x,y,z)再分别对位姿和地图点求导。 求雅可比的关键就是①搞清楚误差量是什么 ②搞清楚优化变量是什么一般是位姿和路标点 还有一个值得注意的点就是这里平移量的不同表达会使得雅可比的结果略有不同如下所示l表示做相机 C P R l w ( P w − t w l ) R l w P w t l w ^CP R_{lw}(P_{w} - t_{wl}) R_{lw}P_{w} t_{lw} CPRlw(Pw−twl)RlwPwtlw 后面那种表达就是十四讲上经常使用的方法推导对于位姿和地图点的导数也和上面一致。但是MSCKF中使用的是前面一种表达两种表达本质上一样但对平移量的表示不一致所以会导致对于位姿中平移量的雅可比会有差异很明显点 C P ^CP CP分别对两个平移量求导一个是 R l w R_{lw} Rlw一个是单位矩阵 I I I。 当然还有一点就是MSCKF中都是jpl四元数所有的李代数扰动都是负的。
/*** brief 计算一个路标点的雅可比* param feature_id 路标点id* param cam_state_ids 这个点对应的所有的相机状态id* param H_x 雅可比* param r 误差*/
void MsckfVio::featureJacobian(const FeatureIDType feature_id,const std::vectorStateIDType cam_state_ids,MatrixXd H_x, VectorXd r)
{// 取出特征const auto feature map_server[feature_id];// Check how many camera states in the provided camera// id camera has actually seen this feature.// 1. 统计有效观测的相机状态因为对应的个别状态有可能被滑走了vectorStateIDType valid_cam_state_ids(0);for (const auto cam_id : cam_state_ids){if (feature.observations.find(cam_id) feature.observations.end())continue;valid_cam_state_ids.push_back(cam_id);}int jacobian_row_size 0;// 行数等于4*观测数量一个观测在双目上都有所以是2*2// 此时还没有0空间投影jacobian_row_size 4 * valid_cam_state_ids.size();// 误差相对于状态量的雅可比没有约束列数因为列数一直是最新的// 216N---21是imu状态、外参、N表示了多少个MatrixXd H_xj MatrixXd::Zero(jacobian_row_size,21 state_server.cam_states.size() * 6);// 误差相对于三维点的雅可比MatrixXd H_fj MatrixXd::Zero(jacobian_row_size, 3);// 误差VectorXd r_j VectorXd::Zero(jacobian_row_size);int stack_cntr 0;// 2. 计算每一个观测同一帧左右目这里被叫成一个观测的雅可比与误差for (const auto cam_id : valid_cam_state_ids){// 一个观测对位姿雅可比-4*6 对地图点的雅可比--4*3Matrixdouble, 4, 6 H_xi Matrixdouble, 4, 6::Zero();Matrixdouble, 4, 3 H_fi Matrixdouble, 4, 3::Zero();Vector4d r_i Vector4d::Zero();// 2.1 计算一个左右目观测的雅可比measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);// 计算这个cam_id在整个矩阵的列数因为要在大矩阵里面放auto cam_state_iter state_server.cam_states.find(cam_id);int cam_state_cntr std::distance(state_server.cam_states.begin(), cam_state_iter);// Stack the Jacobians.H_xj.block4, 6(stack_cntr, 21 6 * cam_state_cntr) H_xi;H_fj.block4, 3(stack_cntr, 0) H_fi;r_j.segment4(stack_cntr) r_i;stack_cntr 4;}
② measurementJacobian() 误差量对相机系下点的导数以左相机 C i , 1 C_{i,1} Ci,1为例 ∂ z i j ∂ C i , 1 p j 1 C i , 1 Z ^ j ( 1 0 − C i , 1 X ^ j C i , 1 Z ^ j 0 1 − C i , 1 Y ^ j C i , 1 Z ^ j 0 0 0 0 0 0 ) \left.\frac{\partial\mathbf{z}_i^j}{\partial^{C_{i,1}}\mathbf{p}_j}\frac{1}{C_{i,1}\hat{Z}_j}\left(\begin{array}{ccc}10-\frac{C_{i,1}\hat{X}_j}{C_{i,1}\hat{Z}_j}\\01-\frac{C_{i,1}\hat{Y}_j}{C_{i,1}\hat{Z}_j}\\000\\000\end{array}\right.\right) ∂Ci,1pj∂zijCi,1Z^j1 10000100−Ci,1Z^jCi,1X^j−Ci,1Z^jCi,1Y^j00 相机系点对位姿的雅可比 ∂ C i , 1 p j ∂ x C i , 1 ( ⌊ C i , 1 p ^ j × ⌋ − C ( C i , 1 G q ^ ) ) \left.\left.\frac{\partial^{C_{i,1}}\mathbf{p}_j}{\partial\mathbf{x}_{C_{i,1}}}\left(\left.\lfloor C_{i,1}\hat{\mathbf{p}}_{j\times}\rfloor\quad-C\left(\begin{matrix}C_{i,1}\\G\end{matrix}\right.\right.\right.\hat{\mathbf{q}}\right)\right) ∂xCi,1∂Ci,1pj(⌊Ci,1p^j×⌋−C(Ci,1Gq^)) 相机系点对地图点的雅可比 ∂ C i , 1 p j ∂ G p j C ( C i , 1 G q ^ ) \frac{\partial^{C_{i,1}}\mathbf{p}_j}{\partial^G\mathbf{p}_j}\left.C\left(\begin{matrix}C_i,1\\G\end{matrix}\right.\mathbf{\hat{q}}\right) ∂Gpj∂Ci,1pjC(Ci,1Gq^)
1 取出观测z和位姿Rwc、路标点p_w
/*** brief 计算一个路标点的雅可比* param cam_state_id 有效的相机状态id* param feature_id 路标点id* param H_x 误差相对于位姿的雅可比* param H_f 误差相对于三维点的雅可比* param r 误差*/
void MsckfVio::measurementJacobian(const StateIDType cam_state_id,const FeatureIDType feature_id,Matrixdouble, 4, 6 H_x, Matrixdouble, 4, 3 H_f, Vector4d r)
{// 1. 取出相机状态与特征const CAMState cam_state state_server.cam_states[cam_state_id];const Feature feature map_server[feature_id];// 2. 取出左目位姿根据外参计算右目位姿// Cam0 pose. Rc0wMatrix3d R_w_c0 quaternionToRotation(cam_state.orientation);const Vector3d t_c0_w cam_state.position; // twc0// Cam1 pose. Matrix3d R_c0_c1 CAMState::T_cam0_cam1.linear();// Rc1wMatrix3d R_w_c1 CAMState::T_cam0_cam1.linear() * R_w_c0;Vector3d t_c1_w t_c0_w - R_w_c1.transpose() * CAMState::T_cam0_cam1.translation(); // twc1 twc0-Rwc1*tc1c0// 3. 取出三维点坐标--地图点与观测值z(前端发来的是归一化坐标)const Vector3d p_w feature.position; // 这个特征对于世界系地图点坐标const Vector4d z feature.observations.find(cam_state_id)-second; // 这个特征在当前观测帧下的归一化坐标-----观测值2 计算雅可比H // 4. 转到左右目相机坐标系下// Convert the feature position from the world frame to// the cam0 and cam1 frame.Vector3d p_c0 R_w_c0 * (p_w - t_c0_w);Vector3d p_c1 R_w_c1 * (p_w - t_c1_w);// p_c1 R_c0_c1 * R_w_c0 * (p_w - t_c0_w R_w_c1.transpose() * t_cam0_cam1)// R_c0_c1 * (p_c0 R_w_c0 * R_w_c1.transpose() * t_cam0_cam1)// R_c0_c1 * (p_c0 R_c0_c1 * t_cam0_cam1)// 5. 计算雅可比// 左相机归一化坐标点误差量相对于左相机坐标系下的点的雅可比// (x, y) (X / Z, Y / Z)Matrixdouble, 4, 3 dz_dpc0 Matrixdouble, 4, 3::Zero();dz_dpc0(0, 0) 1 / p_c0(2);dz_dpc0(1, 1) 1 / p_c0(2);dz_dpc0(0, 2) -p_c0(0) / (p_c0(2) * p_c0(2));dz_dpc0(1, 2) -p_c0(1) / (p_c0(2) * p_c0(2));// 与上同理Matrixdouble, 4, 3 dz_dpc1 Matrixdouble, 4, 3::Zero();dz_dpc1(2, 0) 1 / p_c1(2);dz_dpc1(3, 1) 1 / p_c1(2);dz_dpc1(2, 2) -p_c1(0) / (p_c1(2) * p_c1(2));dz_dpc1(3, 2) -p_c1(1) / (p_c1(2) * p_c1(2));// 左相机坐标系下的三维点相对于左相机位姿的雅可比 先r后tMatrixdouble, 3, 6 dpc0_dxc Matrixdouble, 3, 6::Zero();dpc0_dxc.leftCols(3) skewSymmetric(p_c0);dpc0_dxc.rightCols(3) -R_w_c0;// 右相机坐标系下的三维点相对于左相机位姿的雅可比 先r后tMatrixdouble, 3, 6 dpc1_dxc Matrixdouble, 3, 6::Zero();dpc1_dxc.leftCols(3) R_c0_c1 * skewSymmetric(p_c0);dpc1_dxc.rightCols(3) -R_w_c1;// Vector3d p_c0 R_w_c0 * (p_w - t_c0_w);// Vector3d p_c1 R_w_c1 * (p_w - t_c1_w);// 对地图点的雅可比// p_c0 对 p_wMatrix3d dpc0_dpg R_w_c0;// p_c1 对 p_wMatrix3d dpc1_dpg R_w_c1;// 两个雅可比H_x dz_dpc0 * dpc0_dxc dz_dpc1 * dpc1_dxc;H_f dz_dpc0 * dpc0_dpg dz_dpc1 * dpc1_dpg;3 对H矩阵的可观测性约束 H矩阵就是误差状态的雅可比矩阵。 下面代码u就是对于了这个大矩阵右A就是对位姿的雅可比矩阵左。约束限制就是 A u 0 w Au0w Au0w。所以下面公式里的w其实是0. H c a m [ H θ G H p l ] [ C ( q ^ G , k ∣ k − 1 ) G g ( ⌊ G f ^ k ∣ k − 1 × ⌋ − ⌊ G p ^ I , k ∣ k − 1 × ⌋ ) G g ] 0. \mathbf{H}_{cam}\begin{bmatrix}\mathbf{H}_{\boldsymbol{\theta}_G}\mathbf{H}_{\mathbf{p}_l}\end{bmatrix}\begin{bmatrix}\mathbf{C}\left(\hat{\boldsymbol{q}}_{G,k|k-1}\right)^G\mathbf{g}\\\left(\left\lfloor G\hat{\mathbf{f}}_{k|k-1}\times\right\rfloor-\left\lfloor{}^G\hat{\mathbf{p}}_{I,k|k-1}\times\right\rfloor\right)^G\mathbf{g}\end{bmatrix}\mathbf{0}. Hcam[HθGHpl] C(q^G,k∣k−1)Gg(⌊Gf^k∣k−1×⌋−⌊Gp^I,k∣k−1×⌋)Gg 0. H c a m \mathbf{H}_{cam} Hcam对应代码dz_dpc即残差对相机系点 C P ^CP CP H θ G \mathbf{H}_{\theta_G} HθG对应代码dpc_dxc即相机系点 C P ^CP CP对旋转的雅可比 H p l \mathbf{H}_{\mathbf{p}_l} Hpl对应代码dpc_dxc即相机系点 C P ^CP CP对平移的雅可比 H f \mathbf{H}_{\mathbf{f}} Hf对应代码dpc_dpg即相机系点 C P ^CP CP对路标点 W P ^WP WP的雅可比 A ∗ A − ( A u − w ) ( u T u ) − 1 u T \mathbf{A}^*\mathbf{A}-(\mathbf{A}\mathbf{u}-\mathbf{w})\left(\mathbf{u}^T\mathbf{u}\right)^{-1}\mathbf{u}^T A∗A−(Au−w)(uTu)−1uT 解决来之后根据下面公式依次对应新的雅可比。代码中是双目所以行维都是4. H c a m H θ G A 1 : 2 , 1 : 3 ′ , H c a m H p l A 1 : 2 , 4 : 6 ′ , H c a m H f − A 1 : 2 , 4 : 6 ′ \mathbf{H}_{cam}\mathbf{H}_{\theta_G}\mathbf{A}_{1:2,1:3}^{\prime},\mathbf{H}_{cam}\mathbf{H}_{\mathbf{p}_l}\mathbf{A}_{1:2,4:6}^{\prime},\mathbf{H}_{cam}\mathbf{H}_{\mathbf{f}}-\mathbf{A}_{1:2,4:6}^{\prime} HcamHθGA1:2,1:3′,HcamHplA1:2,4:6′,HcamHf−A1:2,4:6′ // Modifty the measurement Jacobian to ensure// observability constrain.// 6. OCMatrixdouble, 4, 6 A H_x;Matrixdouble, 6, 1 u Matrixdouble, 6, 1::Zero();u.block3, 1(0, 0) quaternionToRotation(cam_state.orientation_null) * IMUState::gravity;u.block3, 1(3, 0) skewSymmetric(p_w - cam_state.position_null) * IMUState::gravity;H_x A - A * u * (u.transpose() * u).inverse() * u.transpose();H_f -H_x.block4, 3(0, 3);//4*3大小从0行3列开始取对应公式4 计算残差r 残差 观测 - 预测 对于观测就是视觉中经过匹配得到的结果。 对于预测就是利用imu状态估计出来的cam状态进行预测的。 // 7. 计算归一化平面坐标误差 观测 - 预测r z - Vector4d(p_c0(0) / p_c0(2), p_c0(1) / p_c0(2),p_c1(0) / p_c1(2), p_c1(1) / p_c1(2));③ 左零空间投影 消除路标点带来的不确定性 r j H x j x ~ H f j G p ~ j n j \mathbf{r}^{j}\mathbf{H}_{\mathrm{x}}^{j}\tilde{\mathbf{x}}\mathbf{H}_{f}^{j~G}\tilde{\mathbf{p}}_{j}\mathbf{n}^{j} rjHxjx~Hfj Gp~jnj 左零空间投影 r o j V ⊤ r j V ⊤ H x j x ~ V ⊤ n j H x , o j x ~ n o j \mathbf{r}_o^j\mathbf{V}^\top\mathbf{r}^j\mathbf{V}^\top\mathbf{H}_\mathrm{x}^j\tilde{\mathbf{x}}\mathbf{V}^\top\mathbf{n}^j\mathbf{H}_{\mathrm{x},o}^j\tilde{\mathbf{x}}\mathbf{n}_o^j rojV⊤rjV⊤Hxjx~V⊤njHx,ojx~noj 注意jacobian_row_size - 3为什么是减三因为对特征点列雅可比的维度是3。理论分析哪里用QR分解解释了把Q分为Q1和Q2选择右边的Q2R是jacobian_row_size*3上三角 但是原作者代码实现用的是SVD分解得到结果和QR分解结果一致 // Project the residual and Jacobians onto the nullspace// of H_fj.// 零空间投影JacobiSVDMatrixXd svd_helper(H_fj, ComputeFullU | ComputeThinV);MatrixXd A svd_helper.matrixU().rightCols(jacobian_row_size - 3);// 上面的效果跟QR分解一样下面的代码可以测试打印对比// Eigen::ColPivHouseholderQRMatrixXd qr(H_fj);// MatrixXd Q qr.matrixQ();// std::cout spqr_helper.matrixQ(): std::endl Q std::endl std::endl;// std::cout A: std::endl A std::endl;// 0空间投影H_x A.transpose() * H_xj;r A.transpose() * r_j;return;
}4.5.3 状态更新 零空间投影后的H矩阵和残差r r o j V ⊤ r j V ⊤ H x j x ~ V ⊤ n j H x , o j x ~ n o j \mathbf{r}_o^j\mathbf{V}^\top\mathbf{r}^j\mathbf{V}^\top\mathbf{H}_\mathrm{x}^j\tilde{\mathbf{x}}\mathbf{V}^\top\mathbf{n}^j\mathbf{H}_{\mathrm{x},o}^j\tilde{\mathbf{x}}\mathbf{n}_o^j rojV⊤rjV⊤Hxjx~V⊤njHx,ojx~noj // resize成实际大小H_x.conservativeResize(stack_cntr, H_x.cols());r.conservativeResize(stack_cntr);// 7. 使用误差及雅可比更新状态measurementUpdate(H_x, r);在理论部分中为了降低计算量利用QR分解的特点行维列维这部分可以参考QR分解推导线性方程那部分对H矩阵进行降维详见后端理论推导。
① 降维 这段代码通过QR分解将输入矩阵 H 投影到QR分解的Q的转置上然后提取其中的前几行从而实现对矩阵 H 的降维。这种处理通常用于稀疏矩阵可以减少计算量。 留待后续 /*** brief 更新* param H 雅可比* param r 误差*/
void MsckfVio::measurementUpdate(const MatrixXd H, const VectorXd r)
{if (H.rows() 0 || r.rows() 0)return;// Decompose the final Jacobian matrix to reduce computational// complexity as in Equation (28), (29).MatrixXd H_thin;VectorXd r_thin;if (H.rows() H.cols()){// Convert H to a sparse matrix.SparseMatrixdouble H_sparse H.sparseView();// Perform QR decompostion on H_sparse.// 利用H矩阵稀疏性QR分解// 这段结合零空间投影一起理解主要作用就是降低计算量SPQRSparseMatrixdouble spqr_helper;spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);spqr_helper.compute(H_sparse);MatrixXd H_temp;VectorXd r_temp;(spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);(spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);H_thin H_temp.topRows(21 state_server.cam_states.size() * 6);r_thin r_temp.head(21 state_server.cam_states.size() * 6);// HouseholderQRMatrixXd qr_helper(H);// MatrixXd Q qr_helper.householderQ();// MatrixXd Q1 Q.leftCols(21state_server.cam_states.size()*6);// H_thin Q1.transpose() * H;// r_thin Q1.transpose() * r;}else{H_thin H;r_thin r;}② 卡尔曼滤波更新 最关键的卡尔曼滤波更新 r即计算的残差对于代码delta_x Δ X K r n \Delta\mathbf{X}\mathbf{Kr}_n ΔXKrn 计算卡尔曼增益进而计算误差状态的更新过程V是观测方程的高斯噪声。 K P pred H ⊤ ( H P pred H ⊤ V ) − 1 , δ x K ( z − h ( x p r e d ) ) K r , x x p r e d δ x , P ( I − K H ) P p r e d . \begin{aligned} \boldsymbol{K} P_\text{pred}H^\top(HP_\text{pred}H^\topV)^{-1}, \\ \delta x K(z-h(\boldsymbol{x_\mathrm{pred}})) Kr, \\ \boldsymbol{x} x_{\mathrm{pred}}\delta\boldsymbol{x}, \\ \text{P} (\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H})\boldsymbol{P}_{\mathrm{pred}}. \end{aligned} KδxxPPpredH⊤(HPpredH⊤V)−1,K(z−h(xpred))Kr,xpredδx,(I−KH)Ppred. // 2. 标准的卡尔曼计算过程// Compute the Kalman gain.const MatrixXd P state_server.state_cov;MatrixXd S H_thin * P * H_thin.transpose() Feature::observation_noise * MatrixXd::Identity(H_thin.rows(), H_thin.rows());// MatrixXd K_transpose S.fullPivHouseholderQr().solve(H_thin*P);MatrixXd K_transpose S.ldlt().solve(H_thin * P);MatrixXd K K_transpose.transpose();// Compute the error of the state.VectorXd delta_x K * r_thin; // δx// Update the IMU state.const VectorXd delta_x_imu delta_x.head21();if ( // delta_x_imu.segment3(0).norm() 0.15 ||// delta_x_imu.segment3(3).norm() 0.15 ||delta_x_imu.segment3(6).norm() 0.5 ||// delta_x_imu.segment3(9).norm() 0.5 ||delta_x_imu.segment3(12).norm() 1.0){printf(delta velocity: %f\n, delta_x_imu.segment3(6).norm());printf(delta position: %f\n, delta_x_imu.segment3(12).norm());ROS_WARN(Update change is too large.);// return;}// 3. 更新到imu状态量const Vector4d dq_imu smallAngleQuaternion(delta_x_imu.head3());// 相当于左乘dq_imustate_server.imu_state.orientation quaternionMultiplication(dq_imu, state_server.imu_state.orientation);state_server.imu_state.gyro_bias delta_x_imu.segment3(3);state_server.imu_state.velocity delta_x_imu.segment3(6);state_server.imu_state.acc_bias delta_x_imu.segment3(9);state_server.imu_state.position delta_x_imu.segment3(12);// 外参const Vector4d dq_extrinsic smallAngleQuaternion(delta_x_imu.segment3(15));state_server.imu_state.R_imu_cam0 quaternionToRotation(dq_extrinsic) * state_server.imu_state.R_imu_cam0;state_server.imu_state.t_cam0_imu delta_x_imu.segment3(18);// Update the camera states.// 更新相机姿态auto cam_state_iter state_server.cam_states.begin();for (int i 0; i state_server.cam_states.size(); i, cam_state_iter){const VectorXd delta_x_cam delta_x.segment6(21 i * 6);const Vector4d dq_cam smallAngleQuaternion(delta_x_cam.head3());cam_state_iter-second.orientation quaternionMultiplication(dq_cam, cam_state_iter-second.orientation);cam_state_iter-second.position delta_x_cam.tail3();}// Update state covariance.// 4. 更新协方差MatrixXd I_KH MatrixXd::Identity(K.rows(), H_thin.cols()) - K * H_thin;// state_server.state_cov I_KH*state_server.state_cov*I_KH.transpose() // K*K.transpose()*Feature::observation_noise;state_server.state_cov I_KH * state_server.state_cov;// Fix the covariance to be symmetricMatrixXd state_cov_fixed (state_server.state_cov state_server.state_cov.transpose()) /2.0;state_server.state_cov state_cov_fixed;return;
}4.6 pruneCamStateBuffer
/*** brief 当cam状态数达到最大值时挑出若干cam状态待删除*/
void MsckfVio::pruneCamStateBuffer()
{// 数量还不到该删的程度配置文件里面是20个if (state_server.cam_states.size() max_cam_state_size)return;// Find two camera states to be removed.// 1. 找出该删的相机状态的id两个vectorStateIDType rm_cam_state_ids(0);findRedundantCamStates(rm_cam_state_ids);// Find the size of the Jacobian matrix.// 2. 找到删减帧涉及的观测雅可比大小int jacobian_row_size 0;for (auto item : map_server){auto feature item.second;// Check how many camera states to be removed are associated// with this feature.// 2.1 在待删去的帧中统计能观测到这个特征的帧vectorStateIDType involved_cam_state_ids(0);for (const auto cam_id : rm_cam_state_ids){if (feature.observations.find(cam_id) !feature.observations.end())involved_cam_state_ids.push_back(cam_id);}if (involved_cam_state_ids.size() 0)continue;// 2.2 这个点只在一个里面有观测那就直接删// 只用一个观测更新不了状态if (involved_cam_state_ids.size() 1){feature.observations.erase(involved_cam_state_ids[0]);continue;}// 程序到这里的时候说明找到了一个特征先不说他一共被几帧观测到// 到这里说明被两帧或两帧以上待删减的帧观测到// 2.3 如果没有做过三角化做一下三角化如果失败直接删if (!feature.is_initialized){// Check if the feature can be initialize.if (!feature.checkMotion(state_server.cam_states)){// If the feature cannot be initialized, just remove// the observations associated with the camera states// to be removed.for (const auto cam_id : involved_cam_state_ids)feature.observations.erase(cam_id);continue;}else{if (!feature.initializePosition(state_server.cam_states)){for (const auto cam_id : involved_cam_state_ids)feature.observations.erase(cam_id);continue;}}}// 2.4 最后的最后得出了行数// 意味着有involved_cam_state_ids.size() 数量的观测要被删去// 但是因为待删去的帧间有共同观测的关系直接删会损失这部分信息// 所以临删前做最后一次更新jacobian_row_size 4 * involved_cam_state_ids.size() - 3;}// cout jacobian row #: jacobian_row_size endl;// Compute the Jacobian and residual.// 3. 计算待删掉的这部分观测的雅可比与误差// 预设大小MatrixXd H_x MatrixXd::Zero(jacobian_row_size,21 6 * state_server.cam_states.size());VectorXd r VectorXd::Zero(jacobian_row_size);int stack_cntr 0;// 又做了一遍类似上面的遍历只不过该三角化的已经三角化该删的已经删了for (auto item : map_server){auto feature item.second;// Check how many camera states to be removed are associated// with this feature.// 这段就是判断一下这个点是否都在待删除帧中有观测vectorStateIDType involved_cam_state_ids(0);for (const auto cam_id : rm_cam_state_ids){if (feature.observations.find(cam_id) !feature.observations.end())involved_cam_state_ids.push_back(cam_id);}// 一个的情况已经被删掉了if (involved_cam_state_ids.size() 0)continue;// 计算出待删去的这部分的雅可比// 这个点假如有多个观测但本次只用待删除帧上的观测MatrixXd H_xj;VectorXd r_j;featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())){H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) H_xj;r.segment(stack_cntr, r_j.rows()) r_j;stack_cntr H_xj.rows();}// 删去观测for (const auto cam_id : involved_cam_state_ids)feature.observations.erase(cam_id);}H_x.conservativeResize(stack_cntr, H_x.cols());r.conservativeResize(stack_cntr);// Perform measurement update.// 4. 用待删去的这些观测更新一下measurementUpdate(H_x, r);// 5. 直接删掉对应的行列直接干掉// 为啥没有做类似于边缘化的操作// 个人认为是上面做最后的更新了所以信息已经更新到了各个地方for (const auto cam_id : rm_cam_state_ids){int cam_sequence std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id));int cam_state_start 21 6 * cam_sequence;int cam_state_end cam_state_start 6;// Remove the corresponding rows and columns in the state// covariance matrix.if (cam_state_end state_server.state_cov.rows()){state_server.state_cov.block(cam_state_start, 0,state_server.state_cov.rows() - cam_state_end,state_server.state_cov.cols()) state_server.state_cov.block(cam_state_end, 0,state_server.state_cov.rows() - cam_state_end,state_server.state_cov.cols());state_server.state_cov.block(0, cam_state_start,state_server.state_cov.rows(),state_server.state_cov.cols() - cam_state_end) state_server.state_cov.block(0, cam_state_end,state_server.state_cov.rows(),state_server.state_cov.cols() - cam_state_end);state_server.state_cov.conservativeResize(state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);}else{state_server.state_cov.conservativeResize(state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);}// Remove this camera state in the state vector.state_server.cam_states.erase(cam_id);}return;
}4.7 发布位姿publish
void MsckfVio::publish(const ros::Time time)
{// Convert the IMU frame to the body frame.// 1. 计算body坐标因为imu与body相对位姿是单位矩阵所以就是imu的坐标const IMUState imu_state state_server.imu_state;Eigen::Isometry3d T_i_w Eigen::Isometry3d::Identity();T_i_w.linear() quaternionToRotation(imu_state.orientation).transpose();T_i_w.translation() imu_state.position;Eigen::Isometry3d T_b_w IMUState::T_imu_body * T_i_w *IMUState::T_imu_body.inverse();Eigen::Vector3d body_velocity IMUState::T_imu_body.linear() * imu_state.velocity;// Publish tf// 2. 发布tf实时的位姿没有轨迹没有协方差if (publish_tf){tf::Transform T_b_w_tf;tf::transformEigenToTF(T_b_w, T_b_w_tf);tf_pub.sendTransform(tf::StampedTransform(T_b_w_tf, time, fixed_frame_id, child_frame_id));}// Publish the odometry// 3. 发布位姿能在rviz留下轨迹的nav_msgs::Odometry odom_msg;odom_msg.header.stamp time;odom_msg.header.frame_id fixed_frame_id;odom_msg.child_frame_id child_frame_id;tf::poseEigenToMsg(T_b_w, odom_msg.pose.pose);tf::vectorEigenToMsg(body_velocity, odom_msg.twist.twist.linear);// Convert the covariance.// 协方差取出旋转平移部分以及它们之间的公共部分组成6自由度的协方差Matrix3d P_oo state_server.state_cov.block3, 3(0, 0);Matrix3d P_op state_server.state_cov.block3, 3(0, 12);Matrix3d P_po state_server.state_cov.block3, 3(12, 0);Matrix3d P_pp state_server.state_cov.block3, 3(12, 12);Matrixdouble, 6, 6 P_imu_pose Matrixdouble, 6, 6::Zero();P_imu_pose P_pp, P_po, P_op, P_oo;// 转下坐标但是这里都是单位矩阵Matrixdouble, 6, 6 H_pose Matrixdouble, 6, 6::Zero();H_pose.block3, 3(0, 0) IMUState::T_imu_body.linear();H_pose.block3, 3(3, 3) IMUState::T_imu_body.linear();Matrixdouble, 6, 6 P_body_pose H_pose *P_imu_pose * H_pose.transpose();// 填充协方差for (int i 0; i 6; i)for (int j 0; j 6; j)odom_msg.pose.covariance[6 * i j] P_body_pose(i, j);// Construct the covariance for the velocity.// 速度协方差Matrix3d P_imu_vel state_server.state_cov.block3, 3(6, 6);Matrix3d H_vel IMUState::T_imu_body.linear();Matrix3d P_body_vel H_vel * P_imu_vel * H_vel.transpose();for (int i 0; i 3; i)for (int j 0; j 3; j)odom_msg.twist.covariance[i * 6 j] P_body_vel(i, j);// 发布位姿odom_pub.publish(odom_msg);// Publish the 3D positions of the features that// has been initialized.// 4. 发布点云boost::shared_ptrpcl::PointCloudpcl::PointXYZ feature_msg_ptr(new pcl::PointCloudpcl::PointXYZ());feature_msg_ptr-header.frame_id fixed_frame_id;feature_msg_ptr-height 1;for (const auto item : map_server){const auto feature item.second;if (feature.is_initialized){Vector3d feature_position IMUState::T_imu_body.linear() * feature.position;feature_msg_ptr-points.push_back(pcl::PointXYZ(feature_position(0), feature_position(1), feature_position(2)));}}feature_msg_ptr-width feature_msg_ptr-points.size();feature_pub.publish(feature_msg_ptr);return;
}4.8 是否重置系统onlineReset
void MsckfVio::onlineReset()
{// Never perform online reset if position std threshold// is non-positive.if (position_std_threshold 0)return;static long long int online_reset_counter 0;// Check the uncertainty of positions to determine if// the system can be reset.double position_x_std std::sqrt(state_server.state_cov(12, 12));double position_y_std std::sqrt(state_server.state_cov(13, 13));double position_z_std std::sqrt(state_server.state_cov(14, 14));if (position_x_std position_std_threshold position_y_std position_std_threshold position_z_std position_std_threshold)return;ROS_WARN(Start %lld online reset procedure...,online_reset_counter);ROS_INFO(Stardard deviation in xyz: %f, %f, %f,position_x_std, position_y_std, position_z_std);// Remove all existing camera states.state_server.cam_states.clear();// Clear all exsiting features in the map.map_server.clear();// Reset the state covariance.double gyro_bias_cov, acc_bias_cov, velocity_cov;nh.paramdouble(initial_covariance/velocity,velocity_cov, 0.25);nh.paramdouble(initial_covariance/gyro_bias,gyro_bias_cov, 1e-4);nh.paramdouble(initial_covariance/acc_bias,acc_bias_cov, 1e-2);double extrinsic_rotation_cov, extrinsic_translation_cov;nh.paramdouble(initial_covariance/extrinsic_rotation_cov,extrinsic_rotation_cov, 3.0462e-4);nh.paramdouble(initial_covariance/extrinsic_translation_cov,extrinsic_translation_cov, 1e-4);state_server.state_cov MatrixXd::Zero(21, 21);for (int i 3; i 6; i)state_server.state_cov(i, i) gyro_bias_cov;for (int i 6; i 9; i)state_server.state_cov(i, i) velocity_cov;for (int i 9; i 12; i)state_server.state_cov(i, i) acc_bias_cov;for (int i 15; i 18; i)state_server.state_cov(i, i) extrinsic_rotation_cov;for (int i 18; i 21; i)state_server.state_cov(i, i) extrinsic_translation_cov;ROS_WARN(%lld online reset complete..., online_reset_counter);return;
}
