template<typename PointT, typename LeafT, typename OctreeT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::nearestKSearch (
int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances)
{
const PointT search_point = this->getPointByIndex (index);
return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
}
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const pcl::PointCloud<PointT> &cloud,
int index,
int k,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances) const
{
return (nearestKSearch (index, k, k_indices, k_sqr_distances));
}
void VFH::doTheGuess(const pcl::PointCloud<PointT>::Ptr object, std::vector<std::string> &guesses)
{
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs(new pcl::PointCloud<pcl::VFHSignature308> ());
computeVFHistogram(object, vfhs);
vfh_model histogram;
pcl::PointCloud <pcl::VFHSignature308> point;
histogram.second.resize (308);
std::vector <pcl::PCLPointField> fields;
int vfh_idx = pcl::getFieldIndex (*vfhs, "vfh", fields);
for (size_t i = 0; i < fields[vfh_idx].count; ++i)
{
histogram.second[i] = vfhs->points[0].histogram[i];
std::cout<<histogram.second[i]<<std::endl;
}
histogram.first = "cloud_from_object.vfh";
//let look for the match
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));
index.buildIndex ();
nearestKSearch (index, histogram, 16, k_indices, k_distances);
pcl::console::print_highlight ("The closest 16 neighbors are:\n");
for (int i = 0; i < 16; ++i)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid (*object, centroid);
std::cerr<<centroid[0]<<centroid[1]<<centroid[2]<<centroid[3]<<std::endl;
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]);
guesses.push_back(models.at (k_indices[0][i]).first);
}
}
int main (int argc, char** argv)
{
PointCloudT::Ptr cloud (new PointCloudT);
PointCloudT::Ptr new_cloud (new PointCloudT);
bool new_cloud_available_flag = false;
//pcl::Grabber* grab = new pcl::OpenNIGrabber ();
PointCloudT::Ptr ddd;
boost::function<void (const PointCloudT::ConstPtr&)> f =
boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag);
grab->registerCallback (f);
viewer->registerKeyboardCallback(keyboardEventOccurred);
grab->start ();
bool first_time = true;
FILE* objects;
objects = fopen ("objects.txt","a");
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag=false;
////////////////////
// invert correction
////////////////////
Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity();
transMat (1,1) = -1;
////////////////////
// pass filter
////////////////////
PointCloudT::Ptr passed_cloud;
pcl::PassThrough<PointT> pass;
passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
PointCloudT::Ptr voxelized_cloud;
voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::VoxelGrid<PointT> vg;
vg.setLeafSize (0.001, 0.001, 0.001);
////////////////////
// sac segmentation
////////////////////
PointCloudT::Ptr cloud_f;
PointCloudT::Ptr cloud_plane;
PointCloudT::Ptr cloud_filtered;
cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
////////////////////
// euclidean clustering
////////////////////
std::vector<pcl::PointIndices> cluster_indices;
std::vector<PointCloudT::Ptr> extracted_clusters;
pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.04);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (eucl_tree);
PointCloudT::Ptr cloud_cluster;
////////////////////
// vfh estimate
////////////////////
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ());
pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ());
std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs;
ne.setSearchMethod (vfh_tree1);
ne.setRadiusSearch (0.05);
vfh.setSearchMethod (vfh_tree2);
vfh.setRadiusSearch (0.05);
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs;
////////////////////
// nearest neighbour
////////////////////
int k = 6;
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<float> data;
// loadFileList (models, training_data_list_file_name);
// flann::load_from_file (data,
// training_data_h5_file_name,
// "training_data");
//
// flann::Index<flann::ChiSquareDistance<float> > index (data,
// flann::SavedIndexParams
// ("kdtree.idx"));
////////////////////
// process nearest neighbour
////////////////////
std::vector<hypothesis> final_hypothesis;
final_hypothesis.clear();
double last = pcl::getTime();
while (! viewer->wasStopped())
{
if (new_cloud_available_flag)
{
new_cloud_available_flag = false;
double now = pcl::getTime();
////////////////////
// pass filter
////////////////////
//passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
//voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// sac segmentation
////////////////////
//cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
//cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
//cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
////////////////////
// euclidean clustering
////////////////////
extracted_clusters.clear();
cluster_indices.clear();
////////////////////
// vfh estimate
////////////////////
computed_vfhs.clear();
////////////////////
// nearest neighbour
////////////////////
cloud_mutex.lock();
//displayCloud(viewer,cloud);
boost::thread displayCloud_(displayCloud,viewer,cloud);
if(now-last > 13 || first_time)
{
first_time = false;
last=now;
////////////////////
// invert correction
////////////////////
pcl::transformPointCloud(*cloud,*new_cloud,transMat);
////////////////////
// pass filter
////////////////////
pass.setInputCloud (new_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*passed_cloud);
////////////////////
// voxel grid
////////////////////
vg.setInputCloud (passed_cloud);
vg.filter (*voxelized_cloud);
////////////////////
// sac segmentation
////////////////////
*cloud_filtered = *voxelized_cloud;
int i=0, nr_points = (int) voxelized_cloud->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Couldnt estimate a planar model for the dataset.\n";
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
////////////////////
// euclidean clustering
////////////////////
// Creating the KdTree object for the search method of the extraction
eucl_tree->setInputCloud (cloud_filtered);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
//PointCloudT::Ptr cloud_cluster (new PointCloudT);
cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points [*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
extracted_clusters.push_back(cloud_cluster);
}
cloud_cluster.reset();
////////////////////
// vfh estimate
////////////////////
for (int z=0; z<extracted_clusters.size(); ++z)
{
vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>);
normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (extracted_clusters.at(z));
ne.compute (*normals);
vfh.setInputCloud (extracted_clusters.at(z));
vfh.setInputNormals (normals);
vfh.compute (*vfhs);
computed_vfhs.push_back(vfhs);
}
vfhs.reset();
normals.reset();
////////////////////
// nearest neighbour
////////////////////
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data,
training_data_h5_file_name,
"training_data");
flann::Index<flann::ChiSquareDistance<float> > index
(data,
flann::SavedIndexParams
("kdtree.idx"));
for(int z=0; z<computed_vfhs.size(); ++z)
{
vfh_model histogram;
histogram.second.resize(308);
for (size_t i = 0; i < 308; ++i)
{
histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i];
}
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
hypothesis x;
x.distance = k_distances[0][0];
size_t index = models.at(k_indices[0][0]).first.find("_v",5);
x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5);
ddd = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat);
x.cluster = ddd;
ddd.reset();
std::string filename ="";
filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
x.cluster_name = filename.c_str();
final_hypothesis.push_back(x);
x.cluster.reset();
//delete x;
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// const char* filen = filename.c_str();
// fprintf(objects,"%s",filen);
// fprintf(objects,"::");
// fprintf(objects,models.at (k_indices[0][0]).first.c_str());
// fprintf(objects,"::");
// fprintf(objects,"%f",k_distances[0][0]);
// fprintf(objects,"\n");
}
delete[] k_indices.ptr ();
delete[] k_distances.ptr ();
delete[] data.ptr ();
std::cout << final_hypothesis.size() << std::endl;
viewer->removeAllShapes();
for(int z=0; z<final_hypothesis.size();++z)
{
if(final_hypothesis.at(z).distance < 100)
{
fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%f",final_hypothesis.at(z).distance);
fprintf(objects,"\n");
std::stringstream ddd;
ddd << final_hypothesis.at(z).object_name;
ddd << "\n" << "(";
ddd << final_hypothesis.at(z).distance;
ddd << ")";
viewer->addText3D(ddd.str().c_str(),
final_hypothesis.at(z).cluster->points[0],
0.02,1,1,1,
boost::lexical_cast<std::string>(z));
drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z);
}
}
//boost::thread allBoxes_(allBoxes,viewer,final_hypothesis);
//allBoxes_.join();
viewer->spinOnce();
final_hypothesis.clear();
j++;
}
// for(int z=0; z<extracted_clusters.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false);
// }
// for(int z=0; z<computed_vfhs.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd";
// pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z));
// }
//viewer->removeAllShapes();
// viewer->removeAllPointClouds();
// viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
// viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud");
// viewer->spinOnce();
//std::cout << final_hypothesis.at(0).cluster->points[0];
//boost::this_thread::sleep(boost::posix_time::milliseconds(10));
displayCloud_.join();
cloud_mutex.unlock();
}
}
grab->stop();
return 0;
}
void
pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::recognize ()
{
models_.reset (new std::vector<ModelT>);
transforms_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
PointInTPtr processed;
typename pcl::PointCloud<FeatureT>::Ptr signatures (new pcl::PointCloud<FeatureT> ());
//pcl::PointCloud<int> keypoints_input;
PointInTPtr keypoints_pointcloud;
if (signatures_ != 0 && processed_ != 0 && (signatures_->size () == keypoints_pointcloud->points.size ()))
{
keypoints_pointcloud = keypoints_input_;
signatures = signatures_;
processed = processed_;
std::cout << "Using the ISPK ..." << std::endl;
}
else
{
processed.reset( (new pcl::PointCloud<PointInT>));
if (indices_.size () > 0)
{
PointInTPtr sub_input (new pcl::PointCloud<PointInT>);
pcl::copyPointCloud (*input_, indices_, *sub_input);
estimator_->estimate (sub_input, processed, keypoints_pointcloud, signatures);
}
else
{
estimator_->estimate (input_, processed, keypoints_pointcloud, signatures);
}
processed_ = processed;
}
std::cout << "Number of keypoints:" << keypoints_pointcloud->points.size () << std::endl;
int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
//feature matching and object hypotheses
std::map<std::string, ObjectHypothesis> object_hypotheses;
{
for (size_t idx = 0; idx < signatures->points.size (); idx++)
{
float* hist = signatures->points[idx].histogram;
std::vector<float> std_hist (hist, hist + size_feat);
flann_model histogram;
histogram.descr = std_hist;
flann::Matrix<int> indices;
flann::Matrix<float> distances;
nearestKSearch (flann_index_, histogram, 1, indices, distances);
//read view pose and keypoint coordinates, transform keypoint coordinates to model coordinates
Eigen::Matrix4f homMatrixPose;
getPose (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, homMatrixPose);
typename pcl::PointCloud<PointInT>::Ptr keypoints (new pcl::PointCloud<PointInT> ());
getKeypoints (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, keypoints);
PointInT view_keypoint = keypoints->points[flann_models_.at (indices[0][0]).keypoint_id];
PointInT model_keypoint;
model_keypoint.getVector4fMap () = homMatrixPose.inverse () * view_keypoint.getVector4fMap ();
typename std::map<std::string, ObjectHypothesis>::iterator it_map;
if ((it_map = object_hypotheses.find (flann_models_.at (indices[0][0]).model.id_)) != object_hypotheses.end ())
{
//if the object hypothesis already exists, then add information
ObjectHypothesis oh = (*it_map).second;
oh.correspondences_pointcloud->points.push_back (model_keypoint);
oh.correspondences_to_inputcloud->push_back (
pcl::Correspondence (static_cast<int> (oh.correspondences_pointcloud->points.size () - 1),
static_cast<int> (idx), distances[0][0]));
oh.feature_distances_->push_back (distances[0][0]);
}
else
{
//create object hypothesis
ObjectHypothesis oh;
typename pcl::PointCloud<PointInT>::Ptr correspondences_pointcloud (new pcl::PointCloud<PointInT> ());
correspondences_pointcloud->points.push_back (model_keypoint);
oh.model_ = flann_models_.at (indices[0][0]).model;
oh.correspondences_pointcloud = correspondences_pointcloud;
//last keypoint for this model is a correspondence the current scene keypoint
pcl::CorrespondencesPtr corr (new pcl::Correspondences ());
oh.correspondences_to_inputcloud = corr;
oh.correspondences_to_inputcloud->push_back (pcl::Correspondence (0, static_cast<int> (idx), distances[0][0]));
boost::shared_ptr < std::vector<float> > feat_dist (new std::vector<float>);
feat_dist->push_back (distances[0][0]);
oh.feature_distances_ = feat_dist;
object_hypotheses[oh.model_.id_] = oh;
}
}
}
typename std::map<std::string, ObjectHypothesis>::iterator it_map;
std::vector<float> feature_distance_avg;
{
//pcl::ScopeTime t("Geometric verification, RANSAC and transform estimation");
for (it_map = object_hypotheses.begin (); it_map != object_hypotheses.end (); it_map++)
{
std::vector < pcl::Correspondences > corresp_clusters;
cg_algorithm_->setSceneCloud (keypoints_pointcloud);
cg_algorithm_->setInputCloud ((*it_map).second.correspondences_pointcloud);
cg_algorithm_->setModelSceneCorrespondences ((*it_map).second.correspondences_to_inputcloud);
cg_algorithm_->cluster (corresp_clusters);
std::cout << "Instances:" << corresp_clusters.size () << " Total correspondences:" << (*it_map).second.correspondences_to_inputcloud->size () << " " << it_map->first << std::endl;
std::vector<bool> good_indices_for_hypothesis (corresp_clusters.size (), true);
if (threshold_accept_model_hypothesis_ < 1.f)
{
//sort the hypotheses for each model according to their correspondences and take those that are threshold_accept_model_hypothesis_ over the max cardinality
int max_cardinality = -1;
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
//std::cout << (corresp_clusters[i]).size() << " -- " << (*(*it_map).second.correspondences_to_inputcloud).size() << std::endl;
if (max_cardinality < static_cast<int> (corresp_clusters[i].size ()))
{
max_cardinality = static_cast<int> (corresp_clusters[i].size ());
}
}
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
if (static_cast<float> ((corresp_clusters[i]).size ()) < (threshold_accept_model_hypothesis_ * static_cast<float> (max_cardinality)))
{
good_indices_for_hypothesis[i] = false;
}
}
}
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
if (!good_indices_for_hypothesis[i])
continue;
//drawCorrespondences (processed, it_map->second, keypoints_pointcloud, corresp_clusters[i]);
Eigen::Matrix4f best_trans;
typename pcl::registration::TransformationEstimationSVD < PointInT, PointInT > t_est;
t_est.estimateRigidTransformation (*(*it_map).second.correspondences_pointcloud, *keypoints_pointcloud, corresp_clusters[i], best_trans);
models_->push_back ((*it_map).second.model_);
transforms_->push_back (best_trans);
}
}
}
std::cout << "Number of hypotheses:" << models_->size() << std::endl;
/**
* POSE REFINEMENT
**/
if (ICP_iterations_ > 0)
{
pcl::ScopeTime ticp ("ICP ");
//Prepare scene and model clouds for the pose refinement step
PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
pcl::VoxelGrid<PointInT> voxel_grid_icp;
voxel_grid_icp.setInputCloud (processed);
voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
voxel_grid_icp.filter (*cloud_voxelized_icp);
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
#pragma omp parallel for schedule(dynamic,1) num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int>(models_->size ()); i++)
{
ConstPointInTPtr model_cloud;
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT>::Ptr rej (
new pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT> ());
rej->setInputTarget (cloud_voxelized_icp);
rej->setMaximumIterations (1000);
rej->setInlierThreshold (0.005f);
rej->setInputSource (model_aligned);
pcl::IterativeClosestPoint<PointInT, PointInT> reg;
reg.addCorrespondenceRejector (rej);
reg.setInputTarget (cloud_voxelized_icp); //scene
reg.setInputSource (model_aligned); //model
reg.setMaximumIterations (ICP_iterations_);
reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 4.f);
typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
reg.align (*output_);
Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
transforms_->at (i) = icp_trans * transforms_->at (i);
}
}
/**
* HYPOTHESES VERIFICATION
**/
if (hv_algorithm_)
{
pcl::ScopeTime thv ("HV verification");
std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
aligned_models.resize (models_->size ());
for (size_t i = 0; i < models_->size (); i++)
{
ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.0025f);
//ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
aligned_models[i] = model_aligned;
}
std::vector<bool> mask_hv;
hv_algorithm_->setSceneCloud (input_);
hv_algorithm_->addModels (aligned_models, true);
hv_algorithm_->verify ();
hv_algorithm_->getMask (mask_hv);
boost::shared_ptr < std::vector<ModelT> > models_temp;
boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_temp;
models_temp.reset (new std::vector<ModelT>);
transforms_temp.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
for (size_t i = 0; i < models_->size (); i++)
{
if (!mask_hv[i])
continue;
models_temp->push_back (models_->at (i));
transforms_temp->push_back (transforms_->at (i));
}
models_ = models_temp;
transforms_ = transforms_temp;
}
}
/** \brief Returns closest poses of of objects in training data to the query object
\param -q the path to the input point cloud
\param -k the number of nearest neighbors to return
*/
int main (int argc, char **argv)
{
//parse data directory
std::string queryCloudName;
pcl::console::parse_argument (argc, argv, "-q", queryCloudName);
boost::filesystem::path queryCloudPath(queryCloudName);
//parse number of nearest neighbors k
int k = 1;
pcl::console::parse_argument (argc, argv, "-k", k);
pcl::console::print_highlight ("using %d nearest neighbors.\n", k);
//read in point cloud
PointCloud::Ptr cloud (new PointCloud);
pcl::PCDReader reader;
//read ply file
pcl::PolygonMesh triangles;
if(queryCloudPath.extension().native().compare(".ply") == 0)
{
if( pcl::io::loadPolygonFilePLY(queryCloudPath.native(), triangles) == -1)
{
PCL_ERROR("Could not read .ply file\n");
return -1;
}
#if PCL17
pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
#endif
#if PCL16
pcl::fromROSMsg(triangles.cloud, *cloud);
#endif
}
//read pcd file
else if(queryCloudPath.extension().native().compare(".pcd") == 0)
{
if( reader.read(queryCloudPath.native(), *cloud) == -1)
{
PCL_ERROR("Could not read .pcd file\n");
return -1;
}
}
else
{
PCL_ERROR("File must have extension .ply or .pcd\n");
return -1;
}
//Move point cloud so it is is centered at the origin
Eigen::Matrix<float,4,1> centroid;
pcl::compute3DCentroid(*cloud, centroid);
pcl::demeanPointCloud(*cloud, centroid, *cloud);
//Estimate normals
Normals::Ptr normals (new Normals);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;
normEst.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr normTree (new pcl::search::KdTree<pcl::PointXYZ>);
normEst.setSearchMethod(normTree);
normEst.setRadiusSearch(0.005);
normEst.compute(*normals);
//Create VFH estimation class
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud(cloud);
vfh.setInputNormals(normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr vfhsTree (new pcl::search::KdTree<pcl::PointXYZ>);
vfh.setSearchMethod(vfhsTree);
//calculate VFHS features
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308>);
vfh.setViewPoint(1, 0, 0);
vfh.compute(*vfhs);
//filenames
std::string featuresFileName = "training_features.h5";
std::string anglesFileName = "training_angles.list";
std::string kdtreeIdxFileName = "training_kdtree.idx";
//allocate flann matrices
std::vector<CloudInfo> cloudInfoList;
std::vector<PointCloud::Ptr> cloudList;
cloudList.resize(k);
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
//load training data angles list
loadAngleData(cloudInfoList, anglesFileName);
flann::load_from_file (data, featuresFileName, "training_data");
flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("training_kdtree.idx"));
//perform knn search
index.buildIndex ();
nearestKSearch (index, vfhs, k, k_indices, k_distances);
// Output the results on screen
pcl::console::print_highlight ("The closest %d neighbors are:\n", k);
for (int i = 0; i < k; ++i)
{
//print nearest neighbor info to screen
pcl::console::print_info (" %d - theta = %f, phi = %f (%s) with a distance of: %f\n",
i,
cloudInfoList.at(k_indices[0][i]).theta*180.0/M_PI,
cloudInfoList.at(k_indices[0][i]).phi*180.0/M_PI,
cloudInfoList.at(k_indices[0][i]).filePath.c_str(),
k_distances[0][i]);
//retrieve pointcloud and store in list
PointCloud::Ptr cloudMatch (new PointCloud);
reader.read(cloudInfoList.at(k_indices[0][i]).filePath.native(), *cloudMatch);
//Move point cloud so it is is centered at the origin
pcl::compute3DCentroid(*cloudMatch, centroid);
pcl::demeanPointCloud(*cloudMatch, centroid, *cloudMatch);
cloudList[i] = cloudMatch;
}
//visualize histogram
/*
pcl::visualization::PCLHistogramVisualizer histvis;
histvis.addFeatureHistogram<pcl::VFHSignature308> (*vfhs, histLength);
*/
//Visualize point cloud and matches
//viewpoint cals
int y_s = (int)std::floor (sqrt ((double)k));
int x_s = y_s + (int)std::ceil ((k / (double)y_s) - y_s);
double x_step = (double)(1 / (double)x_s);
double y_step = (double)(1 / (double)y_s);
int viewport = 0, l = 0, m = 0;
std::string viewName = "match number ";
//setup visualizer and add query cloud
pcl::visualization::PCLVisualizer visu("KNN search");
visu.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
visu.addPointCloud<pcl::PointXYZ> (cloud, ColorHandler(cloud, 0.0 , 255.0, 0.0), "Query Cloud Cloud", viewport);
visu.addText ("Query Cloud", 20, 30, 136.0/255.0, 58.0/255.0, 1, "Query Cloud", viewport);
visu.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, "Query Cloud", viewport);
visu.addCoordinateSystem (0.05, 0);
//add matches to plot
for(int i = 1; i < (k+1); ++i)
{
//shift viewpoint
++l;
if (l >= x_s)
{
l = 0;
m++;
}
//names and text labels
std::string textString = viewName;
std::string cloudname = viewName;
textString.append(boost::lexical_cast<std::string>(i));
cloudname.append(boost::lexical_cast<std::string>(i)).append("cloud");
//color proportional to distance
//add cloud
visu.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
visu.addPointCloud<pcl::PointXYZ> (cloudList[i-1], ColorHandler(cloudList[i-1], 0.0 , 255.0, 0.0), cloudname, viewport);
visu.addText (textString, 20, 30, 136.0/255.0, 58.0/255.0, 1, textString, viewport);
visu.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, textString, viewport);
}
visu.spin();
return 0;
}
void
pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::recognize ()
{
models_.reset (new std::vector<ModelT>);
transforms_.reset (new std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >);
PointInTPtr processed (new pcl::PointCloud<PointInT>);
PointInTPtr in (new pcl::PointCloud<PointInT>);
std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
std::vector < Eigen::Vector3d > centroids;
if (indices_.size ())
pcl::copyPointCloud (*input_, indices_, *in);
else
in = input_;
{
pcl::ScopeTime t ("Estimate feature");
micvfh_estimator_->estimate (in, processed, signatures, centroids);
}
std::vector<index_score> indices_scores;
descriptor_distances_.clear ();
if (signatures.size () > 0)
{
{
pcl::ScopeTime t_matching ("Matching and roll...");
if (use_single_categories_ && (categories_to_be_searched_.size () > 0))
{
//perform search of the different signatures in the categories_to_be_searched_
for (size_t c = 0; c < categories_to_be_searched_.size (); c++)
{
std::cout << "Using category:" << categories_to_be_searched_[c] << std::endl;
for (size_t idx = 0; idx < signatures.size (); idx++)
{
/*double* hist = signatures[idx].points[0].histogram;
std::vector<double> std_hist (hist, hist + getHistogramLength (dummy));
flann_model histogram ("", std_hist);
flann::Matrix<int> indices;
flann::Matrix<double> distances;
std::map<std::string, int>::iterator it;
it = category_to_vectors_indices_.find (categories_to_be_searched_[c]);
assert (it != category_to_vectors_indices_.end ());
nearestKSearch (single_categories_index_[it->second], histogram, nmodels_, indices, distances);*/
double* hist = signatures[idx].points[0].histogram;
int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(double);
std::vector<double> std_hist (hist, hist + size_feat);
//ModelT empty;
flann_model histogram;
histogram.descr = std_hist;
flann::Matrix<int> indices;
flann::Matrix<double> distances;
std::map<std::string, int>::iterator it;
it = category_to_vectors_indices_.find (categories_to_be_searched_[c]);
assert (it != category_to_vectors_indices_.end ());
nearestKSearch (single_categories_index_[it->second], histogram, NN_, indices, distances);
//gather NN-search results
double score = 0;
for (size_t i = 0; i < NN_; ++i)
{
score = distances[0][i];
index_score is;
is.idx_models_ = single_categories_pointers_to_models_[it->second]->at (indices[0][i]);
is.idx_input_ = static_cast<int> (idx);
is.score_ = score;
indices_scores.push_back (is);
}
}
//we cannot add more than nmodels per category, so sort here and remove offending ones...
std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
indices_scores.resize ((c + 1) * NN_);
}
}
else
{
for (size_t idx = 0; idx < signatures.size (); idx++)
{
double* hist = signatures[idx].points[0].histogram;
int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(double);
std::vector<double> std_hist (hist, hist + size_feat);
//ModelT empty;
flann_model histogram;
histogram.descr = std_hist;
flann::Matrix<int> indices;
flann::Matrix<double> distances;
nearestKSearch (flann_index_, histogram, NN_, indices, distances);
//gather NN-search results
double score = 0;
for (int i = 0; i < NN_; ++i)
{
score = distances[0][i];
index_score is;
is.idx_models_ = indices[0][i];
is.idx_input_ = static_cast<int> (idx);
is.score_ = score;
indices_scores.push_back (is);
//ModelT m = flann_models_[indices[0][i]].model;
}
}
}
std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
/*
* There might be duplicated candidates, in those cases it makes sense to take
* the closer one in descriptor space
*/
typename std::map<flann_model, bool> found;
typename std::map<flann_model, bool>::iterator it_map;
for (size_t i = 0; i < indices_scores.size (); i++)
{
flann_model m = flann_models_[indices_scores[i].idx_models_];
it_map = found.find (m);
if (it_map == found.end ())
{
indices_scores[found.size ()] = indices_scores[i];
found[m] = true;
}
}
indices_scores.resize (found.size ());
int num_n = std::min (NN_, static_cast<int> (indices_scores.size ()));
/*
* Filter some hypothesis regarding to their distance to the first neighbour
*/
/*std::vector<index_score> indices_scores_filtered;
indices_scores_filtered.resize (num_n);
indices_scores_filtered[0] = indices_scores[0];
double best_score = indices_scores[0].score_;
int kept = 1;
for (int i = 1; i < num_n; ++i)
{
//std::cout << best_score << indices_scores[i].score_ << (best_score / indices_scores[i].score_) << std::endl;
if ((best_score / indices_scores[i].score_) > 0.65)
{
indices_scores_filtered[i] = indices_scores[i];
kept++;
}
//best_score = indices_scores[i].score_;
}
indices_scores_filtered.resize (kept);
//std::cout << indices_scores_filtered.size () << " § " << num_n << std::endl;
indices_scores = indices_scores_filtered;
num_n = indices_scores.size ();*/
std::cout << "Number of object hypotheses... " << num_n << std::endl;
std::vector<bool> valid_trans;
std::vector < Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > transformations;
micvfh_estimator_->getValidTransformsVec (valid_trans);
micvfh_estimator_->getTransformsVec (transformations);
for (int i = 0; i < num_n; ++i)
{
ModelT m = flann_models_[indices_scores[i].idx_models_].model;
int view_id = flann_models_[indices_scores[i].idx_models_].view_id;
int desc_id = flann_models_[indices_scores[i].idx_models_].descriptor_id;
int idx_input = indices_scores[i].idx_input_;
std::cout << m.class_ << "/" << m.id_ << " ==> " << indices_scores[i].score_ << std::endl;
Eigen::Matrix4d roll_view_pose;
bool roll_pose_found = getRollPose (m, view_id, desc_id, roll_view_pose);
if (roll_pose_found && valid_trans[idx_input])
{
Eigen::Matrix4d transposed = roll_view_pose.transpose ();
//std::cout << transposed << std::endl;
PointInTPtr view;
getView (m, view_id, view);
Eigen::Matrix4d model_view_pose;
getPose (m, view_id, model_view_pose);
Eigen::Matrix4d scale_mat;
scale_mat.setIdentity (4, 4);
if (compute_scale_)
{
//compute scale using the whole view
PointInTPtr view_transformed (new pcl::PointCloud<PointInT>);
Eigen::Matrix4d hom_from_OVC_to_CC;
hom_from_OVC_to_CC = transformations[idx_input].inverse () * transposed;
pcl::transformPointCloud (*view, *view_transformed, hom_from_OVC_to_CC);
Eigen::Vector3d input_centroid = centroids[indices_scores[i].idx_input_];
Eigen::Vector3d view_centroid;
getCentroid (m, view_id, desc_id, view_centroid);
Eigen::Vector4d cmatch4f (view_centroid[0], view_centroid[1], view_centroid[2], 0);
Eigen::Vector4d cinput4f (input_centroid[0], input_centroid[1], input_centroid[2], 0);
Eigen::Vector4d max_pt_input;
pcl::getMaxDistance (*processed, cinput4f, max_pt_input);
max_pt_input[3] = 0;
double max_dist_input = (cinput4f - max_pt_input).norm ();
//compute max dist for transformed model_view
pcl::getMaxDistance (*view, cmatch4f, max_pt_input);
max_pt_input[3] = 0;
double max_dist_view = (cmatch4f - max_pt_input).norm ();
cmatch4f = hom_from_OVC_to_CC * cmatch4f;
std::cout << max_dist_view << " " << max_dist_input << std::endl;
double scale_factor_view = max_dist_input / max_dist_view;
std::cout << "Scale factor:" << scale_factor_view << std::endl;
Eigen::Matrix4d center, center_inv;
center.setIdentity (4, 4);
center (0, 3) = -cinput4f[0];
center (1, 3) = -cinput4f[1];
center (2, 3) = -cinput4f[2];
center_inv.setIdentity (4, 4);
center_inv (0, 3) = cinput4f[0];
center_inv (1, 3) = cinput4f[1];
center_inv (2, 3) = cinput4f[2];
scale_mat (0, 0) = scale_factor_view;
scale_mat (1, 1) = scale_factor_view;
scale_mat (2, 2) = scale_factor_view;
scale_mat = center_inv * scale_mat * center;
}
Eigen::Matrix4d hom_from_OC_to_CC;
hom_from_OC_to_CC = scale_mat * transformations[idx_input].inverse () * transposed * model_view_pose;
models_->push_back (m);
transforms_->push_back (hom_from_OC_to_CC);
descriptor_distances_.push_back (static_cast<double> (indices_scores[i].score_));
}
else
{
PCL_WARN("The roll pose was not found, should use CRH here... \n");
}
}
}
std::cout << "Number of object hypotheses:" << models_->size () << std::endl;
/**
* POSE REFINEMENT
**/
if (ICP_iterations_ > 0)
{
pcl::ScopeTime t ("Pose refinement");
//Prepare scene and model clouds for the pose refinement step
double VOXEL_SIZE_ICP_ = 0.005;
PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
{
pcl::ScopeTime t ("Voxelize stuff...");
pcl::VoxelGrid<PointInT> voxel_grid_icp;
voxel_grid_icp.setInputCloud (processed);
voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
voxel_grid_icp.filter (*cloud_voxelized_icp);
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
}
#pragma omp parallel for num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int> (models_->size ()); i++)
{
ConstPointInTPtr model_cloud;
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
if (compute_scale_)
{
model_cloud = models_->at (i).getAssembled (-1);
PointInTPtr model_aligned_m (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i));
pcl::VoxelGrid<PointInT> voxel_grid_icp;
voxel_grid_icp.setInputCloud (model_aligned_m);
voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
voxel_grid_icp.filter (*model_aligned);
}
else
{
model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
}
pcl::IterativeClosestPoint<PointInT, PointInT> reg;
reg.setInputCloud (model_aligned); //model
reg.setInputTarget (cloud_voxelized_icp); //scene
reg.setMaximumIterations (ICP_iterations_);
reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 3.);
reg.setTransformationEpsilon (1e-6);
typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
reg.align (*output_);
Eigen::Matrix4d icp_trans = reg.getFinalTransformation ();
transforms_->at (i) = icp_trans * transforms_->at (i);
}
}
/**
* HYPOTHESES VERIFICATION
**/
if (hv_algorithm_)
{
pcl::ScopeTime t ("HYPOTHESES VERIFICATION");
std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
aligned_models.resize (models_->size ());
for (size_t i = 0; i < models_->size (); i++)
{
ConstPointInTPtr model_cloud;
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
if (compute_scale_)
{
model_cloud = models_->at (i).getAssembled (-1);
PointInTPtr model_aligned_m (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i));
pcl::VoxelGrid<PointInT> voxel_grid_icp;
voxel_grid_icp.setInputCloud (model_aligned_m);
voxel_grid_icp.setLeafSize (0.005, 0.005, 0.005);
voxel_grid_icp.filter (*model_aligned);
}
else
{
model_cloud = models_->at (i).getAssembled (0.005);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
}
//ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.005);
//PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
//pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
aligned_models[i] = model_aligned;
}
std::vector<bool> mask_hv;
hv_algorithm_->setSceneCloud (input_);
hv_algorithm_->addModels (aligned_models, true);
hv_algorithm_->verify ();
hv_algorithm_->getMask (mask_hv);
boost::shared_ptr < std::vector<ModelT> > models_temp;
boost::shared_ptr < std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > > transforms_temp;
models_temp.reset (new std::vector<ModelT>);
transforms_temp.reset (new std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >);
for (size_t i = 0; i < models_->size (); i++)
{
if (!mask_hv[i])
continue;
models_temp->push_back (models_->at (i));
transforms_temp->push_back (transforms_->at (i));
}
models_ = models_temp;
transforms_ = transforms_temp;
}
}
}
int
main (int argc, char** argv)
{
int k = 6;
double thresh = DBL_MAX; // No threshold, disabled by default
if (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);
}
// this won't be needed for flann > 1.6.10
flann::ObjectFactory<flann::IndexParams, flann_algorithm_t>::instance().register_<flann::LinearIndexParams>(FLANN_INDEX_LINEAR);
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
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 disk
if (!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 disk
if (!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);
}
// Output the results on screen
pcl::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++;
}
sensor_msgs::PointCloud2 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 PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
pcl::fromROSMsg (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);
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);
}
else
p.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 name
p.addText (cloud_name, 20, 10, cloud_name, viewport);
}
// Add coordianate systems to all viewports
p.addCoordinateSystem (0.1, 0);
p.spin ();
return (0);
}
