接著上一篇的介紹繼續

關于在使用readHeader函數讀取點云數據頭的類型的代碼（Read a point cloud data header from a PCD file.）

pcl::PCLPointCloud2 cloud;int version;Eigen::Vector4f origin;Eigen::Quaternionf orientation;pcl::PCDReader r;int type; unsigned int idx;//讀取PCD文件的頭的基本信息/*（in）path.string ()文件名cloud 點云數據集 origin傳感器的采集中心點 orientation傳感器的方位 version為PCD版本type數據類型（0 = ASCII，1 =二進制，2 =二進制壓縮）(in)idx為在文件內的偏移點云數據*/r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);

查看PCD文件里的內容（其實對于如何生成這種高緯度的文件呢？）

# .PCD v.7 - Point Cloud Data file format ? 注釋掉說明這是關于點云的文件格式 VERSION .7 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? PCD文件的版本號? ? ? ? ? ? ? ? FIELDS x y z rgb u v vx vy vz normal_x normal_y normal_z curvature ? 文件中點云的維度數 SIZE 4 4 4 4 4 4 4 4 4 4 4 4 4 ? ? ? ? 每個維度數據的大小 TYPE F F F F F F F F F F F F F ? ? ? ? 數據的類型 COUNT 1 1 1 1 1 1 1 1 1 1 1 1 1 ? ? WIDTH 3484 ? ? ? ? ? ? ? ? ? ? ? ? ? ? 點云大小 HEIGHT 1 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 無序點云 VIEWPOINT 0 0 0 1 0 0 0 ? ? ? ? ? ? ? ? 視點 POINTS 3484 ? ? ? ? ? ? ? ? ? ? ? ? ? ? 大小 DATA ascii ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?數據 -0.0042959 -0.041022 0.97549 7.3757e-39 275 261 0 0 0 -0.15142 0.63581 -0.75685 0.018435 -0.0031521 -0.040989 0.97472 7.0991e-39 276 261 0 0 0 -0.12262 0.63448 -0.76315 0.017282 -0.0042959 -0.03988 0.97549 5.9927e-39 299 261 0 0 0 -0.15385 0.62475 -0.76552 0.017079 -0.0020133 -0.03988 0.97549 5.3473e-39 347 261 0 0 0 -0.11114 0.62014 -0.77658 0.015706 -0.00087171 -0.039294 0.9751 5.6239e-39 277.5 261.5 0 0 0 -0.089597 0.61557 -0.78297 0.015285

另外一種PCD文件的比如VFH的PCD文件

# .PCD v.6 - Point Cloud Data file format FIELDS vfh SIZE 4 TYPE F COUNT 308 WIDTH 1 HEIGHT 1 POINTS 1 DATA ascii 0 0 0 0 0 0 0 0 0.086133 0.31582 ........................................................

?那么接下來我們就可以使用PCL給定的數據集，以及FLANN的庫是實現對點云的識別，方法是按照（1）的思路來做的

首先我們是假設已經有了數據集，以及相應每個數據集的VFH全局表述子的PCD文件，這樣我們就可以使用（1）中的思路把數據集訓練并保存到數中，方便之后我們再輸入給定的點云的VFH的PCD文件進行查找

那么其實這里面，我們如果是自己生成數據集，并對每個數據生成對應的VFH文件就會有點難度，畢竟這是對采集到的數據，對于一些無關點云需要剔除，

然后對有用的有價值的點云數據進行聚類，以及各個角度的點云聚類，然后對聚類的對象生成對應的VFH的特征PCD文件，這就是大致思路，

那么我們來看一下源代碼是如何讀取并訓練數據源的，并生成可用于FLANN使用的文件，并存在磁盤中

源代碼分析如下

