VINS 回環檢測與全域性優化

回環檢測

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)
}//找到最小幀號的匹配幀
if (find_loop && frame_index > 50)
return min_index;
}else
return -1;
}

得到匹配上關鍵幀後，經過計算相對位姿，並把當前幀號記錄到全域性優化內

if (loop_index != -1)
}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;
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_keyframe_point.publish(point_cloud);
}}

全域性優化函式

void posegraph::optimize4dof()
m_optimize_buf.unlock();
if (cur_index != -1)
//add edge
//對於每個i, 只計算它之前五個的位置和yaw殘差
for (int j = 1; j < 5; j++)
}//add loop edge
// 如果有檢測到回環
if((*it)->has_loop)
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++)
//根據當前幀的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++)
m_keyframelist.unlock();
updatepath();
}std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}}

VINS 回環檢測與全域性優化

回環檢測之理解一二三

ORB SLAM（六）回環檢測

ORB SLAM（六）回環檢測

VINS 回環檢測與全域性優化

回環檢測之理解一二三

ORB SLAM（六）回環檢測

ORB SLAM（六）回環檢測

相關推薦