| 2866 | } |
| 2867 | |
| 2868 | void STDescManager::saveToFile(const std::string &save_path) { |
| 2869 | |
| 2870 | std::string desOutPath = save_path + "des.bin"; |
| 2871 | std::ofstream outputFile(desOutPath, std::ios::binary | std::ios::trunc); |
| 2872 | size_t mapSize = data_base_.size(); |
| 2873 | outputFile.write(reinterpret_cast<const char *>(&mapSize), sizeof(mapSize)); |
| 2874 | for (const auto &pair : data_base_) { |
| 2875 | outputFile.write(reinterpret_cast<const char *>(&pair.first), |
| 2876 | sizeof(STDesc_LOC)); |
| 2877 | size_t vectorSize = pair.second.size(); |
| 2878 | outputFile.write(reinterpret_cast<const char *>(&vectorSize), |
| 2879 | sizeof(vectorSize)); |
| 2880 | for (const auto &element : pair.second) { |
| 2881 | outputFile.write(reinterpret_cast<const char *>(&element), |
| 2882 | sizeof(STDesc)); |
| 2883 | } |
| 2884 | } |
| 2885 | // 关闭文件 |
| 2886 | outputFile.close(); |
| 2887 | |
| 2888 | std::string plane_cloud_path = save_path + "plane_clouds/"; |
| 2889 | std::stringstream ss; |
| 2890 | pcl::PointCloud<pcl::PointXYZINormal>::Ptr currPlaneCloud( |
| 2891 | new pcl::PointCloud<pcl::PointXYZINormal>()); |
| 2892 | // ROS_INFO("Current plane cloud size is: %d", |
| 2893 | // currPlaneCloud->points.size()); |
| 2894 | for (int i = 0; i < plane_cloud_vec_.size(); ++i) { |
| 2895 | ss << plane_cloud_path << std::setfill('0') << std::setw(6) << i << ".pcd"; |
| 2896 | currPlaneCloud = plane_cloud_vec_[i]; |
| 2897 | pcl::io::savePCDFileBinary<pcl::PointXYZINormal>(ss.str(), *currPlaneCloud); |
| 2898 | ss.clear(); |
| 2899 | ss.str(std::string()); |
| 2900 | currPlaneCloud->clear(); |
| 2901 | } |
| 2902 | } |
| 2903 | |
| 2904 | void STDescManager::loadExistingSTD(const std::string &save_path, |
| 2905 | int frame_num) { |