#include <pcl/point_types.h> //點云的類型 #include <pcl/point_cloud.h> #include <pcl/console/parse.h> #include <pcl/console/print.h> #include <pcl/io/pcd_io.h> #include <boost/filesystem.hpp> #include <flann/flann.h> #include <flann/io/hdf5.h> #include <fstream> //pair的成員有兩個first second兩個，很明顯第一個vfh_model.first就是std::string //vfh_model.second就是存入的float的變量 typedef std::pair<std::string, std::vector<float> > vfh_model;//用于存儲VFH模型的容器/** \brief Loads an n-D histogram file as a VFH signature 以VFH作為特征直方圖* \param path the input file name 輸入的文件的名稱* \param vfh the resultant VFH model //VFH的模型*/ bool loadHist (const boost::filesystem::path &path, vfh_model &vfh) {int vfh_idx;// Load the file as a PCDtry{pcl::PCLPointCloud2 cloud;int version;Eigen::Vector4f origin; //中心float的向量Eigen::Quaternionf orientation; //方向 pcl::PCDReader r;int type; unsigned int idx;r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);vfh_idx = pcl::getFieldIndex (cloud, "vfh");if (vfh_idx == -1)return (false);if ((int)cloud.width * cloud.height != 1) //點的數目不為0return (false);}catch (const pcl::InvalidConversionException&) //拋出異常 {return (false);}// Treat the VFH signature as a single Point Cloud把相應的VFH特征代表單個點云pcl::PointCloud <pcl::VFHSignature308> point; //申明VFH 的點云pcl::io::loadPCDFile (path.string (), point); vfh.second.resize (308); //因為VFH有308個數據 std::vector <pcl::PCLPointField> fields;pcl::getFieldIndex (point, "vfh", fields);for (size_t i = 0; i < fields[vfh_idx].count; ++i){vfh.second[i] = point.points[0].histogram[i]; //每個點的直方圖 }vfh.first = path.string ();return (true); }/** \brief Load a set of VFH features that will act as the model (training data) //以VFH特征作為模型的訓練數據集* \param argc the number of arguments (pass from main ()) //輸入參數的個數* \param argv the actual command line arguments (pass from main ())* \param extension the file extension containing the VFH features 文件名的后綴* \param models the resultant vector of histogram models //特征模型的直方圖向量*/ void loadFeatureModels (const boost::filesystem::path &base_dir, const std::string &extension, std::vector<vfh_model> &models) {if (!boost::filesystem::exists (base_dir) && !boost::filesystem::is_directory (base_dir))return;for (boost::filesystem::directory_iterator it (base_dir); it != boost::filesystem::directory_iterator (); ++it) //對文件下每一個VFH的PCD文件計數 {if (boost::filesystem::is_directory (it->status ())){std::stringstream ss; //輸入ss << it->path ();pcl::console::print_highlight ("Loading %s (%lu models loaded so far).\n", ss.str ().c_str (), (unsigned long)models.size ());loadFeatureModels (it->path (), extension, models);}if (boost::filesystem::is_regular_file (it->status ()) && boost::filesystem::extension (it->path ()) == extension){vfh_model m;if (loadHist (base_dir / it->path ().filename (), m))models.push_back (m); //裝進容器中 }} }int main (int argc, char** argv) {if (argc < 2) //對輸入命令行的解析 {PCL_ERROR ("Need at least two parameters! Syntax is: %s [model_directory] [options]\n", argv[0]);return (-1);}std::string extension (".pcd");transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);std::string kdtree_idx_file_name = "kdtree.idx"; std::string training_data_h5_file_name = "training_data.h5";std::string training_data_list_file_name = "training_data.list";std::vector<vfh_model> models; //VFH的模型// Load the model histograms 載入模型的直方圖loadFeatureModels (argv[1], extension, models);pcl::console::print_highlight ("Loaded %d VFH models. Creating training data %s/%s.\n", (int)models.size (), training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());// Convert data into FLANN format 把數據轉為FLANN格式flann::Matrix<float> data (new float[models.size () * models[0].second.size ()], models.size (), models[0].second.size ());for (size_t i = 0; i < data.rows; ++i)for (size_t j = 0; j < data.cols; ++j)data[i][j] = models[i].second[j];// Save data to disk (list of models)保存數據集到本地flann::save_to_file (data, training_data_h5_file_name, "training_data");std::ofstream fs;fs.open (training_data_list_file_name.c_str ()); //打開訓練數據集的文件for (size_t i = 0; i < models.size (); ++i)fs << models[i].first << "\n";fs.close ();// Build the tree index and save it to disk 建立樹索引并保存pcl::console::print_error ("Building the kdtree index (%s) for %d elements...\n", kdtree_idx_file_name.c_str (), (int)data.rows);flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());//flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4)); index.buildIndex ();index.save (kdtree_idx_file_name);delete[] data.ptr ();return (0); }

這里面就很明顯的生成了兩個可用于FLANN進行搜索匹配的文件，以及模型的名稱的列表，就是會生成以下三個文件

kdtree.idx（這個是kdtree模型的索引）

training_data.h5（用于FLANN庫中的一種高效的文件格式，上一章有介紹）,

training_data.list（這是訓練數據集的列表）

（2）那么對于已經生成好的點云的數據集，我們就需要使用寫一個程序來實現給定一個點云的VFH的PCD文件來尋找這個點云所在位置并且是什么角度拍照的結果，閑話少說明，直接就上程序

