c++解析讀取pcd點雲檔案
阿新 • • 發佈:2018-12-04
1.pcd檔案格式
2.C++原始碼
讀取思路:連續讀取前11行,獲得pcd檔案資訊頭,取得點雲端儲存方式(ascii或者binary)和點雲數量、點雲格式(XYZ,XYZI....)等關鍵資訊,然後按行依次讀取座標資料即可。
#include <iostream> #include <fstream> #include <string> #include <iomanip> #include <vector> struct PointXYZ { double x, y, z; }; /* Author:chd_ayj Date:2018-6-8 Description: read .PCD file */ //讀取pcd點雲檔案 void readPCDfile(const std::string finname, std::vector<PointXYZ>& points, const std::string foutname) { std::ifstream fin(finname); if (fin.bad()){ std::cout << "開啟檔案失敗!" << std::endl; return; } char s[11][1024]; //儲存Header int Points_Num; //點數量 std::string data_columns_type; //列數: X Y Z std::string data_type; //點座標儲存方式(ascii或者binary) std::vector<PointXYZ> cloud; //連續讀取Header行數 std::cout << "start to read file header....." << std::endl; std::cout << "file header: " << std::endl; for (int i = 0; i < 11; ++i){ fin.getline(s[i], 1024); //std::cout << "第" << i + 1 << "行:" << s[i] << std::endl; std::cout << s[i] << std::endl; //FIELDS x y z rgb if (i == 2){ std::string s2 = s[2]; size_t pos = s2.find("FIELDS"); size_t size = s2.size(); data_columns_type = s2.substr(pos + 7, size); //std::cout << "data_columns_type:" << data_columns_type << std::endl; } //POINTS xxx if (i == 9){ std::string s9 = s[9], Points_Str; size_t pos = s9.find("POINTS"); size_t size = s9.size(); Points_Str = s9.substr(pos + 7, size); Points_Num = atoi(Points_Str.c_str()); //std::cout << "Points:" << std::Points_Num << endl; } //DATA ascii或者binary if (i == 10){ std::string s10 = s[10], DATA_SIZE; size_t pos = s10.find("DATA"); size_t size = s10.size(); data_type = s10.substr(pos + 5, size); //std::cout << "data_type:" << data_type << std::endl; } } std::cout << std::endl; std::cout << "start to read point ....." << std::endl; PointXYZ p; if ((data_columns_type == "x y z") && (data_type == "ascii")){ //讀取點座標記錄 while (!fin.eof()){ fin >> p.x >> p.y >> p.z; if (fin.peek() == EOF){ break; } cloud.push_back(p); } }else{ std::cout << "data_type = binary, read failed!" << std::endl; } //////////////////////////////////////////////////////////// //點座標輸出txt文字 std::cout << "start to write point to txt....." << std::endl; std::ofstream out(foutname); for (size_t i = 0; i < points.size(); ++i) { out << std::setiosflags(std::ios::fixed) << std::setprecision(7) << points.at(i).x << " " << points.at(i).y << " " << points.at(i).z << std::endl; } std::cout << "write point to txt finished!" << std::endl; } int main() { std::string finname = "ascii_pcd.pcd"; std::string foutame = "ascii_pcd.txt"; std::vector<PointXYZ> points; readPCDfile(finname, points, foutame); system("pause"); return 0; }
2.執行結果:
3.總結
- string中的find()函式,功能:查詢第一次出現的目標字串,如果存在則返回查詢到的第一個字元的索引,否則返回-1。
- string中的substr(pos, n)函式,功能:返回一個string,包含pos位置開始的n個字元的拷貝(pos的預設值是0,n的預設值是s.size() - pos,即不加引數會預設拷貝整個s)。
- string s.c_str()函式,功能:把string轉化成const char*,返回一個指向正規C字串的指標常量。
- int atoi(const char *nptr),功能:字串轉換成整型數。