VINS-Mono代码学习记录（七）----processImage( )
一、重定位帧的处理
在process( )函数中，在处理完IMU之后，接着就是对重定位帧的设置，代码片段如下：
//[4] 设置重定位帧setReloFrame( )
sensor_msgs::PointCloudConstPtr relo_msg = NULL;//去看传感器数据的definition
while (!relo_buf.empty())
{
relo_msg = relo_buf.front();// 返回队首元素的值
relo_buf.pop();//删除队列首元素
}
if (relo_msg != NULL)
{
vector match_points;
double frame_stamp = relo_msg->header.stamp.toSec();
//遍历relo_msg中的points特征点
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
//[重定位帧的平移向量T的x,y,z，旋转四元数w,x,y,z和索引值]
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);//平移
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);//四元数表示的旋转
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];//索引
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
relo_buf用来接收relo_msg，从relo_msg中取出点push到match_points,再取出帧的时间戳、帧的id、旋转以及平移量传递给估计器的setReloFrame( )函数。
void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
//[1]将传入的各参数进行赋值给重定位相关的变量
relo_frame_stamp = _frame_stamp;
relo_frame_index = _frame_index;
match_points.clear();
match_points = _match_points;
prev_relo_t = _relo_t;
prev_relo_r = _relo_r;
//【2】遍历滑动窗口，将当前传入的重定位帧的时间戳和滑动窗口中的进行对比
for(int i = 0; i < WINDOW_SIZE; i++)
{
if(relo_frame_stamp == Headers[i].stamp.toSec())
{
relo_frame_local_index = i;//记录滑窗中是那一帧重定位
relocalization_info = 1;//时间戳相等,重定位标志置1
for (int j = 0; j < SIZE_POSE; j++)//SIZE_POSE=7
relo_Pose[j] = para_Pose[i][j];// 把两个匹配的帧的位置和旋转四元数存储起来
}
}
}
二、processImage（ ）函数
先来看张图：
代码部分为：
void Estimator::processImage(const map>>> &image, const std_msgs::Header &header)
{
//image的数据类型分别表示feature_id,camera_id,点的x,y,z坐标，u,v坐标，在x,y方向上的跟踪速度
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
//[1]根据视差来决定marg掉哪一帧，如果次新帧是关键帧，marg掉最老帧，如果次新帧不是关键帧,marg掉次新帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))//添加特征并检测视差
marginalization_flag = MARGIN_OLD;//MARFIN_OLD=0
else
marginalization_flag = MARGIN_SECOND_NEW;//MARGIN_SECOND_NEW=1
ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
ROS_DEBUG("Solving %d", frame_count);
ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());//计算滑窗内被track过的特征点的数量
Headers[frame_count] = header;
//【2】将图像数据、时间、临时预积分值存储到图像帧类中,ImageFrame这个类的定义在initial_alignment.h中
ImageFrame imageframe(image, header.stamp.toSec());
imageframe.pre_integration = tmp_pre_integration;
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]};
//[3]如果ESTIMATE_EXTRINSIC == 2表示需要在线估计imu和camera之间的外参数
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
vector> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
//[4]判断是初始化还是非线性优化
if (solver_flag == INITIAL)//初始化
{
if (frame_count == WINDOW_SIZE)
{
bool result = false;
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
result = initialStructure();
initial_timestamp = header.stamp.toSec();
}
if(result)
{
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
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());
if (failureDetection())
{
ROS_WARN("failure detection!");
failure_occur = 1;
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];
}
}
三、bool FeatureManager::addFeatureCheckParallax（ ）函数
在processImage( )函数中，又调用了很多别的函数：按顺序来看一下吧，但是有些自己也没读明白。
这个addFeatureCheckParallax（ ）函数的功能就是将传入的特征点加入feature这个vector中，并且根据视差判断当前帧是不是关键帧来决定marg掉最老帧还是次新帧。
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map>>> &image, double td)
{
//image数据类型解释：feature_id,camera_id，p.x,p.y,p.z,u,v,u_velocity,v_velocity
ROS_DEBUG("input feature: %d", (int)image.size());
ROS_DEBUG("num of feature: %d", getFeatureCount());//计算滑窗内被track过的特征点的数量
double parallax_sum = 0;//视差总和
int parallax_num = 0;//满足条件的特征点数量
last_track_num = 0;//被跟踪点的个数
//[1]遍历图像image中所有的特征点，和已经记录了特征点的容器feature中进行比较
for (auto &id_pts : image)
{
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);//7*1 x,y,z,u,v,u的速度，v的速度
int feature_id = id_pts.first;//获取特征点的id
//在feature中查找该feature_id的feature是否存在
auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
{
return it.feature_id == feature_id;
});
if (it == feature.end())//这里的feature的数据类型: list feature;
{
//没有找到该feature的id，则把特征点放入feature的list容器中
feature.push_back(FeaturePerId(feature_id, frame_count));
feature.back().feature_per_frame.push_back(f_per_fra);//list链表.back()表示返回最后一个元素
}
else if (it->feature_id == feature_id)
{
/**
* 如果找到了相同ID特征点，就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息，
* it的feature_per_frame容器中存放的是该feature能够被哪些帧看到，存放的是在这些帧中该特征点的信息
* 所以，feature_per_frame.size的大小就表示有多少个帧可以看到该特征点
* */
it->feature_per_frame.push_back(f_per_fra);
last_track_num++;//特征点被跟踪的次数+1
}
}
if (frame_count < 2 || last_track_num < 20)//如果上一次track的点少于20 就添加点 第一个frame_count表示滑窗内的帧数
return true;//marg最老帧
//[2]遍历每一个feature，计算能被当前帧和其前两帧共同看到的特征点视差
for (auto &it_per_id : feature)
{
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)
{
//计算特征点it_per_id在倒数第二帧和倒数第三帧之间的视差，并求所有视差的累加和
parallax_sum += compensatedParallax2(it_per_id, frame_count);//开始补偿视差啦！
parallax_num++;//满足条件的特征点数量+1
}
}
if (parallax_num == 0)
{
return true;//marg最老帧
}
else
{
//视差总和除以参与计算视差的特征点个数，表示每个特征点的平均视差值
ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
return parallax_sum / parallax_num >= MIN_PARALLAX;
//当平均视差大于等于最小视差，返回true，marg最老帧，当平均视差小于最小视差,marg掉次新帧
}
}
当平均视差小于最小视差，我想大概表示的是新来的帧和前两帧挨得很近，图像的信息很相似，所以可以去掉相邻的次新帧。
在addFeatureCheckParallax（ ）这个函数中，又继续调用了compensatedParallax2（ ）来计算视差。
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
//传入第一个参数为滑窗内的每一个feature，第二参数表示当前帧的帧数
//check the second last frame is keyframe or not刺新帧是否为关键帧
//parallax betwwen seconde last frame and third last frame
const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];//右边指啥到底？
const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];
double ans = 0;
Vector3d p_j = frame_j.point;
double u_j = p_j(0);
double v_j = p_j(1);
//这里为啥没有先归一化?
Vector3d p_i = frame_i.point;
Vector3d p_i_comp;
//int r_i = frame_count - 2;
//int r_j = frame_count - 1;
//p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
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;//这跟下面de_comp dv_comp不是一样的吗？
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;
ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));
return ans;
}
没太看明白。
processImage( )中接下来的处理判断是否需要进行imu和camera之间的外参数估计，这一部分主要的内容在initial文件夹里,下次再更。。。要去看演唱会啦啦，回来再更吧！
演唱会回来了。。后遗症持续中，无心学习。。。
代码：
//[3]如果ESTIMATE_EXTRINSIC == 2表示需要在线估计imu和camera之间的外参数
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
vector> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
上段代码中涉及两个函数f_manager.getCorresponding( )和initial_ex_rotation.CalibrationExRotation( ),
getCorresponding( )提供vector
啊，理论知识借用深蓝学院的两张ppt来介绍：
svd分解出的V矩阵的最后一列即为Qbc的解。
bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
Rc.push_back(solveRelativeR(corres));//求解两帧之间的相对位姿
Rimu.push_back(delta_q_imu.toRotationMatrix());
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//求出来的是相机上一帧到下一帧的旋转，此表达式由公式推导得出，理论上跟Rc相等
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]);
double angular_distance = 180 / M_PI * r1.angularDistance(r2);//求角度误差
ROS_DEBUG(
"%d %f", i, angular_distance);
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;//求核函数值
++sum_ok;
Matrix4d L, R;
double w = Quaterniond(Rc[i]).w();//取四元数的实部！！相机的旋转取左L
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]);//imu的旋转取右R
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w;
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);//加上huber权重
}
JacobiSVD svd(A, ComputeFullU | ComputeFullV);//svd分解
Matrix x = svd.matrixV().col(3);
Quaterniond estimated_R(x);//上面估计的R为imu到相机的旋转
ric = estimated_R.toRotationMatrix().inverse();
//cout << svd.singularValues().transpose() << endl;
//cout << ric << endl;
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}
其中的solveRelativeR( )求解两帧图像之间的旋转，注释如下…
Matrix3d InitialEXRotation::solveRelativeR(const vector> &corres)//求两帧之间的旋转及平移
{
//如果点对数大于等于9，利用对极约束来求解相对运动
if (corres.size() >= 9)
{
//[1]构建cv::Point2f类型的数据
vector ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));//取点的x,y坐标push进vector中
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
//【2】求解出本质矩阵并分解求出旋转和平移
cv::Mat E = cv::findFundamentalMat(ll, rr);
cv::Mat_ R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);
//[3]加强条件判断
if (determinant(R1) + 1.0 < 1e-09)
{
E = -E;
decomposeE(E, R1, R2, t1, t2);
}
//[4]选出更合适的旋转矩阵
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));
cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2;
//【5】为啥要这样做？取转置
Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;
}
//如果不满足条件，返回单位矩阵
return Matrix3d::Identity();
}