#include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <pcl/common/common.h> #include <pcl/common/transforms.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/console/parse.h> #include <pcl/console/print.h> #include <pcl/io/pcd_io.h> #include <iostream> #include <flann/flann.h> #include <flann/io/hdf5.h> #include <boost/filesystem.hpp>typedef std::pair<std::string, std::vector<float> > vfh_model;/** \brief Loads an n-D histogram file as a VFH signature* \param path the input file name* \param vfh the resultant VFH model*/ bool loadHist (const boost::filesystem::path &path, vfh_model &vfh) {int vfh_idx;// Load the file as a PCDtry{pcl::PCLPointCloud2 cloud;int version;Eigen::Vector4f origin;Eigen::Quaternionf orientation;pcl::PCDReader r;int type; unsigned int idx;//讀取PCD文件的頭的基本信息/*（in）path.string ()文件名cloud 點云數據集 origin傳感器的采集中心點 orientation傳感器的方位 version為PCD版本type數據類型（0 = ASCII，1 =二進制，2 =二進制壓縮）(in)idx為在文件內的偏移點云數據*/r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);vfh_idx = pcl::getFieldIndex (cloud, "vfh");if (vfh_idx == -1)return (false);if ((int)cloud.width * cloud.height != 1)return (false);}catch (const pcl::InvalidConversionException&){return (false);}// Treat the VFH signature as a single Point Cloudpcl::PointCloud <pcl::VFHSignature308> point;pcl::io::loadPCDFile (path.string (), point);vfh.second.resize (308);std::vector <pcl::PCLPointField> fields;getFieldIndex (point, "vfh", fields);for (size_t i = 0; i < fields[vfh_idx].count; ++i){vfh.second[i] = point.points[0].histogram[i];}vfh.first = path.string ();return (true); }/** \brief Search for the closest k neighbors搜索最近的K鄰域* \param index the tree 樹 的索引* \param model the query model //給定的模型* \param k the number of neighbors to search for* \param indices the resultant neighbor indices* \param distances the resultant neighbor distances*/ inline void nearestKSearch (flann::Index<flann::ChiSquareDistance<float> > &index, const vfh_model &model, int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances) {// Query point 給定的點云flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));indices = flann::Matrix<int>(new int[k], 1, k);distances = flann::Matrix<float>(new float[k], 1, k);index.knnSearch (p, indices, distances, k, flann::SearchParams (512));delete[] p.ptr (); }/** \brief Load the list of file model names from an ASCII file 載入模型文件名* \param models the resultant list of model name* \param filename the input file name*/ bool loadFileList (std::vector<vfh_model> &models, const std::string &filename) {ifstream fs;fs.open (filename.c_str ());if (!fs.is_open () || fs.fail ())return (false);std::string line;while (!fs.eof ()){getline (fs, line);if (line.empty ())continue;vfh_model m;m.first = line;models.push_back (m);}fs.close ();return (true); }int main (int argc, char** argv) {int k = 6;double thresh = DBL_MAX; // No threshold, disabled by defaultif (argc < 2){pcl::console::print_error ("Need at least three parameters! Syntax is: %s <query_vfh_model.pcd> [options] {kdtree.idx} {training_data.h5} {training_data.list}\n", argv[0]);pcl::console::print_info (" where [options] are: -k = number of nearest neighbors to search for in the tree (default: "); pcl::console::print_value ("%d", k); pcl::console::print_info (")\n");pcl::console::print_info (" -thresh = maximum distance threshold for a model to be considered VALID (default: "); pcl::console::print_value ("%f", thresh); pcl::console::print_info (")\n\n");return (-1);}std::string extension (".pcd");transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);// Load the test histogram 載入測試的直方圖std::vector<int> pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");vfh_model histogram;if (!loadHist (argv[pcd_indices.at (0)], histogram)){pcl::console::print_error ("Cannot load test file %s\n", argv[pcd_indices.at (0)]);return (-1);}pcl::console::parse_argument (argc, argv, "-thresh", thresh);// Search for the k closest matches 設置K鄰域的個數pcl::console::parse_argument (argc, argv, "-k", k);pcl::console::print_highlight ("Using "); pcl::console::print_value ("%d", k); pcl::console::print_info (" nearest neighbors.\n");std::string kdtree_idx_file_name = "kdtree.idx";std::string training_data_h5_file_name = "training_data.h5";std::string training_data_list_file_name = "training_data.list";std::vector<vfh_model> models;flann::Matrix<int> k_indices; //索引flann::Matrix<float> k_distances; //距離flann::Matrix<float> data; // Check if the data has already been saved to diskif (!boost::filesystem::exists ("training_data.h5") || !boost::filesystem::exists ("training_data.list")){pcl::console::print_error ("Could not find training data models files %s and %s!\n", training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());return (-1);}else{loadFileList (models, training_data_list_file_name); //載入模型的文件名flann::load_from_file (data, training_data_h5_file_name, "training_data");pcl::console::print_highlight ("Training data found. Loaded %d VFH models from %s/%s.\n", (int)data.rows, training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());}// Check if the tree index has already been saved to diskif (!boost::filesystem::exists (kdtree_idx_file_name)){pcl::console::print_error ("Could not find kd-tree index in file %s!", kdtree_idx_file_name.c_str ());return (-1);}else{flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));index.buildIndex ();nearestKSearch (index, histogram, k, k_indices, k_distances); //搜索K鄰域 }// Output the results on screenpcl::console::print_highlight ("The closest %d neighbors for %s are:\n", k, argv[pcd_indices[0]]);for (int i = 0; i < k; ++i)pcl::console::print_info (" %d - %s (%d) with a distance of: %f\n", i, models.at (k_indices[0][i]).first.c_str (), k_indices[0][i], k_distances[0][i]);// Load the results可視化結果pcl::visualization::PCLVisualizer p (argc, argv, "VFH Cluster Classifier");int y_s = (int)floor (sqrt ((double)k));int x_s = y_s + (int)ceil ((k / (double)y_s) - y_s);double x_step = (double)(1 / (double)x_s);double y_step = (double)(1 / (double)y_s);pcl::console::print_highlight ("Preparing to load "); pcl::console::print_value ("%d", k); pcl::console::print_info (" files ("); pcl::console::print_value ("%d", x_s); pcl::console::print_info ("x"); pcl::console::print_value ("%d", y_s); pcl::console::print_info (" / ");pcl::console::print_value ("%f", x_step); pcl::console::print_info ("x"); pcl::console::print_value ("%f", y_step); pcl::console::print_info (")\n");int viewport = 0, l = 0, m = 0;for (int i = 0; i < k; ++i){std::string cloud_name = models.at (k_indices[0][i]).first;boost::replace_last (cloud_name, "_vfh", "");p.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);l++;if (l >= x_s){l = 0;m++;}pcl::PCLPointCloud2 cloud;pcl::console::print_highlight (stderr, "Loading "); pcl::console::print_value (stderr, "%s ", cloud_name.c_str ());if (pcl::io::loadPCDFile (cloud_name, cloud) == -1)break;// Convert from blob to PointCloudpcl::PointCloud<pcl::PointXYZ> cloud_xyz;pcl::fromPCLPointCloud2 (cloud, cloud_xyz);if (cloud_xyz.points.size () == 0)break;pcl::console::print_info ("[done, "); pcl::console::print_value ("%d", (int)cloud_xyz.points.size ()); pcl::console::print_info (" points]\n");pcl::console::print_info ("Available dimensions: "); pcl::console::print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());// Demean the cloud Eigen::Vector4f centroid;pcl::compute3DCentroid (cloud_xyz, centroid);//Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);// Add to renderer* p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);// Check if the model found is within our inlier tolerance std::stringstream ss;ss << k_distances[0][i];if (k_distances[0][i] > thresh){p.addText (ss.str (), 20, 30, 1, 0, 0, ss.str (), viewport); // display the text with red// Create a red line pcl::PointXYZ min_p, max_p;pcl::getMinMax3D (*cloud_xyz_demean, min_p, max_p);std::stringstream line_name;line_name << "line_" << i;p.addLine (min_p, max_p, 1, 0, 0, line_name.str (), viewport);p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, line_name.str (), viewport);}elsep.addText (ss.str (), 20, 30, 0, 1, 0, ss.str (), viewport);// Increase the font size for the score*p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, ss.str (), viewport);// Add the cluster namep.addText (cloud_name, 20, 10, cloud_name, viewport);}// Add coordianate systems to all viewportsp.addCoordinateSystem (0.1, "global", 0);p.spin ();return (0); }

這里面就涉及到FLANN的庫的函數的使用,執行的結果

打印的結果，這里面會顯示最接近的六個數據集，并且計算這六個最近點云與給定點云之間的“距離”，這也是衡量兩者之間的相似度的大小

可視化的結果

很明顯左下角就是我們給定的數據點云，而且運行查找的速度非常快～

好了就這樣了

?關注微信公眾號，歡迎大家的無私分享

總結

以上是生活随笔為你收集整理的PCL中使用FLANN库(2)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。