1. 主函数
（1）大致流程文章来源：https://uudwc.com/A/b13Yy
- 读取配置文件参数readParameters
- 优化器参数设置setParameter：包括外参与时延，以及视觉重投影的置信度
- 接收imu消息，执行回调函数imu_callback
- 接收前端视觉光流结果，执行回调函数feature_callback
- 接收前端重启命令，执行回调函数restart_callback（在前端光流跟丢之后进行重启）
- 接收回环检测消息，执行回调函数relocalization_callback
- 处理核心线程process
（2）代码实现
// vins_estimator/src/estimator_node.cpp
int main(int argc, char **argv)
{
ros::init(argc, argv, "vins_estimator"); // 初始化ros节点
ros::NodeHandle n("~"); // ros句柄
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
// 与之前跟踪线程feature_tracker_node.cpp中的一样，也是通过ros句柄读取参数
readParameters(n);
// 设置优化器estimator中每个相机的旋转ric 和 平移tic, 以及时间td
estimator.setParameter(); // 外参, 重投影置信度, 延时设置 将读取的这些参数赋给优化器
#ifdef EIGEN_DONT_PARALLELIZE
ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
ROS_WARN("waiting for image and imu...");
// 注册一些publisher
registerPub(n);
// 接收imu消息 每收到一帧IMU数据，都会执行imu_callback回调函数
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
// 接收前端视觉光流结果 执行前端光流结果的回调函数feature_callback
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
// 接收前端重启命令 前端光流跟丢之后就需要重启, 执行重启的回调函数restart_callback
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
// 回环检测的fast relocalization响应
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
// 核心处理线程
std::thread measurement_process{process};
ros::spin();
return 0;
}
1.1 读取配置文件readParameters
（1）大致流程
- 读取优化相关参数：单次优化最大求解时间、单次优化最大求解次数
- 通过虚拟焦距确定用来判断是否为关键帧的视差阈值MIN_PARALLAX
- 获取的imu、图像相关参数
- 根据外参标志进行初始化分类：外参准确无需优化、有较好的外参初值但需优化，需要进行初始化
- 读取传感器时延相关参数
（2）代码实现
// vins_estimator/src/parameters.cpp
void readParameters(ros::NodeHandle &n)
{
std::string config_file;
// 读取配置文件(yaml文件)
config_file = readParam<std::string>(n, "config_file");
// 使用cv::FileStorage打开yaml文件
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
fsSettings["imu_topic"] >> IMU_TOPIC;
SOLVER_TIME = fsSettings["max_solver_time"]; // 单次优化最大求解时间
NUM_ITERATIONS = fsSettings["max_num_iterations"]; // 单次优化最大迭代次数
// 同一个特征点在两个图像中的位置距离若超过设置的距离阈值(视差)，则认为当前帧为关键帧
MIN_PARALLAX = fsSettings["keyframe_parallax"]; // 根据视差确定关键帧
/*
虚拟相机的作用
若每个相机的分辨率都不一样，对应的像素平面分辨率就不一样，即像素平面上相邻像素之间的距离不一样。那么，就无法根据同一个视差阈值MIN_PARALLAX来确定关键帧，使得MIN_PARALLAX需要根据分辨率来进行调整。在实际中，为了避免实时调整MIN_PARALLAX参数，使用给定的虚拟相机焦距，使得归一化平面上的点都统一映射到虚拟相机的像素平面上，并且按照下面公式更新视差阈值MIN_PARALLAX。这样，对于不同相机，其视差阈值MIN_PARALLAX都是同一个，因此无需进行调整。
*/
MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;
std::string OUTPUT_PATH; // 输出结果的路径
fsSettings["output_path"] >> OUTPUT_PATH;
VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv";
std::cout << "result path " << VINS_RESULT_PATH << std::endl;
// create folder if not exists
FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str());
std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
fout.close();
// 预先获取的imu、图像相关参数
ACC_N = fsSettings["acc_n"]; // 加速度计噪声
ACC_W = fsSettings["acc_w"]; // 加速度计随机游走
GYR_N = fsSettings["gyr_n"]; // 陀螺仪噪声
GYR_W = fsSettings["gyr_w"]; // 陀螺仪随机游走
G.z() = fsSettings["g_norm"]; // 重力方向
ROW = fsSettings["image_height"]; // 图像高度
COL = fsSettings["image_width"]; // 图像宽度
ROS_INFO("ROW: %f COL: %f ", ROW, COL);
ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"];
// 若旋转外参标志ESTIMATE_EXTRINSIC为2，则表明没有任何先验，需要对其进行初始化
// 平移外参是不需要初始化的
if (ESTIMATE_EXTRINSIC == 2)
{
ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param");
RIC.push_back(Eigen::Matrix3d::Identity()); // 旋转外参RIC初始化为单位阵
TIC.push_back(Eigen::Vector3d::Zero()); // 平移外参TIC初始化为零向量
EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
}
else
{
if ( ESTIMATE_EXTRINSIC == 1)
{
// 若旋转外参标志ESTIMATE_EXTRINSIC为1, 则表明已有一个较好的初始值，直接将其输送给后端进行优化
ROS_WARN(" Optimize extrinsic param around initial guess!");
EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
}
if (ESTIMATE_EXTRINSIC == 0)
// 若旋转外参标志ESTIMATE_EXTRINSIC为0, 则表示旋转外参足够精确, 将其固定住，不参与后续优化
ROS_WARN(" fix extrinsic param ");
// ESTIMATE_EXTRINSIC == 1 或 0的情况表示已经有了较好的先验旋转外参
cv::Mat cv_R, cv_T;
fsSettings["extrinsicRotation"] >> cv_R; // 获取已有的旋转矩阵
fsSettings["extrinsicTranslation"] >> cv_T; // 获取已有的平移向量
Eigen::Matrix3d eigen_R;
Eigen::Vector3d eigen_T;
cv::cv2eigen(cv_R, eigen_R); // 将旋转矩阵转成eigen格式
cv::cv2eigen(cv_T, eigen_T); // 将平移向量转成eigen格式
Eigen::Quaterniond Q(eigen_R); // 将旋转转成四元数
eigen_R = Q.normalized(); // 将四元数转成单位四元数, 如此才可以表示旋转位姿
RIC.push_back(eigen_R); // 获取旋转外参初值
TIC.push_back(eigen_T); // 获取平移外参初值
ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]);
ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose());
}
INIT_DEPTH = 5.0; // 特征点深度的默认值, 若三角化不成功, 则赋予此处的默认深度值
BIAS_ACC_THRESHOLD = 0.1; // 没用到
BIAS_GYR_THRESHOLD = 0.1;
// 传感器时间延时相关
TD = fsSettings["td"];
ESTIMATE_TD = fsSettings["estimate_td"];
if (ESTIMATE_TD)
ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD);
else
ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD);
ROLLING_SHUTTER = fsSettings["rolling_shutter"];
if (ROLLING_SHUTTER)
{
TR = fsSettings["rolling_shutter_tr"];
ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR);
}
else
{
TR = 0;
}
fsSettings.release();
}
1.2 优化器参数预设setParameter
主要是进行外参和延时的参数传递，以及通过虚拟相机设置视觉重投影的置信度。
// vins_estimator/src/estimator.cpp
/**
* @brief 外参，视觉重投影置信度，延时设置
*
*/
void Estimator::setParameter()
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
tic[i] = TIC[i];
ric[i] = RIC[i];
}
f_manager.setRic(ric);
// 通过虚拟相机，将视觉重投影置信度设置为1.5个像素内
// 也就是说，对于所提取的当前特征点，认为其真值在周围的1.5个像素之内
ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
}
1.3 接收到IMU数据时触发的回调函数imu_callback
（1）大致思路
- 进行时间戳的异常判断
- 在使用线程锁的情况下，将imu数据送入imu_buf中进行备份存储（在后面process主线程中会用到）
- 使用接收到的imu消息，调用predict更新PVQ与零偏
- 在完成初始化的前提下，将更新的PVQ发布出去
（2）代码实现
vins_estimator/src/estimator_node.cpp
/**
* @brief imu消息存进buffer，同时按照imu频率预测位姿(tmp_P, tmp_Q, tmp_V)并发送，这样就可以提高里程计频率
*
* @param[in] imu_msg
*/
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
if (imu_msg->header.stamp.toSec() <= last_imu_t) // 时间戳的判断
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
// 线程锁的作用：保证在队列中读数据与写数据不能同时进行，否则就会发生线程锁冲突
m_buf.lock();
imu_buf.push(imu_msg); // 将imu数据送入imu_buf中
m_buf.unlock();
// 条件变量con.notify_one()保证cpu占用率不会过高（不会一直查询），写完数据后通知系统读数据
// 若没有接到通知，则该线程处于休眠状态，不占用cpu资源。
con.notify_one();
last_imu_t = imu_msg->header.stamp.toSec();
{
std::lock_guard<std::mutex> lg(m_state);
predict(imu_msg); // 使用imu数据更新PVQ：tmp_P, tmp_V, tmp_Q
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
// 只有初始化完成后才发送当前结果 将更新的PVQ发布出去
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}
}
（3）predict更新imu的PVQ
vins_estimator/src/estimator_node.cpp
/**
* @brief 根据当前imu数据预测当前位姿 更新IMU状态量PVQ
*
* @param[in] imu_msg
*/
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
if (init_imu) // 若为第一帧imu数据，则什么也不干
{
latest_time = t;
init_imu = 0;
return;
}
double dt = t - latest_time; // 相邻imu帧之间的时间差
latest_time = t;
// 得到IMU的加速度计测量值
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
// 得到IMU的陀螺仪测量值
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
// acc_0: 上一时刻imu的加速度测量值(imu坐标系下的)
// linear_acceleration:当前时刻imu的加速度测量值(imu坐标系下的)
// un_acc_0: 上一时刻imu在世界坐标系下的加速度(去掉重力加速度后的)
// un_acc_1: 当前时刻imu在世界坐标系下的加速度(去掉重力加速度后的)
// gyr_0: 上一时刻imu中陀螺仪测量值(角速度)
// angular_velocity：当前时刻imu中陀螺仪测量值(角速度)
// un_gyr: 中值法计算得到的两相邻时刻的中值角速度\bar{w}_k
// un_acc: 中值法计算得到的两相邻时刻的中值加速度\bar{a}_k
// tmp_Q: 上一时刻imu在世界坐标系下的位姿R_wi
// 加速度数学模型: \hat{a}_b = q_{bw}(a_w + g) + b_a ==> a_w = q_{wb} * (\hat{a}_b - b_a) - g
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
// 中值法计算得到的两相邻时刻的中值角速度\bar{w}_k
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
// 通过四元数乘法更新当前帧姿态Q q = q \otimes \Delta q
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
// 当前时刻imu在世界坐标系下的加速度(去掉重力加速度后的)
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
// 中值法计算得到的两相邻时刻的中值加速度\bar{a}_k
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
// 经典物理中位置，速度更新方程 (前提是获取当前时刻世界坐标系下的加速度un_acc)
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc; // 更新位移P
tmp_V = tmp_V + dt * un_acc; // 更新速度V
// 将当前时刻加速度计的测量值 更新为 上一时刻加速度计的测量值
acc_0 = linear_acceleration;
// 将当前时刻陀螺仪的测量值 更新为 上一时刻陀螺仪的测量值
gyr_0 = angular_velocity;
}
// 以上过程得到状态量: tmp_P, tmp_V, tmp_Q, 并更新相关参数(如acc_0,gyr_0)
1.4 接收前端光流时触发的回调函数feature_callback
vins_estimator/src/estimator_node.cpp
/**
* @brief 将前端信息送进feature_buf
*
* @param[in] feature_msg
*/
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
if (!init_feature)
{
//skip the first detected feature, which doesn't contain optical flow speed
init_feature = 1;
return;
}
m_buf.lock();
// 将前端光流的追踪结果feature_msg送入feature_buf中
feature_buf.push(feature_msg);
m_buf.unlock();
con.notify_one();
}
1.5 重启时执行的回调函数restart_callback
vins_estimator/src/estimator_node.cpp
/**
* @brief 前端光流追踪失败时，需要重启: 清空buf, 重设优化器
*
* @param[in] restart_msg
*/
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
if (restart_msg->data == true)
{
ROS_WARN("restart the estimator!");
m_buf.lock();
while(!feature_buf.empty())
feature_buf.pop(); // 清空feature_buf
while(!imu_buf.empty())
imu_buf.pop(); // 清空imu_buf
m_buf.unlock();
m_estimator.lock();
// 重新设置优化器
estimator.clearState();
estimator.setParameter();
m_estimator.unlock();
current_time = -1;
last_imu_t = 0;
}
return;
}
2. process主线程
（1）process大致流程
- 在使用线程锁的情况下，使用getMeasurements进行视觉惯性对齐
- 获取对齐后的imu数据。其中，对于最后一个imu数据，需要根据时间戳通过插值将最后一个imu数据对齐到image上。然后再执行processIMU，主要是更新预积分量和滑窗中的状态量。目的是为了给滑窗中的非线性优化提供可信的初始值。
- 进行回环检测
- 获取对齐后的image中像素信息，然后执行视觉重投影约束的优化processImage
- 发送topic，用最新VIO结果最新imu对应的位姿update
（2）process代码实现
// vins_estimator/src/estimator_node.cpp
void process()
{
while (true)
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>,
sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
// 通过getMeasurements获取对齐后的图像和IMU数据
// con.wait: 等待lk锁的通知, 无论是IMU还是feature，在获得新数据时都要执行
// con.notify_one(); 等待通知 若return 返回true，则可以进行后续操作
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0; // 将视觉与IMU进行对齐
});
lk.unlock(); // 数据buffer的锁解锁，回调可以继续塞数据了
m_estimator.lock(); // 进行后端求解，不能和复位重启冲突
// 给予范围的for循环，这里是遍历每组对齐后的image imu组合,一组imu-image组合包含一个image和多个imu数据
for (auto &measurement : measurements)
{
auto img_msg = measurement.second; // 获取对齐的img
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
// 遍历这一组对齐imu-image组合中的所有imu (图像帧之前的imu数据 + 后一个imu数据)
// 目的是进行IMU的优化???
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec(); // imu时间戳
double img_t = img_msg->header.stamp.toSec() + estimator.td; // 图像时间戳
if (t <= img_t)
{
// 因为最后一个imu时间戳在图像帧时间戳的后面，所以此处表示的是最后一个imu之前的所有imu数据
if (current_time < 0)
current_time = t;
double dt = t - current_time; // 相邻imu数据之间的时间差
ROS_ASSERT(dt >= 0);
current_time = t;
// 取出当前imu的加速度(加速度计数据)
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
// 取出当前imu的角速度(陀螺仪数据)
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
// 时间差和imu数据送到后端的processIMU中(进行优化？)
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
}
// 针对最后一个imu数据，需要做一个简单的线性插值
else
{
// current_time: 上一个imu时间戳 img_t: 图像帧的时间戳 t: 当前imu时间戳
// 排列顺序：current_time -- img_t -- t
// 下面求出彼此之间的时间间隔，便于插值
double dt_1 = img_t - current_time;
double dt_2 = t - img_t;
current_time = img_t;
ROS_ASSERT(dt_1 >= 0);
ROS_ASSERT(dt_2 >= 0);
ROS_ASSERT(dt_1 + dt_2 > 0);
// 根据时间间隔计算插值权重
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
// 获取线性插值后的imu加速度
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
// 获取线性插值后的imu角速度
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
// 将dt_1时间差和插值后的imu数据送进去
estimator.processIMU(dt_1,
Vector3d(dx, dy, dz),
Vector3d(rx, ry, rz));
}
}
// 回环相关部分
// 提问: 回环检测部分为什么是在视觉重投影约束之前???
xxxxxx
TicToc t_s;
// 特征点id->特征点信息
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
// 对于前面对齐后得到的img，遍历上面的像素(进行视觉重投影约束的优化???)
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
int v = img_msg->channels[0].values[i] + 0.5; // 每个点的像素值
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
// 获取特征点去畸变后的归一化坐标
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
// 特征点像素坐标
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
// 特征点速度
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
ROS_ASSERT(z == 1); // 检查是不是归一化后的
// 组成7维的eigen
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
// 存入map结构
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);
}
}
}
2.1 视觉惯性对齐getMeasurements
（1）大致思路
主要是分情况将传入的imu与图像进行过滤，然后返回对齐后的imu和image，具体情况如下：
- 若imu或image只要有一个为空，则无法对齐，直接返回
- 若imu数据与image没有重叠部分，则无法对齐，直接返回
- 若image在imu数据之前开始，则应该丢掉前面的部分image帧
- 若imu数据在image之前开始，则获取在image之前的imu数据，以及在image之后的一个imu数据
（2）代码实现
// 获得匹配好的图像imu组 将视觉与IMU进行对齐
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>,
sensor_msgs::PointCloudConstPtr>> getMeasurements()
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>,
sensor_msgs::PointCloudConstPtr>> measurements;
while (true)
{
// 若imu或视觉feature只要有一个为空，则无法对齐，直接返回
// 这里的imu_buf和feature_buf是在前面imu_callback和feature_callback中获取的
if (imu_buf.empty() || feature_buf.empty())
return measurements;
// 第一个时间在左 最后一个时间在右
// imu *******
// image *****
// 希望最后一个imu数据的时间大于第一个视觉feature的时间+相应的延时时间estimator.td(括号中的)
// 这样可保证imu与视觉feature之间存在重叠部分，从而可以对齐
// 若取反，最后一个imu数据的时间 小于 第一个视觉feature的时间 + 相应的延时时间estimator.td
// 则表明imu数据与视觉feature没有重叠部分，自然就无法对齐
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->
header.stamp.toSec() + estimator.td))
{
//ROS_WARN("wait for imu, only should happen at the beginning");
sum_of_wait++; // 等待imu数据，使其与视觉feature存在重叠部分
return measurements;
}
/*
imu ****
image ******
希望第一个imu数据时间 小于 第一个视觉feature的时间 也就是要求imu先开始，然后再有视觉feature
若第一个imu数据时间 大于 第一个视觉feature的时间，则表明视觉feature在imu之前开始，此时应该丢掉前面的部分image帧
*/
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->
header.stamp.toSec() + estimator.td))
{
ROS_WARN("throw img, only should happen at the beginning");
// 由于在第一帧image之前没有imu数据，处理此时的image没有任何意义，故丢弃掉此时的第一帧image，再进行判断
feature_buf.pop();
continue;
}
// 此时就保证了图像前一定有imu数据
// imu ****************
// img * *
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front(); // 取出第一个图像帧
feature_buf.pop(); // 取出之后，在feature_buf队列中删除掉该第一帧
// 一般第一帧不会严格对齐，但是后面就都会对齐，当然第一帧也不会用到
std::vector<sensor_msgs::ImuConstPtr> IMUs;
// 从imu_buf中删除掉小于第一个图像帧 + 延时的imu数据，并将这些imu数据放到IMUs中
while (imu_buf.front()->header.stamp.toSec() < img_msg->
header.stamp.toSec() + estimator.td)
{
IMUs.emplace_back(imu_buf.front());
imu_buf.pop();
}
// 保留图像时间戳后一个imu数据，但不会从buffer中扔掉
// 也就是说，imu_buf中小于第一个图像帧 + 延时的imu数据都被删除掉了
// 而IMUs中不仅存入了这些删掉的imu数据，而且将第一个图像帧的后一个imu数据也存入进来
// 这样IMUs中最后两个imu数据就是第一个图像帧左右两边相邻的imu数据
// 后面取图像帧左右两边相邻imu数据，对其做插值，从而与图像帧进行对齐
// 最终将经过滤后的imu与image放到measurements中，格式大致为:
// imu * * * * * * *
// image *
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
// 取出此时对齐的imu和图像帧数据放到下面的measurements中, 传入外面的process()函数中进行对齐
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
2.2 IMU后端优化estimator.processIMU
主要是维持滑窗中的预积分量pre_integrations，为后续的优化提高状态初值。
vins_estimator/src/estimator.cpp
/**
* @brief 对imu数据进行处理，包括更新预积分量，和提供优化状态量的初始值
*
* @param[in] dt
* @param[in] linear_acceleration
* @param[in] angular_velocity
*/
void Estimator::processIMU(
double dt,
const Vector3d &linear_acceleration,
const Vector3d &angular_velocity)
{
// 若为第一个imu数据，则直接将imu测量值(加速度linear_acceleration和角速度angular_velocity)
// 赋给相应变量(acc_0和gyr_0)
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
// 判断滑窗中第frame_count帧的预积分量是否存在
// 若不存在，则new一个IntegrationBase，然后在下面调用push_back计算预积分量
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0,
Bas[frame_count], Bgs[frame_count]};
}
/*
滑窗中保留11帧，frame_count表示现在处理第几帧，一般处理到第11帧时就保持不变了(一直表示最后一帧)
在滑窗中维持11个预积分量，由于预积分是帧间约束，因此第1个预积分量实际上是用不到的，同时避免第一帧对应的imu数据量过大造成出错
*/
if (frame_count != 0)
{
// 调用push_back, 计算预积分量以及更新雅可比与协方差矩阵, 便于以imu的频率发布出去
pre_integrations[frame_count]->push_back(dt, linear_acceleration,
angular_velocity);
// tmp_pre_integration中备份此时的预积分量，后面用来做初始化
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
// 保存传感器数据
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration); // 线加速度
angular_velocity_buf[frame_count].push_back(angular_velocity); // 角加速度
// 又是一个中值积分，更新滑窗中状态量(Rs, Ps, Vs)，本质是给滑窗中的非线性优化提供可信的初始值
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
}
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
2.3 图像帧后端优化estimator.processImage
2.3.1 VINS中特征点的管理方法
如下图所示，对于当前帧的所有特征点，使用list<FeaturPerId>存储它们的id。对于每一个特征点，它具有的属性包括：
- 该特征点的id
- 第一个观测到该特征点的图像帧id（起始帧id）
- 该特征点在起始帧下可恢复的3D点的深度
- 该特征点在起始帧中的求解状态（是否可成功三角化恢复出3D点）
- 所有观测到该特征点的图像帧属性vector<FeaturePerFrame>，具体包括如下：
- 该特征点在被观测帧中的像素坐标
- 该特征点在被观测帧中的归一化相机坐标
- 该特征点在被观测帧中的速度
2.3.2 processImage的实现
（1）大致流程
- 通过addFeatureCheckParallax进行关键帧的判断，从而确定边缘化的标志marginalization_flag。如果上一帧(倒数第2帧)是关键帧，则滑窗中最老的帧就要被移出滑窗；否则移出上一帧(倒数第2帧)。
- 维护变量all_image_frame和tmp_pre_integration。其中，tmp_pre_integration为当前帧的预积分量，all_image_frame存储滑窗起始到当前的所有帧信息，它将图像帧与预积分绑定在一起，用于下一帧的初始化
- 在旋转外参标志ESTIMATE_EXTRINSIC为2的前提下，若当前帧不是第一帧，则通过getCorresponding获取当前帧与上一帧的特征点，然后在CalibrationExRotation中计算外参。
- 根据是否需要进行初始化，执行相应的状态优化操作：
-
- 若未初始化，则执行的操作如下：
- 基于图像的纯视觉单目SLAM初始化（SFM）
- 非线性优化求解VIO（solveOdometry）
- 更新滑动窗口（slideWindow）
- 移除无效地图点（removeFailures）
- 若已进行了初始化，则执行的操作如下：
- 非线性优化求解VIO（solveOdometry）
- 更新滑动窗口（slideWindow）
- 移除无效地图点（removeFailures）
- 若未初始化，则执行的操作如下：
（2）代码实现
void Estimator::processImage(const map<int,
vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image,
const std_msgs::Header &header)
{
// Step 1 将特征点信息加到f_manager这个特征点管理器中，同时进行是否关键帧的检查
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
// 如果上一帧(倒数第2帧)是关键帧，则滑窗中最老的帧就要被移出滑窗
marginalization_flag = MARGIN_OLD;
else
// 否则移除上一帧(倒数第2帧)
marginalization_flag = MARGIN_SECOND_NEW;
Headers[frame_count] = header;
// 根据对齐的当前帧image和时间戳定义ImageFrame对象
ImageFrame imageframe(image, header.stamp.toSec());
// 获取当前帧的预积分量 每一帧的预积分量tmp_pre_integration是在processIMU中计算得到的
imageframe.pre_integration = tmp_pre_integration;
// all_image_frame: 维护滑窗起始帧到当前帧之间的所有帧状态，用于后续初始化
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
// 初始化时：当前帧处理完之后，无论当前帧是否为关键帧，都认为它已经得到了两帧之间的预积分结果
// 那么需要将预积分复位，为下一帧做准备
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count],
Bgs[frame_count]};
// Step 2： 外参初始化
// 2表示没有任何外参的先验初值，需要进行初始化
// 1表示初始外参比较可靠，不需要通过初始化来计算旋转外参，只需要将初始外参值送给后端进行滑窗优化即可
// 0表示初始外参比较准确，在滑窗中固定住，不对其进行优化
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
/*
标定imu和相机的旋转外参的初值 因为预积分是相邻图像帧的约束，因此这里得到的图像关联也是相邻的
通过getCorresponding获取当前图像帧frame_count和上一图像帧frame_count-1观察到的特征点在各自图像帧下的坐标corres
*/
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(
frame_count - 1, frame_count);
Matrix3d calib_ric;
// 将预积分的delta_q和刚才求出的特征点在两相邻帧上的坐标corres作为参数
// 传入CalibrationExRotation函数中进行外参初始化，得到标定后的外参结果calib_ric
if (initial_ex_rotation.CalibrationExRotation(
corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ric[0] = calib_ric;
RIC[0] = calib_ric;
// 标志位设置成可信的外参初值
ESTIMATE_EXTRINSIC = 1;
}
}
}
// 基于图像的纯视觉单目SLAM ———— SFM
// solver_flag == INITIAL表示需要进行初始化 solver_flag = NON_LINEAR表示不需要进行初始化
if (solver_flag == INITIAL)
{
// 是否有足够的帧数，滑窗是否已满(后面在滑窗中，frame_count会一直等于滑窗大小)
if (frame_count == WINDOW_SIZE)
{
// 要有可信的旋转外参值，同时距离上次初始化不成功至少相邻0.1s
// Step 3： VIO初始化
if( ESTIMATE_EXTRINSIC!=2 && (header.stamp.toSec()-initial_timestamp) > 0.1)
{
result = initialStructure(); // 单目视觉SLAM的三维重建
initial_timestamp = header.stamp.toSec();
}
if(result)
{
// solver_flag = INITIAL表示处于初始化阶段
// solver_flag = NON_LINEAR表示初始化已完成
solver_flag = NON_LINEAR;
// Step 4： 非线性优化求解VIO
solveOdometry();
// Step 5： 更新滑动窗口
slideWindow();
// Step 6： 移除无效地图点
f_manager.removeFailures(); // 移除无效地图点
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE]; // 滑窗里最新的位姿
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0]; // 滑窗里最老的位姿
last_P0 = Ps[0];
}
else
// 更新滑动窗口
slideWindow();
}
else
frame_count++;
}
else // 不需要进行初始化的清空
{
TicToc t_solve;
solveOdometry();
ROS_DEBUG("solver costs: %fms", t_solve.toc());
// 检测VIO是否正常
if (failureDetection())
{
ROS_WARN("failure detection!");
failure_occur = 1;
// 如果异常，重启VIO
clearState();
setParameter();
ROS_WARN("system reboot!");
return;
}
TicToc t_margin;
slideWindow();
f_manager.removeFailures();
ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
// prepare output of VINS
// 给可视化用的
key_poses.clear();
for (int i = 0; i <= WINDOW_SIZE; i++)
key_poses.push_back(Ps[i]);
last_R = Rs[WINDOW_SIZE]; // 保存最新帧信息
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0]; // 保存最老帧信息
last_P0 = Ps[0];
}
}
2.3.2.1 关键帧判断addFeatureCheckParallax
（1）大致流程
- 遍历传入的特征点信息image，完善特征点属性
- 对遍历到的每个特征点，将它的xyz_uv_velocity信息封装到f_per_fra中
- 对遍历到的每个特征点，在特征点存放容器feature（list<FeaturePerId>类型）中查找是否已经存在这个特征点
- 若没有查找到，则表明遍历到的这个特征点是一个新特征点，则向特征点存放容器feature中添加这个特征点的一整套属性，即FeaturePerId属性与对应的FeaturePerFrame属性
- 若查找到的，则只需完善该特征点的FeaturePerFrame属性，然后添加到特征点存放容器feature中，同时更新成功追踪的特征点数目last_track_num
- 在完善传入的特征点属性之后， 进行关键帧判断
- 通过上一帧在滑窗中的索引frame_count和成功追踪的特征点数目，先提前判断倒数第二帧（上一帧）是否为关键帧
- 遍历feature中的所有特征点，对于每个特征点，在它能够被滑窗中倒数第二帧和倒数第三帧观测到的前提下，在compensatedParallax2中计算该特征点在滑窗中倒数第二帧与倒数第三帧中的视差，然后累加每个特征点的视差
- 若累积视差为0，则直接判定倒数第二帧为关键帧
- 否则，若平均视差大于视差阈值，则也认为倒数第二帧是关键帧
（2）addFeatureCheckParallax代码实现
// vins_estimator/src/feature_manager.cpp
/**
* @brief 增加特征点信息，同时检查上一帧是否时关键帧
*
* @param[in] frame_count
* @param[in] image
* @param[in] td
* @return true
* @return false
*/
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
double parallax_sum = 0;
int parallax_num = 0;
last_track_num = 0;
// 遍历每个特征点
for (auto &id_pts : image)
{
// 用遍历到的当前特征点信息(xyz_uv_velocity)以及时间戳td构造一个对象，即它在最新帧中的属性
// xyz: 归一化像素坐标 uv: 像素坐标系下的坐标 velocity: 特征点速度
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
// first: feature_id
// second: [camera_id0--xyz_uv_velocity0, ..., camera_idn--xyz_uv_velocityn]
// 前面的id_pts.second[0].second就表示xyz_uv_velocity0
int feature_id = id_pts.first; // 获取特征点的id
// 在已有的特征点id中寻找是否有相同的特征点
// feature表示存储当前帧中所有特征点的list容器
auto it = find_if(feature.begin(), feature.end(),
[feature_id](const FeaturePerId &it)
{
return it.feature_id == feature_id; // 若查找到了，则直接返回
});
// 若it为feature.end()，表示没有找到相同id的特征点，即这是一个新的特征点
// 因此，需要将这个新的特征点更新到特征点管理器中
if (it == feature.end())
{
// 根据FeaturePerId的构造函数，新创建一个特征点信息，然后将创建的对象push到feature中
// 这里的frame_count就是该特征点在滑窗中的当前位置，作为这个特征点的起始位置
// feature_id: 特征点id frame_count: 第一个观测到该特征点的图像帧，即起始帧
feature.push_back(FeaturePerId(feature_id, frame_count));
// 对于feature中的最后一个元素feature.back(), 也是刚才上一步push进去的特征点
// 添加观测到该特征点的所有图像帧的属性feature_per_frame
feature.back().feature_per_frame.push_back(f_per_fra);
}
// 如果这是一个已有的特征点，就在对应的“组织”下增加一个帧属性
else if (it->feature_id == feature_id)
{
// 在观测到当前特征点it的图像帧中添加对应的特征点属性f_per_fra
it->feature_per_frame.push_back(f_per_fra); // 增加帧属性
last_track_num++; // 更新追踪到上一帧的特征点数目
}
}
// 判断当前帧是否为KF: 前两帧全部设置为KF，追踪过少也认为是KF
// 若当前帧为前两帧(第0帧或第1帧)，则认为当前帧为关键帧
// 若追踪到上一帧的特征点数目小于20，表示当前帧的特征关联比较弱了，则认为当前帧为关键帧
if (frame_count < 2 || last_track_num < 20)
return true;
// 进行简单的视差计算: 判断倒数第2帧和倒数第3帧之间的视差是否足够大，以此来决定倒数第2帧是否为关键帧
for (auto &it_per_id : feature) // 遍历所有的特征点
{
/*
通过判断倒数第二帧(frame_count - 1)和倒数第三帧(frame_count - 2)的视差是否足够大，来确定倒数第二帧是否为关键帧。因此起始帧至少得是frame_count - 2(至少要从倒数第三帧开始被看到),同时至少覆盖到frame_count - 1帧(倒数第二帧)
*/
if (it_per_id.start_frame <= frame_count - 2 && it_per_id.start_frame +
int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
{
parallax_sum += compensatedParallax2(it_per_id, frame_count); // 计算视差
parallax_num++;
}
}
// 若累计视差为0，则表示倒数第二帧与倒数第三帧之间不存在特征关联，认为倒数第二帧为关键帧
if (parallax_num == 0)
{
return true;
}
else
{
// 看看平均视差是否超过所设阈值MIN_PARALLAX 总视差 / 总特征点数
// 若为true，表明视差较大，二者之间的特征关联较弱，也认为倒数第2帧为关键帧
return parallax_sum / parallax_num >= MIN_PARALLAX;
}
}
计算视差compensatedParallax2
- 大致思路
首先，获取该特征点在滑窗中倒数第二帧与倒数第三帧中的索引。然后，根据索引获取它在倒数第二帧与倒数第三帧中的归一化坐标。最后，根据归一化坐标，计算这两个归一化点之间的距离，即为视差。
- 实现代码
// vins_estimator/src/feature_manager.cpp
// it_per_id: 遍历到的当前特征点 frame_count: 当前帧索引
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
// 找到相邻两帧
// frame_count - 2 - it_per_id.start_frame表示倒数第3帧在it_per_id.feature_per_frame中的索引
// frame_count - 1 - it_per_id.start_frame表示倒数第2帧在it_per_id.feature_per_frame中的索引
const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame]; // 取出倒数第3帧
const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame]; // 取出倒数第2帧
double ans = 0;
Vector3d p_j = frame_j.point; // 倒数第2帧中的归一化坐标
double u_j = p_j(0);
double v_j = p_j(1);
Vector3d p_i = frame_i.point; // 倒数第3帧中的归一化坐标
Vector3d p_i_comp;
p_i_comp = p_i;
// 归一化操作，其实没必要，因为存的就是归一化值
double dep_i = p_i(2);
double u_i = p_i(0) / dep_i;
double v_i = p_i(1) / dep_i;
// 特征点在倒数第三帧和倒数第二帧中归一化点的坐标差
double du = u_i - u_j, dv = v_i - v_j;
// 当都是归一化坐标系时，他们两个都是一样的
double dep_i_comp = p_i_comp(2);
double u_i_comp = p_i_comp(0) / dep_i_comp;
double v_i_comp = p_i_comp(1) / dep_i_comp;
double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
// 当前特征点it_per_id在倒数第2帧和倒数第3帧归一化平面上的距离(视差)
ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));
return ans;
}
2.3.2.2 获取当前帧与倒数第二帧中的特征点getCorresponding
/**
* @brief 得到同时被frame_count_l frame_count_r帧看到的特征点在各自帧下的坐标
*
* @param[in] frame_count_l
* @param[in] frame_count_r
* @return vector<pair<Vector3d, Vector3d>>
*/
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
vector<pair<Vector3d, Vector3d>> corres;
for (auto &it : feature) // 遍历所有的特征点
{
// 保证需要的特征点被这两帧都观察到
if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
{
Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
// 获取两图像帧frame_count_l和frame_count_r在feature_per_frame（vector类型）中的索引
int idx_l = frame_count_l - it.start_frame;
int idx_r = frame_count_r - it.start_frame;
// 获取当前特征点it在第idx_l帧中去畸变后的归一化坐标
a = it.feature_per_frame[idx_l].point;
// 获取当前特征点it在第idx_r帧中去畸变后的归一化坐标
b = it.feature_per_frame[idx_r].point;
corres.push_back(make_pair(a, b)); // 返回该两帧下同时观测到的特征点的各自归一化坐标
}
}
return corres;
}
2.3.2.3 外参标定CalibrationExRotation
（1）大致思路
-
根据传入的两相邻图像帧之间的特征点，通过对极约束计算该两相邻图像帧之间的旋转
-
根据传入的预积分，获取两相邻图像帧对应的imu旋转
-
根据公式将两相邻图像帧对应的IMU之间的旋转转到图像帧上
-
构建求取外参方程的系数矩阵
-
使用SVD分解求取旋转外参，并根据特征值大小判断求解出的旋转外参是否有效（需要有足够大的置信度）
（2）求取外参的原理
在两关键帧之间，可通过IMU积分得到IMU旋转为 q b k b k + 1 q_{b_kb_{k+1}} qbkbk+1。通过光流追踪可以得到两相邻图像帧之间的特征点匹配，然后使用对极约束可以得到两相邻图像帧之间的位姿变换。其中，平移是带尺度的，但在标定旋转时不需要平移，故可假设旋转为 q c k c k + 1 q_{c_kc_{k+1}} qckck+1。
由于相机与IMU之间的旋转外参
q
c
b
q_{cb}
qcb在标定完成之后是不会变的，所以
q
c
b
=
q
c
k
b
k
=
q
c
k
+
1
b
k
+
1
q_{cb}=q_{c_kb_k} = q_{c_{k+1}b_{k+1}}
qcb=qckbk=qck+1bk+1，那么就有
q
c
b
⊗
q
b
k
b
k
+
1
=
q
c
k
b
k
⊗
q
b
k
b
k
+
1
=
q
c
k
b
k
+
1
q
c
k
c
k
+
1
⊗
q
c
b
=
q
c
k
c
k
+
1
⊗
q
c
k
+
1
b
k
+
1
=
q
c
k
b
k
+
1
⇒
q
c
b
⊗
q
b
k
b
k
+
1
=
q
c
k
c
k
+
1
⊗
q
c
b
q_{cb} \otimes q_{b_kb_{k+1}} = q_{c_kb_k} \otimes q_{b_kb_{k+1}} = q_{c_kb_{k+1}} \\ q_{c_kc_{k+1}} \otimes q_{cb} = q_{c_kc_{k+1}} \otimes q_{c_{k+1}b_{k+1}} = q_{c_kb_{k+1}} \\ \Rightarrow \ \ \ \ \ q_{cb} \otimes q_{b_kb_{k+1}} = q_{c_kc_{k+1}} \otimes q_{cb}
qcb⊗qbkbk+1=qckbk⊗qbkbk+1=qckbk+1qckck+1⊗qcb=qckck+1⊗qck+1bk+1=qckbk+1⇒ qcb⊗qbkbk+1=qckck+1⊗qcb
将四元数乘法转成矩阵乘法：
q
c
b
⊗
q
b
k
b
k
+
1
=
q
c
k
c
k
+
1
⊗
q
c
b
⇒
[
q
b
k
b
k
+
1
]
R
⋅
q
c
b
=
[
q
c
k
c
k
+
1
]
L
⋅
q
c
b
⇒
(
[
q
b
k
b
k
+
1
]
R
−
[
q
c
k
c
k
+
1
]
L
)
⋅
q
c
b
=
0
q_{cb} \otimes q_{b_kb_{k+1}} = q_{c_kc_{k+1}} \otimes q_{cb} \ \ \Rightarrow \ \ [q_{b_kb_{k+1}}]_R \cdot q_{cb} = [q_{c_kc_{k+1}}]_L \cdot q_{cb} \ \ \Rightarrow \ \ ([q_{b_kb_{k+1}}]_R - [q_{c_kc_{k+1}}]_L) \cdot q_{cb} = 0
qcb⊗qbkbk+1=qckck+1⊗qcb ⇒ [qbkbk+1]R⋅qcb=[qckck+1]L⋅qcb ⇒ ([qbkbk+1]R−[qckck+1]L)⋅qcb=0
对于多个帧而言，也满足上述方程：
(
[
q
b
k
b
k
+
1
]
R
−
[
q
c
k
c
k
+
1
]
L
)
⋅
q
c
b
=
0
(
[
q
b
k
+
1
b
k
+
2
]
R
−
[
q
c
k
+
1
c
k
+
2
]
L
)
⋅
q
c
b
=
0
⋮
(
[
q
b
k
+
N
b
k
+
N
+
1
]
R
−
[
q
c
k
+
N
c
k
+
N
+
1
]
L
)
⋅
q
c
b
=
0
([q_{b_kb_{k+1}}]_R - [q_{c_kc_{k+1}}]_L) \cdot q_{cb} = 0 \\ ([q_{b_{k+1}b_{k+2}}]_R - [q_{c_{k+1}c_{k+2}}]_L) \cdot q_{cb} = 0 \\ \vdots \\ ([q_{b_{k+N}b_{k+N+1}}]_R - [q_{c_{k+N}c_{k+N+1}}]_L) \cdot q_{cb} = 0
([qbkbk+1]R−[qckck+1]L)⋅qcb=0([qbk+1bk+2]R−[qck+1ck+2]L)⋅qcb=0⋮([qbk+Nbk+N+1]R−[qck+Nck+N+1]L)⋅qcb=0
写成矩阵的形式为：
[
[
q
b
k
b
k
+
1
]
R
−
[
q
c
k
c
k
+
1
]
L
[
q
b
k
+
1
b
k
+
2
]
R
−
[
q
c
k
+
1
c
k
+
2
]
L
⋮
[
q
b
k
+
N
b
k
+
N
+
1
]
R
−
[
q
c
k
+
N
c
k
+
N
+
1
]
L
]
(
4
N
+
4
)
×
4
⋅
[
q
c
b
]
4
×
1
=
0
\left[\begin{array}{cc} [q_{b_kb_{k+1}}]_R - [q_{c_kc_{k+1}}]_L \\ [q_{b_{k+1}b_{k+2}}]_R - [q_{c_{k+1}c_{k+2}}]_L \\ \vdots \\ [q_{b_{k+N}b_{k+N+1}}]_R - [q_{c_{k+N}c_{k+N+1}}]_L \end{array} \right]_{(4N+4) \times 4} \cdot [q_{cb}]_{4 \times 1} = 0
[qbkbk+1]R−[qckck+1]L[qbk+1bk+2]R−[qck+1ck+2]L⋮[qbk+Nbk+N+1]R−[qck+Nck+N+1]L
(4N+4)×4⋅[qcb]4×1=0
其中，每个时刻的IMU旋转和相机旋转可以通过计算得到，因此方程左侧的系数矩阵是已知的。
另外，对于系数矩阵而言， q b k b k + 1 q_{b_kb_{k+1}} qbkbk+1是在body坐标系下两相邻图像帧对应的imu之间的旋转， q c k c k + 1 q_{c_kc_{k+1}} qckck+1是在相机坐标系下两相邻图像帧之间的旋转，二者需要统一坐标系，代码中是将 q b k b k + 1 q_{b_kb_{k+1}} qbkbk+1统一到相机坐标系下。
（3）SVD求解原理 —— 方程 A x = 0 Ax=0 Ax=0的求解
通常将矩阵
A
A
A进行SVD奇异值分解，那么：
A
N
×
4
⋅
x
4
×
1
=
0
⇒
U
N
×
N
⋅
D
N
×
4
⋅
V
4
×
4
T
⋅
x
4
×
1
=
0
A_{N\times 4} \cdot x_{4 \times 1}=0 \ \ \ \ \ \Rightarrow \ \ \ \ \ U_{N \times N} \cdot D_{N \times 4} \cdot V^T_{4\times 4} \cdot x_{4 \times 1} = 0
AN×4⋅x4×1=0 ⇒ UN×N⋅DN×4⋅V4×4T⋅x4×1=0
其中，
U
U
U为矩阵
A
A
A的左奇异矩阵，矩阵
V
V
V为矩阵
A
A
A的右奇异矩阵，矩阵
D
D
D为由矩阵
A
A
A的特征值构成的对角矩阵，并且矩阵
U
U
U和
V
V
V都是正交矩阵。
在实际中，不可能满足 U D V T ⋅ x = 0 UDV^T \cdot x=0 UDVT⋅x=0，因此只能求 ∣ ∣ U D V T ⋅ x ∣ ∣ m i n ||UDV^T \cdot x||_{min} ∣∣UDVT⋅x∣∣min。
由于正交矩阵 R R R满足 ∣ ∣ R ⋅ x ∣ ∣ = x ||R \cdot x|| = x ∣∣R⋅x∣∣=x，因此 ∣ ∣ U D V T ⋅ x ∣ ∣ = ∣ ∣ D V T ⋅ x ∣ ∣ ||UDV^T \cdot x|| = ||DV^T \cdot x|| ∣∣UDVT⋅x∣∣=∣∣DVT⋅x∣∣。
令
V
T
⋅
x
=
y
V^T \cdot x = y
VT⋅x=y，由于
x
x
x是一个四元数，故
∣
∣
x
∣
∣
=
1
||x|| = 1
∣∣x∣∣=1，因此也有
∣
∣
y
∣
∣
=
1
||y||=1
∣∣y∣∣=1，则问题可转化为求
∣
∣
D
⋅
y
∣
∣
m
i
n
||D \cdot y||_{min}
∣∣D⋅y∣∣min，具体展开如下：
[
σ
0
σ
1
σ
2
σ
3
⋮
]
⋅
[
y
0
y
1
y
2
y
3
]
=
σ
0
⋅
y
0
+
σ
1
⋅
y
1
+
σ
2
⋅
y
2
+
σ
3
⋅
y
3
\left[\begin{array}{cc} \sigma_0 & & & \\ & \sigma_1 & & \\ & & \sigma_2 & \\ & & & \sigma_3 \\ & & & & \\ & & & & \\ & & & & \\ & & \vdots & \end{array} \right] \cdot \left[\begin{array}{cc} y_0 \\ y_1 \\ y_2 \\ y_3 \end{array} \right] = \sigma_0 \cdot y_0 + \sigma_1 \cdot y_1 + \sigma_2 \cdot y_2 + \sigma_3 \cdot y_3
σ0σ1σ2⋮σ3
⋅
y0y1y2y3
=σ0⋅y0+σ1⋅y1+σ2⋅y2+σ3⋅y3
因此，问题转化为求
m
i
n
∑
(
σ
0
⋅
y
0
+
σ
1
⋅
y
1
+
σ
2
⋅
y
2
+
σ
3
⋅
y
3
)
min\sum(\sigma_0 \cdot y_0 + \sigma_1 \cdot y_1 + \sigma_2 \cdot y_2 + \sigma_3 \cdot y_3)
min∑(σ0⋅y0+σ1⋅y1+σ2⋅y2+σ3⋅y3)。
由于构成矩阵 D D D的特征值大小满足： σ 0 > σ 1 > σ 2 > σ 3 \sigma_0 > \sigma_1 > \sigma_2 > \sigma_3 σ0>σ1>σ2>σ3，且 y y y需要满足 ∣ ∣ y ∣ ∣ = 1 ||y||=1 ∣∣y∣∣=1，因此只有当 y 0 = y 1 = y 2 = 0 y_0=y_1=y_2=0 y0=y1=y2=0时才可获得最小值。
进一步地，问题转化为：
V
T
⋅
x
=
[
0
0
0
1
]
V^T \cdot x = \left[\begin{array}{cc} 0 \\ 0 \\ 0 \\ 1 \end{array} \right]
VT⋅x=
0001
假设矩阵
V
=
[
v
0
,
v
1
,
v
2
,
v
3
]
V = [v_0, v_1, v_2, v_3]
V=[v0,v1,v2,v3]，由于矩阵
V
V
V为正交矩阵，两不同元素之间乘积为0，相同元素之间乘积为1，因此只有当所求变量
x
x
x为矩阵
V
V
V最后一个特征值对应的特征向量时，才满足上述等式。
（4）实现代码
// 标定imu和相机之间的旋转外参
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
// 根据特征关联(特征点在两相邻连续帧下的坐标)
// 利用对极约束solveRelativeR求解两个连续帧相机的旋转Rc ———— 2D-2D 对极约束
Rc.push_back(solveRelativeR(corres)); // 两相邻图像帧之间的旋转Rc
Rimu.push_back(delta_q_imu.toRotationMatrix()); // 预积分段的IMU旋转
// 通过外参把imu的旋转转移到相机坐标系
// q_{bc}^{-1} * q_{bkb{k+1}} * q_{bc} = q_{cb} * q_{bkb{k+1}} * q_{bc}
// 对于IMU与相机之间的外参q_{cb}，有：q_{cb} = q_{ckbk} = q_{c{k+1}b{k+1}} ==>
// q_{cb} * q_{bkb{k+1}} * q_{bc} = q_{ckbk} * q_{bkb{k+1}} * q_{b{k+1}c{k+1}} = q_{ckc{k+1}}
// 目的是通过上一次求解得到的外参 将IMU的旋转 转换到 相机的旋转，然后与通过对极约束求出的旋转进行对比
// 若差距较大，则在后续优化时将权重设置得小一点
Rc_g.push_back(ric.inverse() * delta_q_imu * ric); // ric是上一次求解得到的外参
// 定义推导出来的外参标定公式A * q_{cb} = 0中的系数矩阵A
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之间的旋转转到图像帧上得到的旋转
// 上面说的两个旋转之间的比较 相对姿态的绝对值delta_q，并从弧度转成角度delta_theta
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
// 一个简单的核函数 根据相差的角度设置相应的权重，相差的角度越大，则权重越小
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和q
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
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R); // 作用在残差上面
}
// 解方程A * q_{cb} = 0 SVD求解证明前面已推导
// SVD求解：Ax = 0 ==> UDV^Tx = 0 其中U=(N, N), D=(N, 4), V=(4, 4), x=(4, 1)
// 求解出来的x为V的最小特征值对应的特征向量，即V的最后一列
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
// 上面求解出来的x为4x1的Matrix形式，这里将其转换成四元数，即q_{cb}
Quaterniond estimated_R(x);
// 目的是得到旋转矩阵r_{bc}，因此先将四元数q_{cb}转换成矩阵r_{cb}，然后再进行转置得到r_{bc}
ric = estimated_R.toRotationMatrix().inverse();
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>(); // 取出最后3个奇异值
// 根据特征值大小来判断求出的旋转外参置信度是否足够大
// frame_count表示2D-2D的匹配点数，需要保证具有足够多的匹配点对，这样求解出的外参置信度才会高
// 因为旋转是3自由度，检查一下第三小的奇异值是否足够大，通常需要足够的运动激励才能保证得到没有奇异的解
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric; // 获取最终的旋转外参
return true;
}
else
return false;
}
对极约束solveRelativeR
（1）大致流程
- 若特征点对个数少于9，则直接返回单位阵
- 在特征点对个数不少于9的前提下，进行对极约束
-
- 获取输入的特征点对
- 调用opencv函数cv::findFundamentalMat计算本质矩阵E
- 使用函数decomposeE对本质矩阵E进行分解，并对分解出的R和t进行筛选
- 若|R| = -1，则重新对矩阵E进行分解
- 通过testTriangulation进行三角化筛选：三角化成功比例最高的R即为最佳的R
- 将求解出的R进行转换：由R21转成R12，并由cv::Mat格式转成eigen格式
（2）代码实现
// 通过对极约束，求解两图像帧之间的旋转R
Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
// 首先，要求至少有9对点
if (corres.size() >= 9)
{
vector<cv::Point2f> ll, rr;
// 获取输入的特征点对
for (int i = 0; i < int(corres.size()); i++)
{
// 相邻帧1上的点(u, v)
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
// 相邻帧2上的点(u, v)
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
// 由于ll和rr中存储的是特征点在对应图像帧中的归一化坐标，故根据它们使用opencv求解的是本质矩阵E
cv::Mat E = cv::findFundamentalMat(ll, rr);
cv::Mat_<double> R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2); // 根据本质矩阵E求解R和t
// 旋转矩阵的行列式应该是1,这里如果是-1就取一下反，然后重新分解
// 因为R1的行列式与R2的行列式相等，所以这里只考虑R1的行列式是否为-1的情况
if (determinant(R1) + 1.0 < 1e-09)
{
E = -E;
decomposeE(E, R1, R2, t1, t2); // 重新分解
}
// 通过三角化testTriangulation检查4组(R1-t1, R1-t2, R2-t1, R2-t2)结果R和t是否合理
double ratio1 = max(testTriangulation(ll, rr, R1, t1),
testTriangulation(ll, rr, R1, t2));
double ratio2 = max(testTriangulation(ll, rr, R2, t1),
testTriangulation(ll, rr, R2, t2));
// 取三角化成功比例最高的那个R(认为是合理的R)
cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;
// 这里本质矩阵E解算出来的R是R21，即第一帧到第二帧的旋转
// 而IMU解算得到的是R12，即第二帧到第一帧的旋转
// 因此，这里需要进行统一，将本质矩阵解算出来的R21转换成R12
Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
// 通过转置R21^T = R12 同时将opencv格式转成eigen，方便后续计算
ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;
}
return Matrix3d::Identity();
}
本质矩阵分解decomposeE
// 本质矩阵的分解: 视觉SLAM14讲P169 公式(7.15)
void InitialEXRotation::decomposeE(cv::Mat E,
cv::Mat_<double> &R1, cv::Mat_<double> &R2,
cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
cv::SVD svd(E, cv::SVD::MODIFY_A);
// W: R_Z^T(\pi/2) 将单位阵绕Z轴旋转\pi/2，然后进行转置
cv::Matx33d W(0, -1, 0,
1, 0, 0,
0, 0, 1);
// Wt: R_Z^T(\pi/2) 将单位阵绕Z轴旋转\pi/2
cv::Matx33d Wt(0, 1, 0,
-1, 0, 0,
0, 0, 1);
R1 = svd.u * cv::Mat(W) * svd.vt;
R2 = svd.u * cv::Mat(Wt) * svd.vt;
t1 = svd.u.col(2);
t2 = -svd.u.col(2);
}
测试三角化是否成功testTriangulation
（1）大致流程
- 根据输入的位姿R和t，定义两个相机的位姿（变换矩阵）
- 调用opencv函数cv::triangulatePoints，对输入的两个相机上的归一化点进行三角化，恢复出对应的3D点
- 遍历恢复出的每个3D点，将其转换到各自相机坐标系下，统计恢复出的有效3D点个数（只有转换到各自相机坐标系下的3D点深度均大于0才为有效）
- 返回恢复出的有效3D点比例
（2）实现代码文章来源地址https://uudwc.com/A/b13Yy
/**
* @brief 通过三角化来检查R t是否合理
*
* @param[in] l l相机的观测
* @param[in] r r相机的观测
* @param[in] R 旋转矩阵
* @param[in] t 位移
* @return double
*/
double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
const vector<cv::Point2f> &r,
cv::Mat_<double> R, cv::Mat_<double> t)
{
cv::Mat pointcloud;
// 其中一帧设置为单位阵
// 以第一帧为参考，则第一帧到第一帧本身的位姿为3x3的单位阵与三维零列向量构成的增广矩阵
cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
// 第二帧就设置为R t对应的位姿
// 第二帧到参考帧(第一帧)的位姿为本质矩阵解算出来的由R和t构成的增广矩阵
cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),
R(1, 0), R(1, 1), R(1, 2), t(1),
R(2, 0), R(2, 1), R(2, 2), t(2));
// 得到的pointcloud为三角化出来的3D点坐标
cv::triangulatePoints(P, P1, l, r, pointcloud);
int front_count = 0;
for (int i = 0; i < pointcloud.cols; i++)
{
double normal_factor = pointcloud.col(i).at<float>(3);
// 得到的3D点pointcloud是齐次的4维坐标，将其前三维除以第四维得到非齐次坐标xyz
// 同时，将其转换到各自相机坐标系下
cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
// 通过判断当前特征点三角化后在各自相机下的深度z是否大于0来确认是否三角化成功
if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
front_count++; // 记录成功三角化的特征点数
}
// 三角化成功的比例，比例越高，则表示分解出来的当前R和t的置信度越高
return 1.0 * front_count / pointcloud.cols;
}
