| 695 | |
| 696 | |
| 697 | void PoseGraph::savePoseGraph() |
| 698 | { |
| 699 | m_keyframelist.lock(); |
| 700 | TicToc tmp_t; |
| 701 | FILE *pFile; |
| 702 | printf("pose graph path: %s\n",POSE_GRAPH_SAVE_PATH.c_str()); |
| 703 | printf("pose graph saving... \n"); |
| 704 | string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt"; |
| 705 | pFile = fopen (file_path.c_str(),"w"); |
| 706 | //fprintf(pFile, "index time_stamp Tx Ty Tz Qw Qx Qy Qz loop_index loop_info\n"); |
| 707 | list<KeyFrame*>::iterator it; |
| 708 | for (it = keyframelist.begin(); it != keyframelist.end(); it++) |
| 709 | { |
| 710 | std::string image_path, descriptor_path, brief_path, keypoints_path; |
| 711 | if (DEBUG_IMAGE) |
| 712 | { |
| 713 | image_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_image.png"; |
| 714 | imwrite(image_path.c_str(), (*it)->image); |
| 715 | } |
| 716 | Quaterniond VIO_tmp_Q{(*it)->vio_R_w_i}; |
| 717 | Quaterniond PG_tmp_Q{(*it)->R_w_i}; |
| 718 | Vector3d VIO_tmp_T = (*it)->vio_T_w_i; |
| 719 | Vector3d PG_tmp_T = (*it)->T_w_i; |
| 720 | |
| 721 | fprintf (pFile, " %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f %f %d\n",(*it)->index, (*it)->time_stamp, |
| 722 | VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(), |
| 723 | PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(), |
| 724 | VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(), |
| 725 | PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(), |
| 726 | (*it)->loop_index, |
| 727 | (*it)->loop_info(0), (*it)->loop_info(1), (*it)->loop_info(2), (*it)->loop_info(3), |
| 728 | (*it)->loop_info(4), (*it)->loop_info(5), (*it)->loop_info(6), (*it)->loop_info(7), |
| 729 | (int)(*it)->keypoints.size()); |
| 730 | |
| 731 | // write keypoints, brief_descriptors vector<cv::KeyPoint> keypoints vector<BRIEF::bitset> brief_descriptors; |
| 732 | assert((*it)->keypoints.size() == (*it)->brief_descriptors.size()); |
| 733 | brief_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_briefdes.dat"; |
| 734 | std::ofstream brief_file(brief_path, std::ios::binary); |
| 735 | keypoints_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_keypoints.txt"; |
| 736 | FILE *keypoints_file; |
| 737 | keypoints_file = fopen(keypoints_path.c_str(), "w"); |
| 738 | for (int i = 0; i < (int)(*it)->keypoints.size(); i++) |
| 739 | { |
| 740 | brief_file << (*it)->brief_descriptors[i] << endl; |
| 741 | fprintf(keypoints_file, "%f %f %f %f\n", (*it)->keypoints[i].pt.x, (*it)->keypoints[i].pt.y, |
| 742 | (*it)->keypoints_norm[i].pt.x, (*it)->keypoints_norm[i].pt.y); |
| 743 | } |
| 744 | brief_file.close(); |
| 745 | fclose(keypoints_file); |
| 746 | } |
| 747 | fclose(pFile); |
| 748 | |
| 749 | printf("save pose graph time: %f s\n", tmp_t.toc() / 1000); |
| 750 | m_keyframelist.unlock(); |
| 751 | } |
| 752 | void PoseGraph::loadPoseGraph() |
| 753 | { |
| 754 | TicToc tmp_t; |