| 211 | |
| 212 | |
| 213 | void PoseGraph::loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) |
| 214 | { |
| 215 | cur_kf->index = global_index; |
| 216 | global_index++; |
| 217 | int loop_index = -1; |
| 218 | if (flag_detect_loop) |
| 219 | loop_index = detectLoop(cur_kf, cur_kf->index); |
| 220 | else |
| 221 | { |
| 222 | addKeyFrameIntoVoc(cur_kf); |
| 223 | } |
| 224 | if (loop_index != -1) |
| 225 | { |
| 226 | printf(" %d detect loop with %d \n", cur_kf->index, loop_index); |
| 227 | KeyFrame* old_kf = getKeyFrame(loop_index); |
| 228 | if (cur_kf->findConnection(old_kf)) |
| 229 | { |
| 230 | if (earliest_loop_index > loop_index || earliest_loop_index == -1) |
| 231 | earliest_loop_index = loop_index; |
| 232 | m_optimize_buf.lock(); |
| 233 | optimize_buf.push(cur_kf->index); |
| 234 | m_optimize_buf.unlock(); |
| 235 | } |
| 236 | } |
| 237 | m_keyframelist.lock(); |
| 238 | Vector3d P; |
| 239 | Matrix3d R; |
| 240 | cur_kf->getPose(P, R); |
| 241 | Quaterniond Q{R}; |
| 242 | geometry_msgs::PoseStamped pose_stamped; |
| 243 | pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp); |
| 244 | pose_stamped.header.frame_id = "world"; |
| 245 | pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; |
| 246 | pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; |
| 247 | pose_stamped.pose.position.z = P.z(); |
| 248 | pose_stamped.pose.orientation.x = Q.x(); |
| 249 | pose_stamped.pose.orientation.y = Q.y(); |
| 250 | pose_stamped.pose.orientation.z = Q.z(); |
| 251 | pose_stamped.pose.orientation.w = Q.w(); |
| 252 | base_path.poses.push_back(pose_stamped); |
| 253 | base_path.header = pose_stamped.header; |
| 254 | |
| 255 | //draw local connection |
| 256 | if (SHOW_S_EDGE) |
| 257 | { |
| 258 | list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin(); |
| 259 | for (int i = 0; i < 1; i++) |
| 260 | { |
| 261 | if (rit == keyframelist.rend()) |
| 262 | break; |
| 263 | Vector3d conncected_P; |
| 264 | Matrix3d connected_R; |
| 265 | if((*rit)->sequence == cur_kf->sequence) |
| 266 | { |
| 267 | (*rit)->getPose(conncected_P, connected_R); |
| 268 | posegraph_visualization->add_edge(P, conncected_P); |
| 269 | } |
| 270 | rit++; |