void
init(v4r::GlobalNNClassifier<flann::L1, PointT> &esf_classifier) {
boost::shared_ptr<v4r::MeshSource<PointT> > mesh_source (new v4r::MeshSource<PointT>);
mesh_source->setMeshDir(MESH_DIR_);
mesh_source->setPath (MODELS_DIR_);
mesh_source->setResolution (150);
mesh_source->setTesselationLevel (0);
// mesh_source->setViewAngle (57.f);
mesh_source->setRadiusSphere (3.f);
mesh_source->setModelScale (1.f);
mesh_source->setTesselationLevel(1);
mesh_source->generate ();
boost::shared_ptr<v4r::Source<PointT> > source;
source = boost::static_pointer_cast<v4r::MeshSource<PointT> > (mesh_source);
boost::shared_ptr<v4r::ESFEstimation<PointT> > estimator;
estimator.reset (new v4r::ESFEstimation<PointT>);
boost::shared_ptr<v4r::GlobalEstimator<PointT> > cast_estimator;
cast_estimator = boost::dynamic_pointer_cast<v4r::ESFEstimation<PointT> > (estimator);
esf_classifier.setDataSource(source);
esf_classifier.setTrainingDir(MODELS_DIR_);
esf_classifier.setDescriptorName("esf");
esf_classifier.setFeatureEstimator (cast_estimator);
esf_classifier.setNN(KNN_);
esf_classifier.initialize (false);
}
void GlobalNNPipelineROS<Distance,PointT,FeatureT>::initializeROS(int argc, char ** argv)
{
ros::init (argc, argv, "classifier_service");
n_.reset( new ros::NodeHandle ( "~" ) );
n_->getParam ( "models_dir", models_dir_ );
n_->getParam ( "training_dir", this->training_dir_ );
n_->getParam ( "descriptor_name", this->descr_name_ );
n_->getParam ( "nn", this->NN_ );
n_->getParam ( "chop_z", chop_at_z_ );
if(!n_->getParam ( "camera_frame", camera_frame_ ))
camera_frame_ = "/head_xtion_depth_optical_frame";
ROS_INFO("models_dir, training dir, desc, camera_frame: %s, %s, %s, %s", models_dir_.c_str(), this->training_dir_.c_str(), this->descr_name_.c_str(), camera_frame_.c_str());
if(models_dir_.compare("") == 0)
{
PCL_ERROR("Set -models_dir option in the command line, ABORTING");
return;
}
if(this->training_dir_.compare("") == 0)
{
PCL_ERROR("Set -training_dir option in the command line, ABORTING");
return;
}
boost::shared_ptr<MeshSource<pcl::PointXYZ> > mesh_source (new MeshSource<pcl::PointXYZ>);
mesh_source->setPath (models_dir_);
mesh_source->setResolution (150);
mesh_source->setTesselationLevel (0);
mesh_source->setViewAngle (57.f);
mesh_source->setRadiusSphere (1.f);
mesh_source->setModelScale (1.f);
mesh_source->generate (this->training_dir_);
this->source_ = boost::static_pointer_cast<v4r::MeshSource<pcl::PointXYZ> > (mesh_source);
boost::shared_ptr<v4r::ESFEstimation<PointT, pcl::ESFSignature640> > estimator;
estimator.reset (new v4r::ESFEstimation<PointT, pcl::ESFSignature640>);
boost::shared_ptr<v4r::GlobalEstimator<PointT, pcl::ESFSignature640> > cast_estimator;
cast_estimator = boost::dynamic_pointer_cast<v4r::ESFEstimation<PointT, pcl::ESFSignature640> > (estimator);
this->setFeatureEstimator (cast_estimator);
this->initialize (false);
// segment_and_classify_service_ = n_->advertiseService("segment_and_classify", &ShapeClassifier::segmentAndClassify, this);
classify_service_ = n_->advertiseService("classify", &v4r::GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS, this);
vis_pub_ = n_->advertise<visualization_msgs::MarkerArray>( "visualization_marker", 0 );
vis_pc_pub_ = n_->advertise<sensor_msgs::PointCloud2>( "clusters", 1 );
ros::spin();
}
int
main (int argc, char ** argv)
{
std::string path = "models/";
std::string desc_name = "esf";
std::string training_dir = "trained_models/";
int NN = 1;
pcl::console::parse_argument (argc, argv, "-models_dir", path);
pcl::console::parse_argument (argc, argv, "-training_dir", training_dir);
pcl::console::parse_argument (argc, argv, "-descriptor_name", desc_name);
pcl::console::parse_argument (argc, argv, "-nn", NN);
//pcl::console::parse_argument (argc, argv, "-z_dist", chop_at_z_);
//pcl::console::parse_argument (argc, argv, "-tesselation_level", views_level_);
boost::shared_ptr<pcl::rec_3d_framework::MeshSource<pcl::PointXYZ> > mesh_source (new pcl::rec_3d_framework::MeshSource<pcl::PointXYZ>);
mesh_source->setPath (path);
mesh_source->setResolution (150);
mesh_source->setTesselationLevel (1);
mesh_source->setViewAngle (57.f);
mesh_source->setRadiusSphere (1.5f);
mesh_source->setModelScale (1.f);
mesh_source->generate (training_dir);
boost::shared_ptr<pcl::rec_3d_framework::Source<pcl::PointXYZ> > cast_source;
cast_source = boost::static_pointer_cast<pcl::rec_3d_framework::MeshSource<pcl::PointXYZ> > (mesh_source);
boost::shared_ptr<pcl::rec_3d_framework::PreProcessorAndNormalEstimator<pcl::PointXYZ, pcl::Normal> > normal_estimator;
normal_estimator.reset (new pcl::rec_3d_framework::PreProcessorAndNormalEstimator<pcl::PointXYZ, pcl::Normal>);
normal_estimator->setCMR (true);
normal_estimator->setDoVoxelGrid (true);
normal_estimator->setRemoveOutliers (true);
normal_estimator->setFactorsForCMR (3, 7);
if (desc_name.compare ("vfh") == 0)
{
boost::shared_ptr<pcl::rec_3d_framework::VFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > vfh_estimator;
vfh_estimator.reset (new pcl::rec_3d_framework::VFHEstimation<pcl::PointXYZ, pcl::VFHSignature308>);
vfh_estimator->setNormalEstimator (normal_estimator);
boost::shared_ptr<pcl::rec_3d_framework::GlobalEstimator<pcl::PointXYZ, pcl::VFHSignature308> > cast_estimator;
cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::VFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > (vfh_estimator);
pcl::rec_3d_framework::GlobalNNPipeline<flann::L1, pcl::PointXYZ, pcl::VFHSignature308> global;
global.setDataSource (cast_source);
global.setTrainingDir (training_dir);
global.setDescriptorName (desc_name);
global.setNN (NN);
global.setFeatureEstimator (cast_estimator);
global.initialize (true);
segmentAndClassify<flann::L1, pcl::PointXYZ, pcl::VFHSignature308> (global);
}
if (desc_name.compare ("cvfh") == 0)
{
boost::shared_ptr<pcl::rec_3d_framework::CVFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > vfh_estimator;
vfh_estimator.reset (new pcl::rec_3d_framework::CVFHEstimation<pcl::PointXYZ, pcl::VFHSignature308>);
vfh_estimator->setNormalEstimator (normal_estimator);
boost::shared_ptr<pcl::rec_3d_framework::GlobalEstimator<pcl::PointXYZ, pcl::VFHSignature308> > cast_estimator;
cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::CVFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > (vfh_estimator);
pcl::rec_3d_framework::GlobalNNPipeline<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308> global;
global.setDataSource (cast_source);
global.setTrainingDir (training_dir);
global.setDescriptorName (desc_name);
global.setFeatureEstimator (cast_estimator);
global.setNN (NN);
global.initialize (false);
segmentAndClassify<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308> (global);
}
if (desc_name.compare ("esf") == 0)
{
boost::shared_ptr<pcl::rec_3d_framework::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640> > estimator;
estimator.reset (new pcl::rec_3d_framework::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640>);
boost::shared_ptr<pcl::rec_3d_framework::GlobalEstimator<pcl::PointXYZ, pcl::ESFSignature640> > cast_estimator;
cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640> > (estimator);
pcl::rec_3d_framework::GlobalNNPipeline<flann::L1, pcl::PointXYZ, pcl::ESFSignature640> global;
global.setDataSource (cast_source);
global.setTrainingDir (training_dir);
global.setDescriptorName (desc_name);
global.setFeatureEstimator (cast_estimator);
global.setNN (NN);
global.initialize (false);
segmentAndClassify<flann::L1, pcl::PointXYZ, pcl::ESFSignature640> (global);
}
}
