void
pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
{
boost::shared_ptr < std::vector<ModelT> > models;
if(search_model_.compare("") == 0) {
models = source_->getModels ();
} else {
models = source_->getModels (search_model_);
//reset cache and flann structures
if(flann_index_ != 0)
delete flann_index_;
flann_models_.clear();
poses_cache_.clear();
keypoints_cache_.clear();
}
std::cout << "Models size:" << models->size () << std::endl;
if (force_retrain)
{
for (size_t i = 0; i < models->size (); i++)
{
source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
}
}
for (size_t i = 0; i < models->size (); i++)
{
std::cout << models->at (i).class_ << " " << models->at (i).id_ << std::endl;
if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
{
for (size_t v = 0; v < models->at (i).views_->size (); v++)
{
PointInTPtr processed (new pcl::PointCloud<PointInT>);
typename pcl::PointCloud<FeatureT>::Ptr signatures (new pcl::PointCloud<FeatureT> ());
PointInTPtr keypoints_pointcloud;
bool success = estimator_->estimate (models->at (i).views_->at (v), processed, keypoints_pointcloud, signatures);
if (success)
{
std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
bf::path desc_dir = path;
if (!bf::exists (desc_dir))
bf::create_directory (desc_dir);
std::stringstream path_view;
path_view << path << "/view_" << v << ".pcd";
pcl::io::savePCDFileBinary (path_view.str (), *processed);
std::stringstream path_pose;
path_pose << path << "/pose_" << v << ".txt";
PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
if(v < models->at (i).self_occlusions_->size()) {
std::stringstream path_entropy;
path_entropy << path << "/entropy_" << v << ".txt";
PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
}
//save keypoints and signatures to disk
std::stringstream keypoints_sstr;
keypoints_sstr << path << "/keypoint_indices_" << v << ".pcd";
/*
* boost::shared_ptr < std::vector<int> > indices (new std::vector<int> ());
indices->resize (keypoints.points.size ());
for (size_t kk = 0; kk < indices->size (); kk++)
(*indices)[kk] = keypoints.points[kk];
typename pcl::PointCloud<PointInT> keypoints_pointcloud;
pcl::copyPointCloud (*processed, *indices, keypoints_pointcloud);
*/
pcl::io::savePCDFileBinary (keypoints_sstr.str (), *keypoints_pointcloud);
std::stringstream path_descriptor;
path_descriptor << path << "/descriptor_" << v << ".pcd";
pcl::io::savePCDFileBinary (path_descriptor.str (), *signatures);
}
}
} else {
std::cout << "Model already trained..." << std::endl;
//there is no need to keep the views in memory once the model has been trained
models->at (i).views_->clear();
}
}
loadFeaturesAndCreateFLANN ();
}
bool
LocalRecognitionPipeline<PointT>::initialize (bool force_retrain)
{
if(!estimator_)
throw std::runtime_error("Keypoint extractor with feature estimator is not set!");
descr_name_ = estimator_->getFeatureDescriptorName();
std::vector<ModelTPtr> models = source_->getModels();
std::cout << "Models size:" << models.size () << std::endl;
if (force_retrain)
{
for (size_t i = 0; i < models.size (); i++)
source_->removeDescDirectory (*models[i], models_dir_, descr_name_);
}
for (ModelTPtr &m : models)
{
const std::string dir = models_dir_ + "/" + m->class_ + "/" + m->id_ + "/" + descr_name_;
if (!io::existsFolder(dir))
{
std::cout << "Model " << m->class_ << " " << m->id_ << " not trained. Training " << estimator_->getFeatureDescriptorName() << " on " << m->view_filenames_.size () << " views..." << std::endl;
if(!source_->getLoadIntoMemory())
source_->loadInMemorySpecificModel(*m);
for (size_t v = 0; v < m->view_filenames_.size(); v++)
{
std::vector<std::vector<float> > all_signatures;
std::vector<std::vector<float> > object_signatures;
typename pcl::PointCloud<PointT> all_keypoints;
typename pcl::PointCloud<PointT> object_keypoints;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
computeNormals<PointT>(m->views_[v], normals, param_.normal_computation_method_);
std::vector<int> all_kp_indices, obj_kp_indices;
estimator_->setNormals(normals);
typename pcl::PointCloud<PointT> foo;
bool success = estimator_->estimate (*m->views_[v], foo, all_keypoints, all_signatures);
(void) success;
estimator_->getKeypointIndices(all_kp_indices);
// remove signatures and keypoints which do not belong to object
std::vector<bool> obj_mask = createMaskFromIndices(m->indices_[v].indices, m->views_[v]->points.size());
obj_kp_indices.resize( all_kp_indices.size() );
object_signatures.resize( all_kp_indices.size() ) ;
size_t kept=0;
for (size_t kp_id = 0; kp_id < all_kp_indices.size(); kp_id++)
{
int idx = all_kp_indices[kp_id];
if ( obj_mask[idx] )
{
obj_kp_indices[kept] = idx;
object_signatures[kept] = all_signatures[kp_id];
kept++;
}
}
object_signatures.resize( kept );
obj_kp_indices.resize( kept );
pcl::copyPointCloud( *m->views_[v], obj_kp_indices, object_keypoints);
if (object_keypoints.points.size()) //save descriptors and keypoints to disk
{
io::createDirIfNotExist(dir);
std::string descriptor_basename (m->view_filenames_[v]);
boost::replace_last(descriptor_basename, source_->getViewPrefix(), "/descriptors_");
boost::replace_last(descriptor_basename, ".pcd", ".desc");
std::ofstream f(dir + descriptor_basename, std::ofstream::binary );
int rows = object_signatures.size();
int cols = object_signatures[0].size();
f.write((const char*)(&rows), sizeof(rows));
f.write((const char*)(&cols), sizeof(cols));
for(size_t sig_id=0; sig_id<object_signatures.size(); sig_id++)
f.write(reinterpret_cast<const char*>(&object_signatures[sig_id][0]), sizeof(object_signatures[sig_id][0]) * object_signatures[sig_id].size());
f.close();
std::string keypoint_basename (m->view_filenames_[v]);
boost::replace_last(keypoint_basename, source_->getViewPrefix(), "/keypoints_");
pcl::io::savePCDFileBinary (dir + keypoint_basename, object_keypoints);
std::string kp_normals_basename (m->view_filenames_[v]);
boost::replace_last(kp_normals_basename, source_->getViewPrefix(), "/keypoint_normals_");
pcl::PointCloud<pcl::Normal>::Ptr normals_keypoints(new pcl::PointCloud<pcl::Normal>);
pcl::copyPointCloud(*normals, obj_kp_indices, *normals_keypoints);
pcl::io::savePCDFileBinary (dir + kp_normals_basename, *normals_keypoints);
}
}
if(!source_->getLoadIntoMemory())
m->views_.clear();
}
else
{
std::cout << "Model " << m->class_ << " " << m->id_ << " already trained using " << estimator_->getFeatureDescriptorName() << "." << std::endl;
m->views_.clear(); //there is no need to keep the views in memory once the model has been trained
}
}
loadFeaturesAndCreateFLANN ();
if(param_.icp_iterations_ > 0 && param_.icp_type_ == 1)
source_->createVoxelGridAndDistanceTransform(param_.voxel_size_icp_);
return true;
}
void
pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
{
//use the source to know what has to be trained and what not, checking if the descr_name directory exists
//unless force_retrain is true, then train everything
boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
std::cout << "Models size:" << models->size () << std::endl;
if (force_retrain)
{
for (size_t i = 0; i < models->size (); i++)
{
source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
}
}
for (size_t i = 0; i < models->size (); i++)
{
if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
{
for (size_t v = 0; v < models->at (i).views_->size (); v++)
{
PointInTPtr processed (new pcl::PointCloud<PointInT>);
PointInTPtr view = models->at (i).views_->at (v);
if (view->points.size () == 0)
PCL_WARN("View has no points!!!\n");
if (noisify_)
{
double noise_std = noise_;
boost::posix_time::ptime time = boost::posix_time::microsec_clock::local_time();
boost::posix_time::time_duration duration( time.time_of_day() );
boost::mt19937 rng;
rng.seed (static_cast<unsigned int> (duration.total_milliseconds()));
boost::normal_distribution<> nd (0.0, noise_std);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
// Noisify each point in the dataset
for (size_t cp = 0; cp < view->points.size (); ++cp)
view->points[cp].z += static_cast<double> (var_nor ());
}
//pro view, compute signatures
std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
std::vector < Eigen::Vector3d > centroids;
micvfh_estimator_->estimate (view, processed, signatures, centroids);
std::vector<bool> valid_trans;
std::vector < Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > transforms;
micvfh_estimator_->getValidTransformsVec (valid_trans);
micvfh_estimator_->getTransformsVec (transforms);
std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
bf::path desc_dir = path;
if (!bf::exists (desc_dir))
bf::create_directory (desc_dir);
std::stringstream path_view;
path_view << path << "/view_" << v << ".pcd";
pcl::io::savePCDFileBinary (path_view.str (), *processed);
std::stringstream path_pose;
path_pose << path << "/pose_" << v << ".txt";
PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
std::stringstream path_entropy;
path_entropy << path << "/entropy_" << v << ".txt";
PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
//save signatures and centroids to disk
for (size_t j = 0; j < signatures.size (); j++)
{
if (valid_trans[j])
{
std::stringstream path_centroid;
path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
Eigen::Vector3d centroid (centroids[j][0], centroids[j][1], centroids[j][2]);
PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid);
std::stringstream path_descriptor;
path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]);
//save roll transform
std::stringstream path_pose;
path_pose << path << "/roll_trans_" << v << "_" << j << ".txt";
PersistenceUtils::writeMatrixToFile (path_pose.str (), transforms[j]);
}
}
}
}
else
{
//else skip model
std::cout << "The model has already been trained..." << std::endl;
//there is no need to keep the views in memory once the model has been trained
models->at (i).views_->clear ();
}
}
//load features from disk
//initialize FLANN structure
loadFeaturesAndCreateFLANN ();
}