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 (); }