1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
# Cluster Recognition and 6DOF Pose Estimation using VFH descriptors
# http://pointclouds.org/documentation/tutorials/vfh_recognition.php#vfh-recognition
#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>
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 PCD
try
{
pcl::PCLPointCloud2 cloud;
int version;
Eigen::Vector4f origin;
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)
return (false);
}
catch (const pcl::InvalidConversionException&)
{
return (false);
}
// Treat the VFH signature as a single Point Cloud
pcl::PointCloud <pcl::VFHSignature308> point;
pcl::io::loadPCDFile (path.string (), point);
vfh.second.resize (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)
* \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)
{
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;
// 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::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);
}
|
