VINS-Mono+Fusion源码解析系列（十一）：后端processIMU与processImage
发布时间
阅读量：
阅读量
1. 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. 图像帧后端优化estimator.processImage
2.1 VINS中特征点的管理方法
如下图所示，对于当前帧的所有特征点，使用list
- 该特征点的id
- 第一个观测到该特征点的图像帧id（起始帧id）
- 该特征点在起始帧下可恢复的3D点的深度
- 该特征点在起始帧中的求解状态（是否可成功三角化恢复出3D点）
- 所有观测到该特征点的图像帧属性vector
，具体包括如下： - 该特征点在被观测帧中的像素坐标
- 该特征点在被观测帧中的归一化相机坐标
- 该特征点在被观测帧中的速度
2.2 processImage的实现
（1）大致流程
- 通过addFeatureCheckParallax进行关键帧的判断，从而确定边缘化的标志marginalization_flag。如果上一帧(倒数第2帧)是关键帧，则滑窗中最老的帧就要被移出滑窗；否则移出上一帧(倒数第2帧)。
- 维护变量all_image_frame和tmp_pre_integration。其中，tmp_pre_integration为前面在processIMU中计算出的当前帧预积分量，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 关键帧判断addFeatureCheckParallax
（1）大致流程
-
遍历传入的特征点信息image，完善特征点属性 * 对遍历到的每个特征点，将它的xyz_uv_velocity信息封装到f_per_fra中
- 对遍历到的每个特征点，在特征点存放容器feature（list
类型）中查找是否已经存在这个特征点 - 若没有查找到，则表明遍历到的这个特征点是一个新特征点，则向特征点存放容器feature中添加这个特征点的一整套属性，即FeaturePerId属性与对应的FeaturePerFrame属性
- 若查找到的，则只需完善该特征点的FeaturePerFrame属性，然后添加到特征点存放容器feature中，同时更新成功追踪的特征点数目last_track_num
- 对遍历到的每个特征点，在特征点存放容器feature（list
-
在完善传入的特征点属性之后， 进行关键帧判断 * 通过上一帧在滑窗中的索引frame_count和成功追踪的特征点数目，先提前判断倒数第二帧（上一帧）是否为关键帧
- 遍历feature中的所有特征点，对于每个特征点，在它能够被滑窗中倒数第二帧和倒数第三帧观测到的前提下，在compensatedParallax2中计算该特征点在滑窗中倒数第二帧与倒数第三帧中的视差，然后累加每个特征点的视差
- 若累积视差为0，则直接判定倒数第二帧为关键帧
- 否则，若平均视差大于视差阈值，则也认为倒数第二帧是关键帧
- 遍历feature中的所有特征点，对于每个特征点，在它能够被滑窗中倒数第二帧和倒数第三帧观测到的前提下，在compensatedParallax2中计算该特征点在滑窗中倒数第二帧与倒数第三帧中的视差，然后累加每个特征点的视差
（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;
}获取当前帧与倒数第二帧中的特征点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.4 外参标定CalibrationExRotation
（1）大致思路
根据传入的两相邻图像帧之间的特征点，通过对极约束 计算该两相邻图像帧之间的旋转
根据传入的预积分，获取两相邻图像帧对应的imu旋转
根据公式将两相邻图像帧对应的IMU之间的旋转转到图像帧上
构建求取外参方程的系数矩阵
使用SVD分解求取旋转外参，并根据特征值大小判断求解出的旋转外参是否有效（需要有足够大的置信度）
（2）求取外参的原理
在两关键帧之间，可通过IMU积分得到IMU旋转为q_{b_kb_{k+1}}。通过光流追踪可以得到两相邻图像帧之间的特征点匹配，然后使用对极约束可以得到两相邻图像帧之间的位姿变换。其中，平移是带尺度的，但在标定旋转时不需要平移，故可假设旋转为q_{c_kc_{k+1}}。
由于相机与IMU之间的旋转外参q_{cb}在标定完成之后是不会变的，所以q_{cb}=q_{c_kb_k} = q_{c_{k+1}b_{k+1}}，那么就有
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}
将四元数乘法转成矩阵乘法：
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
对于多个帧而言，也满足上述方程：
([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
写成矩阵的形式为：
\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
其中，每个时刻的IMU旋转和相机旋转可以通过计算得到，因此方程左侧的系数矩阵是已知的。
另外，对于系数矩阵而言，q_{b_kb_{k+1}}是在body坐标系下两相邻图像帧对应的imu之间的旋转，q_{c_kc_{k+1}}是在相机坐标系下两相邻图像帧之间的旋转，二者需要统一坐标系，代码中是将q_{b_kb_{k+1}}统一到相机坐标系下。
（3）SVD求解原理 —— 方程Ax=0的求解
通常将矩阵A进行SVD奇异值分解，那么：
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
其中，U为矩阵A的左奇异矩阵，矩阵V为矩阵A的右奇异矩阵，矩阵D为由矩阵A的特征值构成的对角矩阵，并且矩阵U和V都是正交矩阵。
在实际中，不可能满足UDV^T \cdot x=0，因此只能求||UDV^T \cdot x||_{min}。
由于正交矩阵R满足||R \cdot x|| = x，因此||UDV^T \cdot x|| = ||DV^T \cdot x||。
令V^T \cdot x = y，由于x是一个四元数，故||x|| = 1，因此也有||y||=1，则问题可转化为求||D \cdot y||_{min}，具体展开如下：
\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
因此，问题转化为求min\sum(\sigma_0 \cdot y_0 + \sigma_1 \cdot y_1 + \sigma_2 \cdot y_2 + \sigma_3 \cdot y_3)。
由于构成矩阵D的特征值大小满足：\sigma_0 > \sigma_1 > \sigma_2 > \sigma_3，且y需要满足||y||=1，因此只有当y_0=y_1=y_2=0时才可获得最小值。
进一步地，问题转化为：
V^T \cdot x = \left[\begin{array}{cc} 0 \\ 0 \\ 0 \\ 1 \end{array} \right]
假设矩阵V = [v_0, v_1, v_2, v_3]，由于矩阵V为正交矩阵，两不同元素之间乘积为0，相同元素之间乘积为1，因此只有当所求变量x为矩阵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）实现代码
/** * @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;
}全部评论 (0)
还没有任何评论哟~