| 750 | m_keyframelist.unlock(); |
| 751 | } |
| 752 | void PoseGraph::loadPoseGraph() |
| 753 | { |
| 754 | TicToc tmp_t; |
| 755 | FILE * pFile; |
| 756 | string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt"; |
| 757 | printf("lode pose graph from: %s \n", file_path.c_str()); |
| 758 | printf("pose graph loading...\n"); |
| 759 | pFile = fopen (file_path.c_str(),"r"); |
| 760 | if (pFile == NULL) |
| 761 | { |
| 762 | printf("lode previous pose graph error: wrong previous pose graph path or no previous pose graph \n the system will start with new pose graph \n"); |
| 763 | return; |
| 764 | } |
| 765 | int index; |
| 766 | double time_stamp; |
| 767 | double VIO_Tx, VIO_Ty, VIO_Tz; |
| 768 | double PG_Tx, PG_Ty, PG_Tz; |
| 769 | double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz; |
| 770 | double PG_Qw, PG_Qx, PG_Qy, PG_Qz; |
| 771 | double loop_info_0, loop_info_1, loop_info_2, loop_info_3; |
| 772 | double loop_info_4, loop_info_5, loop_info_6, loop_info_7; |
| 773 | int loop_index; |
| 774 | int keypoints_num; |
| 775 | Eigen::Matrix<double, 8, 1 > loop_info; |
| 776 | int cnt = 0; |
| 777 | while (fscanf(pFile,"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d", &index, &time_stamp, |
| 778 | &VIO_Tx, &VIO_Ty, &VIO_Tz, |
| 779 | &PG_Tx, &PG_Ty, &PG_Tz, |
| 780 | &VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz, |
| 781 | &PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz, |
| 782 | &loop_index, |
| 783 | &loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3, |
| 784 | &loop_info_4, &loop_info_5, &loop_info_6, &loop_info_7, |
| 785 | &keypoints_num) != EOF) |
| 786 | { |
| 787 | /* |
| 788 | printf("I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\n", index, time_stamp, |
| 789 | VIO_Tx, VIO_Ty, VIO_Tz, |
| 790 | PG_Tx, PG_Ty, PG_Tz, |
| 791 | VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz, |
| 792 | PG_Qw, PG_Qx, PG_Qy, PG_Qz, |
| 793 | loop_index, |
| 794 | loop_info_0, loop_info_1, loop_info_2, loop_info_3, |
| 795 | loop_info_4, loop_info_5, loop_info_6, loop_info_7, |
| 796 | keypoints_num); |
| 797 | */ |
| 798 | cv::Mat image; |
| 799 | std::string image_path, descriptor_path; |
| 800 | if (DEBUG_IMAGE) |
| 801 | { |
| 802 | image_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_image.png"; |
| 803 | image = cv::imread(image_path.c_str(), 0); |
| 804 | } |
| 805 | |
| 806 | Vector3d VIO_T(VIO_Tx, VIO_Ty, VIO_Tz); |
| 807 | Vector3d PG_T(PG_Tx, PG_Ty, PG_Tz); |
| 808 | Quaterniond VIO_Q; |
| 809 | VIO_Q.w() = VIO_Qw; |