回環檢測
VINS回環檢測與全局優化都在pose_graph.cpp內處理。首先在pose_graph_node加載vocabulary文件給BriefDatabase用，如果要加載地圖，會loadPoseGraph, 它會讀取一些列文件，然后加載所有的Keyframe。同時在經過一系列回調函數得到建立新的Keyframe所用的數據之后，構造Keyframe，且在其內重新提取更多的特征點並計算描述子，然后pose_graph調用addKeyframe。loadKeyframe 和addKeyframe 都會用到flag_detect_loop這個標志，如果是加載則為0，如果是新建則為1. 新建的Keyframe 需要調用detectLoop函數，得出具有最小index的歷史關鍵幀。
//為了展示更核心的內容，我把DEBUG_IMAGE部分都刪去了
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
TicToc tmp_t;
//first query; then add this frame into database!
QueryResults ret;
TicToc t_query;
// 第一個參數是描述子，第二個是檢測結果，第三個是結果個數，第四個是結果幀號必須小於此
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//printf("query time: %f", t_query.toc());
//cout << "Searching for Image " << frame_index << ". " << ret << endl;
TicToc t_add;
db.add(keyframe->brief_descriptors);
//printf("add feature time: %f", t_add.toc());
// ret[0] is the nearest neighbour's score. threshold change with neighour score
bool find_loop = false;
cv::Mat loop_result;
// a good match with its nerghbour，
if (ret.size() >= 1 &&ret[0].Score > 0.05)
for (unsigned int i = 1; i < ret.size(); i++)
{
if (ret[i].Score > 0.015)
{
find_loop = true;
int tmp_index = ret[i].Id;
}
}
//找到最小幀號的匹配幀
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
min_index = ret[i].Id;
}
return min_index;
}
else
return -1;
}
得到匹配上關鍵幀后，經過計算相對位姿，並把當前幀號記錄到全局優化內
if (loop_index != -1)
{
//printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
KeyFrame* old_kf = getKeyFrame(loop_index);
// findConnection 是為了計算相對位姿，最主要的就是利用了PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old)函數，
//並且它負責把匹配好的點發送到estimator節點中去
if (cur_kf->findConnection(old_kf))
{
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
old_kf->getVioPose(w_P_old, w_R_old);
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
Vector3d relative_t;
Quaterniond relative_q;
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
// 根據old frame 和相對位姿能計算出當前幀位姿，也就能得出和已知當前幀位姿的差別
//分別計算出shift_r, shift_t，用來更新其他幀位姿
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
// shift vio pose of whole sequence to the world frame
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
w_r_vio = shift_r;
w_t_vio = shift_t;
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
{
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
m_keyframelist.lock();
Vector3d P;
Matrix3d R;
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
cur_kf->updatePose(P, R);
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;
//發送path主題數據，用以顯示
keyframelist.push_back(cur_kf);
publish();
m_keyframelist.unlock();
全局優化
VINS論文里有一句：因為他們的設備能保證roll和pitch是一直可觀的，所以只需要優化x,y,z和yaw這幾個有漂移的參數。在vins_estimator文件的visualization.cpp內我們看到：
void pubKeyframe(const Estimator &estimator)
{
// pub camera pose, 2D-3D points of keyframe
//當一個舊幀被邊緣化后，才被發送到pose-graph內
//pose_graph收到vio主題后，僅僅是為了顯示用，真正構造Keyframe的是邊緣化的幀的數據。
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR && estimator.marginalization_flag == 0)
{
int i = WINDOW_SIZE - 2;
//Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0];
Vector3d P = estimator.Ps[i];
Quaterniond R = Quaterniond(estimator.Rs[i]);
nav_msgs::Odometry odometry;
odometry.header = estimator.Headers[WINDOW_SIZE - 2];
odometry.header.frame_id = "world";
odometry.pose.pose.position.x = P.x();
odometry.pose.pose.position.y = P.y();
odometry.pose.pose.position.z = P.z();
odometry.pose.pose.orientation.x = R.x();
odometry.pose.pose.orientation.y = R.y();
odometry.pose.pose.orientation.z = R.z();
odometry.pose.pose.orientation.w = R.w();
//printf("time: %f t: %f %f %f r: %f %f %f %f\n", odometry.header.stamp.toSec(), P.x(), P.y(), P.z(), R.w(), R.x(), R.y(), R.z());
pub_keyframe_pose.publish(odometry);
//被邊緣化的幀的特征點也被發送到pose_graph內
sensor_msgs::PointCloud point_cloud;
point_cloud.header = estimator.Headers[WINDOW_SIZE - 2];
for (auto &it_per_id : estimator.f_manager.feature)
{
int frame_size = it_per_id.feature_per_frame.size();
if(it_per_id.start_frame < WINDOW_SIZE - 2 && it_per_id.start_frame + frame_size - 1 >= WINDOW_SIZE - 2 && it_per_id.solve_flag == 1)
{
int imu_i = it_per_id.start_frame;
Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;
Vector3d w_pts_i = estimator.Rs[imu_i] * (estimator.ric[0] * pts_i + estimator.tic[0])
+ estimator.Ps[imu_i];
geometry_msgs::Point32 p;
p.x = w_pts_i(0);
p.y = w_pts_i(1);
p.z = w_pts_i(2);
point_cloud.points.push_back(p);
int imu_j = WINDOW_SIZE - 2 - it_per_id.start_frame;
sensor_msgs::ChannelFloat32 p_2d;
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.x());
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].point.y());
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.x());
p_2d.values.push_back(it_per_id.feature_per_frame[imu_j].uv.y());
p_2d.values.push_back(it_per_id.feature_id);
point_cloud.channels.push_back(p_2d);
}
}
pub_keyframe_point.publish(point_cloud);
}
}
全局優化函數
void PoseGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3];
Quaterniond q_array[max_length];
double euler_array[max_length][3];
double sequence_array[max_length];
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
list<KeyFrame*>::iterator it;
int i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
//回環檢測到幀以前的都略過
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i] = tmp_q;
Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
//回環檢測到的幀參數設為固定
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge
//對於每個i, 只計算它之前五個的位置和yaw殘差
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
relative_t = q_array[i-j].inverse() * relative_t;
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]);
}
}
//add loop edge
// 如果有檢測到回環
if((*it)->has_loop)
{
//必須回環檢測的幀號大於或者等於當前幀的回環檢測匹配幀號
assert((*it)->loop_index >= first_looped_index);
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
double relative_yaw = (*it)->getLoopRelativeYaw();
ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index],
t_array[connected_index],
euler_array[i],
t_array[i]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
m_keyframelist.lock();
i = 0;
//根據優化后的參數更新參與優化的關鍵幀的位姿
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q;
tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index)
break;
i++;
}
//根據當前幀的drift，更新全部關鍵幀位姿
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
it++;
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
(*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
}